Merge branch 'master' of https://github.com/MaslowCNC/Firmware

MaslowCNC · Aug 17, 2017 · 60c00ea · 60c00ea
2 parents 3ac59f6 + c4fd355
commit 60c00ea
Show file tree

Hide file tree

Showing 3 changed files with 72 additions and 50 deletions.
diff --git a/cnc_ctrl_v1/Axis.cpp b/cnc_ctrl_v1/Axis.cpp
@@ -54,6 +54,7 @@ void   Axis::loadPositionFromMemory(){
         if (EEPROM.read(_eepromAdr) == EEPROMVALIDDATA){
             set(_readFloat(_eepromAdr + SIZEOFFLOAT));
         }
+
 }
 
 void   Axis::initializePID(){
@@ -72,6 +73,7 @@ float  Axis::read(){
     //returns the true axis position
 
     return (motorGearboxEncoder.encoder.read()/_encoderSteps)*_mmPerRotation;
+
 }
 
 float  Axis::target(){

diff --git a/cnc_ctrl_v1/CNC_Functions.h b/cnc_ctrl_v1/CNC_Functions.h
@@ -145,8 +145,6 @@ String gcodeLine;                         //The next individual line of gcode (f
 
 int   lastCommand           =  0;         //Stores the value of the last command run eg: G01 -> 1
 
-//These are used in place of a forward kinematic function at the beginning of each move. They should be replaced
-//by a call to the forward kinematic function when it is available.
 float xTarget = 0;
 float yTarget = 0;
 
@@ -933,7 +931,7 @@ void  printBeforeAndAfter(const float& before, const float& after){
     Serial.println(after);
 }
 
-void  updateSettings(const String& readString){
+void  updateKinematicsSettings(const String& readString){
     /*
     Updates the machine dimensions from the Ground Control settings
     */
@@ -947,6 +945,59 @@ void  updateSettings(const String& readString){
     float sledWidth          = extractGcodeValue(readString, 'F', -1);
     float sledHeight         = extractGcodeValue(readString, 'R', -1);
     float sledCG             = extractGcodeValue(readString, 'H', -1);
+
+    float kinematicsType     = extractGcodeValue(readString, 'Y', -1);
+    float rotationDiskRadius = extractGcodeValue(readString, 'Z', -1);
+
+
+
+    //Change the machine dimensions in the kinematics if new values have been received
+    if (sledWidth != -1){
+        kinematics.l            = sledWidth;
+    }
+    if (sledHeight != -1){
+        kinematics.s            = sledHeight;
+    }
+    if (sledCG != -1){
+        kinematics.h3           = sledCG;
+    }
+    //if (distPerRot != -1){
+    //    kinematics.R            = distPerRot / (2.0*3.14159);
+    //}
+    if (distBetweenMotors != -1){
+        kinematics.D            = distBetweenMotors;
+    }
+    if (motorOffsetY != -1){
+        kinematics.motorOffsetY = motorOffsetY;
+    }
+    if (bedWidth != -1){
+        kinematics.machineWidth = bedWidth;
+    }
+    if (bedHeight != -1){
+        kinematics.machineHeight= bedHeight;
+    }
+    if (kinematicsType != -1){
+        kinematics.kinematicsType = kinematicsType;
+    }
+    if (rotationDiskRadius != -1){
+        kinematics.rotationDiskRadius = rotationDiskRadius;
+    }
+
+    //propagate the new values
+    kinematics.recomputeGeometry();
+
+    kinematics.forward(leftAxis.read(), rightAxis.read(), &xTarget, &yTarget);
+
+    Serial.println(F("Kinematics Settings Loaded"));
+}
+
+void updateMotorSettings(const String& readString){
+    /*
+    
+    Update settings related to the motor configurations
+    
+    */
+
     if (extractGcodeValue(readString, 'I', -1) != -1){
         zAxisAttached            = extractGcodeValue(readString, 'I', -1);
     }
@@ -963,8 +1014,6 @@ void  updateSettings(const String& readString){
     float KpV                = extractGcodeValue(readString, 'V', -1);
     float KiV                = extractGcodeValue(readString, 'W', -1);
     float KdV                = extractGcodeValue(readString, 'X', -1);
-    float kinematicsType     = extractGcodeValue(readString, 'Y', -1);
-    float rotationDiskRadius = extractGcodeValue(readString, 'Z', -1);
 
     //Write the PID values to the axis if new ones have been received
     if (KpPos != -1){
@@ -993,45 +1042,6 @@ void  updateSettings(const String& readString){
         zAxis.changeEncoderResolution(zEncoderSteps);
         zAxis.loadPositionFromMemory();
     }
-
-    //Change the machine dimensions in the kinematics if new values have been received
-    if (sledWidth != -1){
-        kinematics.l            = sledWidth;
-    }
-    if (sledHeight != -1){
-        kinematics.s            = sledHeight;
-    }
-    if (sledCG != -1){
-        kinematics.h3           = sledCG;
-    }
-    if (distPerRot != -1){
-        kinematics.R            = distPerRot / (2.0*3.14159);
-    }
-    if (distBetweenMotors != -1){
-        kinematics.D            = distBetweenMotors;
-    }
-    if (motorOffsetY != -1){
-        kinematics.motorOffsetY = motorOffsetY;
-    }
-    if (bedWidth != -1){
-        kinematics.machineWidth = bedWidth;
-    }
-    if (bedHeight != -1){
-        kinematics.machineHeight= bedHeight;
-    }
-    if (kinematicsType != -1){
-        kinematics.kinematicsType = kinematicsType;
-    }
-    if (rotationDiskRadius != -1){
-        kinematics.rotationDiskRadius = rotationDiskRadius;
-    }
-
-    //propagate the new values
-    kinematics.recomputeGeometry();
-
-    kinematics.forward(leftAxis.read(), rightAxis.read(), &xTarget, &yTarget);
-
-    Serial.println(F("Machine Settings Updated"));
 }
 
 void  executeGcodeLine(const String& gcodeLine){
@@ -1057,7 +1067,7 @@ void  executeGcodeLine(const String& gcodeLine){
     }
 
     if(gcodeLine.substring(0, 3) == "B03"){
-        updateSettings(gcodeLine);
+        updateKinematicsSettings(gcodeLine);
         return;
     }
 
@@ -1179,6 +1189,12 @@ void  executeGcodeLine(const String& gcodeLine){
         return;
     }
 
+    if(gcodeLine.substring(0, 3) == "B12"){
+        //Update the motor characteristics
+        updateMotorSettings(gcodeLine);
+        return;
+    }
+
     //Handle G-Codes
 
     int gNumber = extractGcodeValue(gcodeLine,'G', -1);

diff --git a/cnc_ctrl_v1/Kinematics.cpp b/cnc_ctrl_v1/Kinematics.cpp
@@ -223,7 +223,7 @@ void  Kinematics::forward(const float& chainALength, const float& chainBLength,
         //adjust the guess based on the result
         xGuess = xGuess + .1*aChainError - .1*bChainError;
         yGuess = yGuess - .1*aChainError - .1*bChainError;
-
+        
         guessCount++;
 
         //Prevent the connection from timing out
@@ -238,14 +238,18 @@ void  Kinematics::forward(const float& chainALength, const float& chainBLength,
         //if we've converged on the point...or it's time to give up, exit the loop
         if((abs(aChainError) < .1 && abs(bChainError) < .1) or guessCount > maxNumberOfGuesses){
             if(guessCount > maxNumberOfGuesses){
-                Serial.println(F("Message: Unable to find valid machine position. Please calibrate chain lengths."));
-                Serial.println(F("Lengths: "));
-                Serial.println(chainALength);
-                Serial.println(chainBLength);
+                Serial.print(F("Message: Unable to find valid machine position for chain lengths "));
+                Serial.print(chainALength);
+                Serial.print(", ");
+                Serial.print(chainBLength);
+                Serial.println(F(" . Please calibrate chain lengths."));
                 *xPos = 0;
                 *yPos = 0;
             }
             else{
+                Serial.println("position loaded at:");
+                Serial.println(xGuess);
+                Serial.println(yGuess);
                 *xPos = xGuess;
                 *yPos = yGuess;
             }