diff --git a/cnc_ctrl_v1/Kinematics.cpp b/cnc_ctrl_v1/Kinematics.cpp index 0bc1b2bc..39343522 100644 --- a/cnc_ctrl_v1/Kinematics.cpp +++ b/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))); diff --git a/cnc_ctrl_v1/Kinematics.h b/cnc_ctrl_v1/Kinematics.h index 7adfb22d..01b6379b 100644 --- a/cnc_ctrl_v1/Kinematics.h +++ b/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 diff --git a/cnc_ctrl_v1/Report.cpp b/cnc_ctrl_v1/Report.cpp index 6087b19f..3a90ce93 100644 --- a/cnc_ctrl_v1/Report.cpp +++ b/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 -} \ No newline at end of file +} diff --git a/cnc_ctrl_v1/Settings.cpp b/cnc_ctrl_v1/Settings.cpp index 845ed60d..492aec49 100644 --- a/cnc_ctrl_v1/Settings.cpp +++ b/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); } diff --git a/cnc_ctrl_v1/Settings.h b/cnc_ctrl_v1/Settings.h index f661ac2e..64210318 100644 --- a/cnc_ctrl_v1/Settings.h +++ b/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 \ No newline at end of file +#endif