Merge pull request #422 from reecej/Chain-Tolerance

Add chain tolerance parameters

cnc_ctrl_v1/Kinematics.cpp

@@ -213,23 +213,23 @@ void  Kinematics::triangularInverse(float xTarget,float yTarget, float* aChainLe
 
     //Calculate the chain angles from horizontal, based on if the chain connects to the sled from the top or bottom of the sprocket
     if(sysSettings.chainOverSprocket == 1){
-        Chain1Angle = asin((_yCordOfMotor - yTarget)/Motor1Distance) + asin(R/Motor1Distance);
-        Chain2Angle = asin((_yCordOfMotor - yTarget)/Motor2Distance) + asin(R/Motor2Distance);
+        Chain1Angle = asin((_yCordOfMotor - yTarget)/Motor1Distance) + asin(RleftChainTolerance/Motor1Distance);
+        Chain2Angle = asin((_yCordOfMotor - yTarget)/Motor2Distance) + asin(RrightChainTolerance/Motor2Distance);
 
-        Chain1AroundSprocket = R * Chain1Angle;
-        Chain2AroundSprocket = R * Chain2Angle;
+        Chain1AroundSprocket = RleftChainTolerance * Chain1Angle;
+        Chain2AroundSprocket = RrightChainTolerance * Chain2Angle;
     }
     else{
-        Chain1Angle = asin((_yCordOfMotor - yTarget)/Motor1Distance) - asin(R/Motor1Distance);
-        Chain2Angle = asin((_yCordOfMotor - yTarget)/Motor2Distance) - asin(R/Motor2Distance);
+        Chain1Angle = asin((_yCordOfMotor - yTarget)/Motor1Distance) - asin(RleftChainTolerance/Motor1Distance);
+        Chain2Angle = asin((_yCordOfMotor - yTarget)/Motor2Distance) - asin(RrightChainTolerance/Motor2Distance);
 
-        Chain1AroundSprocket = R * (3.14159 - Chain1Angle);
-        Chain2AroundSprocket = R * (3.14159 - Chain2Angle);
+        Chain1AroundSprocket = RleftChainTolerance * (3.14159 - Chain1Angle);
+        Chain2AroundSprocket = RrightChainTolerance * (3.14159 - Chain2Angle);
     }
 
     //Calculate the straight chain length from the sprocket to the bit
-    float Chain1Straight = sqrt(pow(Motor1Distance,2)-pow(R,2));
-    float Chain2Straight = sqrt(pow(Motor2Distance,2)-pow(R,2));
+    float Chain1Straight = sqrt(pow(Motor1Distance,2)-pow(RleftChainTolerance,2));
+    float Chain2Straight = sqrt(pow(Motor2Distance,2)-pow(RrightChainTolerance,2));
 
     //Correct the straight chain lengths to account for chain sag
     Chain1Straight *= (1 + ((sysSettings.chainSagCorrection / 1000000000000) * pow(cos(Chain1Angle),2) * pow(Chain1Straight,2) * pow((tan(Chain2Angle) * cos(Chain1Angle)) + sin(Chain1Angle),2)));

cnc_ctrl_v1/Kinematics.h

@@ -38,6 +38,9 @@
             //geometry
             float h; //distance between sled attach point and bit
             float R             = 10.1;                                //sprocket radius
+            float RleftChainTolerance = 10.1;    // Left sprocket radius including chain tolerance
+            float RrightChainTolerance = 10.1;    // Right sprocket radius including chain tolerance
+
 
 
             float halfWidth;                      //Half the machine width

cnc_ctrl_v1/Report.cpp

@@ -167,6 +167,9 @@ void reportMaslowSettings() {
     Serial.print(F("$37=")); Serial.println(sysSettings.chainSagCorrection, 8);
     Serial.print(F("$38=")); Serial.println(sysSettings.chainOverSprocket);
     Serial.print(F("$39=")); Serial.println(sysSettings.fPWM);
+    Serial.print(F("$40=")); Serial.println(sysSettings.distPerRotLeftChainTolerance, 8);
+    Serial.print(F("$41=")); Serial.println(sysSettings.distPerRotRightChainTolerance, 8);
+
   #else
     Serial.print(F("$0=")); Serial.print(sysSettings.machineWidth);
     Serial.print(F(" (machine width, mm)\r\n$1=")); Serial.print(sysSettings.machineHeight, 8);
@@ -207,7 +210,9 @@ void reportMaslowSettings() {
     Serial.print(F(" (z axis Velocity proportional weight)\r\n$37=")); Serial.print(sysSettings.chainSagCorrection, 8);
     Serial.print(F(" (chain sag correction value)\r\n$38=")); Serial.print(sysSettings.chainOverSprocket);
     Serial.print(F(" (chain over sprocket)\r\n$39=")); Serial.print(sysSettings.fPWM);
-    Serial.print(F(" (PWM frequency value 1=39,000Hz, 2=4,100Hz, 3=490Hz)"));
+    Serial.print(F(" (PWM frequency value 1=39,000Hz, 2=4,100Hz, 3=490Hz)\r\n$40=")); Serial.print(sysSettings.distPerRotLeftChainTolerance, 8);
+    Serial.print(F(" (distance / rotation, including chain tolerance, left chain, mm)\r\n$41=")); Serial.print(sysSettings.distPerRotRightChainTolerance, 8);
+    Serial.print(F(" (distance / rotation, including chain tolerance, right chain, mm)"));
     Serial.println();
   #endif
 }
@@ -277,4 +282,4 @@ void  reportMaslowHelp(){
         // Serial.println(F("? (current status)"));
         // Serial.println(F("ctrl-x (reset Maslow)"));
     #endif
-}
+}

cnc_ctrl_v1/Settings.cpp

@@ -49,8 +49,8 @@ void settingsLoadFromEEprom(){
     kinematics.recomputeGeometry();
     leftAxis.changeEncoderResolution(&sysSettings.encoderSteps);
     rightAxis.changeEncoderResolution(&sysSettings.encoderSteps);
-    leftAxis.changePitch(&sysSettings.distPerRot);
-    rightAxis.changePitch(&sysSettings.distPerRot);
+    leftAxis.changePitch(&sysSettings.distPerRotLeftChainTolerance);
+    rightAxis.changePitch(&sysSettings.distPerRotRightChainTolerance);
     zAxis.changePitch(&sysSettings.zDistPerRot);
     zAxis.changeEncoderResolution(&sysSettings.zEncoderSteps);
 }
@@ -101,6 +101,8 @@ void settingsReset() {
     sysSettings.chainSagCorrection = 0.0;  // float chainSagCorrection;
     sysSettings.chainOverSprocket = 1;   // byte chainOverSprocket;
     sysSettings.fPWM = 3;   // byte fPWM;
+    sysSettings.distPerRotLeftChainTolerance = 63.5;    // float distPerRotLeftChainTolerance;
+    sysSettings.distPerRotRightChainTolerance = 63.5;    // float distPerRotRightChainTolerance;
     sysSettings.eepromValidData = EEPROMVALIDDATA; // byte eepromValidData;
 }
 
@@ -275,8 +277,6 @@ byte settingsStoreGlobalSetting(const byte& parameter,const float& value){
               break;
         case 13: 
               sysSettings.distPerRot = value;
-              leftAxis.changePitch(&sysSettings.distPerRot);
-              rightAxis.changePitch(&sysSettings.distPerRot);
               kinematics.R = (sysSettings.distPerRot)/(2.0 * 3.14159);
               if (sys.oldSettingsFlag){
                 bit_false(sys.oldSettingsFlag, NEED_DIST_PER_ROT);
@@ -394,6 +394,16 @@ byte settingsStoreGlobalSetting(const byte& parameter,const float& value){
               sysSettings.fPWM = value;
               setPWMPrescalers(value);
               break;
+        case 40:
+              sysSettings.distPerRotLeftChainTolerance = value;
+              leftAxis.changePitch(&sysSettings.distPerRotLeftChainTolerance);
+              kinematics.RleftChainTolerance = (sysSettings.distPerRotLeftChainTolerance)/(2.0 * 3.14159);
+              break;
+        case 41:
+              sysSettings.distPerRotRightChainTolerance = value;
+              rightAxis.changePitch(&sysSettings.distPerRotRightChainTolerance);
+              kinematics.RrightChainTolerance = (sysSettings.distPerRotRightChainTolerance)/(2.0 * 3.14159);
+              break;
         default:
               return(STATUS_INVALID_STATEMENT);
     }

cnc_ctrl_v1/Settings.h

@@ -72,6 +72,8 @@ typedef struct {  // I think this is about ~128 bytes in size if I counted corre
   float chainSagCorrection;
   byte chainOverSprocket;
   byte fPWM;
+  float distPerRotLeftChainTolerance;
+  float distPerRotRightChainTolerance;
   byte eepromValidData;  // This should always be last, that way if an error
                          // happens in writing, it will not be written and we
 } settings_t;            // will know to reset the settings
@@ -97,4 +99,4 @@ void settingsSaveStepstoEEprom();
 void settingsLoadStepsFromEEprom();
 byte settingsStoreGlobalSetting(const byte&,const float&);
 
-#endif
+#endif