Add Chain Wrap Direction

cnc_ctrl_v1/Kinematics.cpp

@@ -199,14 +199,32 @@ void  Kinematics::triangularInverse(float xTarget,float yTarget, float* aChainLe
 
     //Confirm that the coordinates are on the wood
     _verifyValidTarget(&xTarget, &yTarget);
-
+
+    //Set up variables
+    float Chain1Angle = 0;
+    float Chain2Angle = 0;
+    float Chain1AroundSprocket = 0;
+    float Chain2AroundSprocket = 0;
+
     //Calculate motor axes length to the bit
     float Motor1Distance = sqrt(pow((-1*_xCordOfMotor - xTarget),2)+pow((_yCordOfMotor - yTarget),2));
     float Motor2Distance = sqrt(pow((_xCordOfMotor - xTarget),2)+pow((_yCordOfMotor - yTarget),2));
 
-    //Calculate the chain angles from horizontal
-    float Chain1Angle = 3.14159 - acos(R/Motor1Distance) - acos((_yCordOfMotor - yTarget)/Motor1Distance);
-    float Chain2Angle = 3.14159 - acos(R/Motor2Distance) - acos((_yCordOfMotor - yTarget)/Motor2Distance);
+    //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);
+
+        Chain1AroundSprocket = R * Chain1Angle;
+        Chain2AroundSprocket = R * Chain2Angle;
+    }
+    else{
+        Chain1Angle = asin((_yCordOfMotor - yTarget)/Motor1Distance) - asin(R/Motor1Distance);
+        Chain2Angle = asin((_yCordOfMotor - yTarget)/Motor2Distance) - asin(R/Motor2Distance);
+
+        Chain1AroundSprocket = R * (3.14159 - Chain1Angle);
+        Chain2AroundSprocket = R * (3.14159 - Chain2Angle);
+    }
 
     //Calculate the straight chain length from the sprocket to the bit
     float Chain1Straight = sqrt(pow(Motor1Distance,2)-pow(R,2));
@@ -217,8 +235,8 @@ void  Kinematics::triangularInverse(float xTarget,float yTarget, float* aChainLe
     Chain2Straight *= (1 + ((sysSettings.chainSagCorrection / 1000000000000) * pow(cos(Chain2Angle),2) * pow(Chain2Straight,2) * pow((tan(Chain1Angle) * cos(Chain2Angle)) + sin(Chain2Angle),2)));
 
     //Calculate total chain lengths accounting for sprocket geometry and chain sag
-    float Chain1 = (R * Chain1Angle) + Chain1Straight;
-    float Chain2 = (R * Chain2Angle) + Chain2Straight;
+    float Chain1 = Chain1AroundSprocket + Chain1Straight;
+    float Chain2 = Chain2AroundSprocket + Chain2Straight;
 
     //Subtract of the virtual length which is added to the chain by the rotation mechanism
     Chain1 = Chain1 - sysSettings.rotationDiskRadius;

cnc_ctrl_v1/Report.cpp

@@ -164,6 +164,7 @@ void reportMaslowSettings() {
     Serial.print(F("$35=")); Serial.println(sysSettings.zKdV);
     Serial.print(F("$36=")); Serial.println(sysSettings.zPropWeightV);
     Serial.print(F("$37=")); Serial.println(sysSettings.chainSagCorrection);
+    Serial.print(F("$38=")); Serial.println(sysSettings.chainOverSprocket);
   #else
     Serial.print(F("$0=")); Serial.print(sysSettings.machineWidth);
     Serial.print(F(" (machine width, mm, $K)\r\n$1=")); Serial.print(sysSettings.machineHeight);
@@ -201,7 +202,8 @@ void reportMaslowSettings() {
     Serial.print(F(" (z axis Ki Velocity)\r\n$35=")); Serial.print(sysSettings.zKdV);
     Serial.print(F(" (z axis Kd Velocity)\r\n$36=")); Serial.print(sysSettings.zPropWeightV);
     Serial.print(F(" (z axis Velocity proportional weight)\r\n$37=")); Serial.print(sysSettings.chainSagCorrection);
-    Serial.println(F(" (chain sag correction value)"));
+    Serial.print(F(" (chain sag correction value)\r\n$38=")); Serial.print(sysSettings.chainOverSprocket);
+    Serial.println(F(" (chain over sprocket)"));
   #endif
 }
 

cnc_ctrl_v1/Settings.cpp

@@ -103,6 +103,7 @@ void settingsReset() {
     sysSettings.zKdV = 0.28;   // float zKdV;
     sysSettings.zPropWeightV = 1.0;    // float zPropWeightV;
     sysSettings.chainSagCorrection = 0.0;  // float chainSagCorrection;
+    sysSettings.chainOverSprocket = 1;   // byte chainOverSprocket;
     sysSettings.eepromValidData = EEPROMVALIDDATA; // byte eepromValidData;
 }
 
@@ -384,6 +385,9 @@ byte settingsStoreGlobalSetting(const byte& parameter,const float& value){
         case 37:
               sysSettings.chainSagCorrection = value;
               break;
+        case 38:
+              sysSettings.chainOverSprocket = value;
+              break;
         default:
               return(STATUS_INVALID_STATEMENT);
     }

cnc_ctrl_v1/Settings.h

@@ -69,6 +69,7 @@ typedef struct {  // I think this is about ~128 bytes in size if I counted corre
   float zKdV;
   float zPropWeightV;
   float chainSagCorrection;
+  byte chainOverSprocket;
   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

cnc_ctrl_v1/System.cpp

@@ -151,8 +151,16 @@ void   setupAxes(){
 
 
     }
-    leftAxis.setup (ENC, IN6, IN5, ENCODER3B, ENCODER3A, 'L', LOOPINTERVAL);
-    rightAxis.setup(ENA, IN1, IN2, ENCODER1A, ENCODER1B, 'R', LOOPINTERVAL);
+
+    if(sysSettings.chainOverSprocket == 1){
+        leftAxis.setup (ENC, IN6, IN5, ENCODER3B, ENCODER3A, 'L', LOOPINTERVAL);
+        rightAxis.setup(ENA, IN1, IN2, ENCODER1A, ENCODER1B, 'R', LOOPINTERVAL);
+    }
+    else{
+        leftAxis.setup (ENC, IN5, IN6, ENCODER3A, ENCODER3B, 'L', LOOPINTERVAL);
+        rightAxis.setup(ENA, IN2, IN1, ENCODER1B, ENCODER1A, 'R', LOOPINTERVAL);
+    }
+
     zAxis.setup    (ENB, IN3, IN4, ENCODER2B, ENCODER2A, 'Z', LOOPINTERVAL);
 }
 

cnc_ctrl_v1/System.h

@@ -83,6 +83,7 @@ void maslowDelay(unsigned long);
 void  _watchDog();
 void execSystemRealtime();
 void systemSaveAxesPosition();
+void systemReset();
 byte systemExecuteCmdstring(String&);
 
 #endif

cnc_ctrl_v1/cnc_ctrl_v1.ino

@@ -38,8 +38,8 @@ void setup(){
     Serial.print(getPCBVersion());
     Serial.println(F(" Detected"));
     sys.inchesToMMConversion = 1;
-    setupAxes();
     settingsInit();
+    setupAxes();
     // TODO This seems wrong, if the encoder steps are changed, axis position
     // will be in the wrong place.  Would be better if we stored positions as
     // steps