From 7d9c11824469e4bd9d4a7ef452a838245f6c3de6 Mon Sep 17 00:00:00 2001 From: Kevin R Keegan Date: Wed, 27 Sep 2017 19:57:10 -0700 Subject: [PATCH 1/7] Single Loop Timing Variable; Improve Movement Loop Accuracy Unify loop timing setting to a single variable for easy adjustments. Improve Movement loop accuracy by lays counting from the same starting point rather than resetting each time. Likely is a litte if any improvement, but seems more correct. --- cnc_ctrl_v1/CNC_Functions.h | 24 +++++------------------- cnc_ctrl_v1/cnc_ctrl_v1.ino | 2 +- 2 files changed, 6 insertions(+), 20 deletions(-) diff --git a/cnc_ctrl_v1/CNC_Functions.h b/cnc_ctrl_v1/CNC_Functions.h index a866a8f3..85ebe597 100644 --- a/cnc_ctrl_v1/CNC_Functions.h +++ b/cnc_ctrl_v1/CNC_Functions.h @@ -43,7 +43,7 @@ bool zAxisAttached = false; #define INCHES 25.4 #define MAXFEED 900 //The maximum allowable feedrate in mm/min #define MAXZROTMIN 12.60 // the maximum z rotations per minute - +#define LOOPINTERVAL 10.00 int ENCODER1A; int ENCODER1B; @@ -376,13 +376,7 @@ float calculateDelay(const float& stepSizeMM, const float& feedrateMMPerMin){ Calculate the time delay between each step for a given feedrate */ - #define MINUTEINMS 60000.0 - - // derivation: ms / step = 1 min in ms / dist in one min - - float msPerStep = (stepSizeMM*MINUTEINMS)/feedrateMMPerMin; - - return msPerStep; + return LOOPINTERVAL; } float calculateFeedrate(const float& stepSizeMM, const float& msPerStep){ @@ -407,7 +401,7 @@ float computeStepSize(const float& MMPerMin){ */ - return .0001575*MMPerMin; //value found empirically by running loop until there were not spare cycles + return (LOOPINTERVAL/60000)*MMPerMin; } int cordinatedMove(const float& xEnd, const float& yEnd, const float& zEnd, float MMPerMin){ @@ -463,10 +457,7 @@ int cordinatedMove(const float& xEnd, const float& yEnd, const float& zEnd, fl while(numberOfStepsTaken < finalNumberOfSteps){ //if enough time has passed to take the next step - if (millis() - beginingOfLastStep > delayTime){ - - //reset the counter - beginingOfLastStep = millis(); + if (millis() - beginingOfLoop > (LOOPINTERVAL*numberOfStepsTaken)) { //find the target point for this step float whereXShouldBeAtThisStep = xStartingLocation + (numberOfStepsTaken*xStepSize); @@ -752,8 +743,6 @@ int arc(const float& X1, const float& Y1, const float& X2, const float& Y2, co float aChainLength; float bChainLength; - - float delayTime = calculateDelay(stepSizeMM, MMPerMin); //attach the axes leftAxis.attach(); @@ -764,10 +753,7 @@ int arc(const float& X1, const float& Y1, const float& X2, const float& Y2, co while(numberOfStepsTaken < abs(finalNumberOfSteps)){ //if enough time has passed to take the next step - if (millis() - beginingOfLastStep > delayTime){ - - //reset the counter - beginingOfLastStep = millis(); + if (millis() - beginingOfLoop > (numberOfStepsTaken * LOOPINTERVAL)){ degreeComplete = float(numberOfStepsTaken)/float(finalNumberOfSteps); diff --git a/cnc_ctrl_v1/cnc_ctrl_v1.ino b/cnc_ctrl_v1/cnc_ctrl_v1.ino index 9b0c7779..e6cb8025 100644 --- a/cnc_ctrl_v1/cnc_ctrl_v1.ino +++ b/cnc_ctrl_v1/cnc_ctrl_v1.ino @@ -32,7 +32,7 @@ void setup(){ _signalReady(); - Timer1.initialize(10000); + Timer1.initialize(LOOPINTERVAL*1000); Timer1.attachInterrupt(runsOnATimer); Serial.println(F("Grbl v1.00")); From e13c1f6fe6227c4932692dc1495929af7005a6b3 Mon Sep 17 00:00:00 2001 From: Kevin R Keegan Date: Fri, 29 Sep 2017 20:52:35 -0700 Subject: [PATCH 2/7] Allow Variable Single Axis Move Step; Fix Timing Errors --- cnc_ctrl_v1/CNC_Functions.h | 18 +++++++----------- 1 file changed, 7 insertions(+), 11 deletions(-) diff --git a/cnc_ctrl_v1/CNC_Functions.h b/cnc_ctrl_v1/CNC_Functions.h index 85ebe597..fd6ad666 100644 --- a/cnc_ctrl_v1/CNC_Functions.h +++ b/cnc_ctrl_v1/CNC_Functions.h @@ -453,7 +453,7 @@ int cordinatedMove(const float& xEnd, const float& yEnd, const float& zEnd, fl float bChainLength; float zPosition = zStartingLocation; long numberOfStepsTaken = 0; - unsigned long beginingOfLastStep = millis() - delayTime; + unsigned long beginingOfLoop = millis(); while(numberOfStepsTaken < finalNumberOfSteps){ //if enough time has passed to take the next step @@ -527,24 +527,22 @@ void singleAxisMove(Axis* axis, const float& endPos, const float& MMPerMin){ float direction = moveDist/abs(moveDist); //determine the direction of the move - float stepSizeMM = 0.01; //step size in mm + float stepSizeMM = computeStepSize(MMPerMin); //step size in mm //the argument to abs should only be a variable -- splitting calc into 2 lines long finalNumberOfSteps = abs(moveDist/stepSizeMM); //number of steps taken in move finalNumberOfSteps = abs(finalNumberOfSteps); - - float delayTime = calculateDelay(stepSizeMM, MMPerMin); long numberOfStepsTaken = 0; //attach the axis we want to move axis->attach(); - unsigned long beginingOfLastStep = millis() - delayTime; + unsigned long beginingOfLastStep = millis() - LOOPINTERVAL; float whereAxisShouldBeAtThisStep; while(numberOfStepsTaken < finalNumberOfSteps){ - if (millis() - beginingOfLastStep > delayTime){ + if (millis() - beginingOfLastStep > LOOPINTERVAL){ beginingOfLastStep = millis(); //find the target point for this step whereAxisShouldBeAtThisStep = startingPos + numberOfStepsTaken*stepSizeMM*direction; @@ -748,7 +746,7 @@ int arc(const float& X1, const float& Y1, const float& X2, const float& Y2, co leftAxis.attach(); rightAxis.attach(); - unsigned long beginingOfLastStep = millis() - delayTime; + unsigned long beginingOfLoop = millis(); while(numberOfStepsTaken < abs(finalNumberOfSteps)){ @@ -902,17 +900,15 @@ void G38(const String& readString) { long finalNumberOfSteps = moveDist / stepSizeMM; //number of steps taken in move finalNumberOfSteps = abs(finalNumberOfSteps); - float delayTime = calculateDelay(stepSizeMM, MMPerMin); - long numberOfStepsTaken = 0; - unsigned long beginingOfLastStep = millis() - delayTime; + unsigned long beginingOfLastStep = millis() - LOOPINTERVAL; float whereAxisShouldBeAtThisStep; axis->attach(); // zAxis->attach(); while (numberOfStepsTaken < finalNumberOfSteps) { - if (millis() - beginingOfLastStep > delayTime){ + if (millis() - beginingOfLastStep > LOOPINTERVAL){ //reset the counter beginingOfLastStep = millis(); From 424e05054550698ce611712d63830b52edb00607 Mon Sep 17 00:00:00 2001 From: Kevin R Keegan Date: Sun, 1 Oct 2017 16:25:04 -0700 Subject: [PATCH 3/7] Change Loop Interval to Microseconds; Remove Movement Loop Timer Instead of running the movement loop on a timer, run whenever the PID loop has cleared the last move. There was an odd instance in which the movement loop was scheduled to happen just before PID loop. In that case, the movement loop would at times fail to complete (maybe becuase of a serial interrupt). The movement loop would continue to oscillate like this. The new code does have the issue wherein if the movement loop fails to complete, it will get slowed down because it no longer has the concept of time. This is fine, if this is happening a lot then the loop interval is too high. --- cnc_ctrl_v1/CNC_Functions.h | 67 +++++++++++++++++++++---------------- cnc_ctrl_v1/cnc_ctrl_v1.ino | 3 +- 2 files changed, 39 insertions(+), 31 deletions(-) diff --git a/cnc_ctrl_v1/CNC_Functions.h b/cnc_ctrl_v1/CNC_Functions.h index fd6ad666..a541ba93 100644 --- a/cnc_ctrl_v1/CNC_Functions.h +++ b/cnc_ctrl_v1/CNC_Functions.h @@ -43,7 +43,7 @@ bool zAxisAttached = false; #define INCHES 25.4 #define MAXFEED 900 //The maximum allowable feedrate in mm/min #define MAXZROTMIN 12.60 // the maximum z rotations per minute -#define LOOPINTERVAL 10.00 +#define LOOPINTERVAL 10000 // What is the frequency of the PID loop in microseconds int ENCODER1A; int ENCODER1B; @@ -150,6 +150,8 @@ bool rcvdKinematicSettings = false; bool rcvdMotorSettings = false; bool encoderStepsChanged = false; bool zEncoderStepsChanged = false; +volatile bool movementUpdated = false; + // Commands that can safely be executed before machineReady String safeCommands[] = {"B01", "B03", "B04", "B05", "B07", "B12", "G20", "G21", "G90", "G91"}; String readyCommandString; //next command queued up and ready to send @@ -373,7 +375,7 @@ bool checkForProbeTouch(const int& probePin) { float calculateDelay(const float& stepSizeMM, const float& feedrateMMPerMin){ /* - Calculate the time delay between each step for a given feedrate + Calculate the time delay in microseconds between each step for a given feedrate */ return LOOPINTERVAL; @@ -397,11 +399,15 @@ float computeStepSize(const float& MMPerMin){ /* Determines the minimum step size which can be taken for the given feed-rate - and still have there be enough time for the kinematics to run + based on the loop interval frequency. Converts to MM per microsecond first, + then mutiplies by the number of microseconds in each loop interval */ - - return (LOOPINTERVAL/60000)*MMPerMin; + return LOOPINTERVAL*(MMPerMin/(60 * 1000000)); +} + +void movementUpdate(){ + movementUpdated = true; } int cordinatedMove(const float& xEnd, const float& yEnd, const float& zEnd, float MMPerMin){ @@ -452,34 +458,41 @@ int cordinatedMove(const float& xEnd, const float& yEnd, const float& zEnd, fl float aChainLength; float bChainLength; float zPosition = zStartingLocation; + float whereXShouldBeAtThisStep = xStartingLocation; + float whereYShouldBeAtThisStep = yStartingLocation; long numberOfStepsTaken = 0; - unsigned long beginingOfLoop = millis(); while(numberOfStepsTaken < finalNumberOfSteps){ - //if enough time has passed to take the next step - if (millis() - beginingOfLoop > (LOOPINTERVAL*numberOfStepsTaken)) { - + + //if last movment was performed start the next + if (!movementUpdated) { //find the target point for this step - float whereXShouldBeAtThisStep = xStartingLocation + (numberOfStepsTaken*xStepSize); - float whereYShouldBeAtThisStep = yStartingLocation + (numberOfStepsTaken*yStepSize); - zPosition = zStartingLocation + (numberOfStepsTaken*zStepSize); + // This section ~20us + whereXShouldBeAtThisStep += xStepSize; + whereYShouldBeAtThisStep += yStepSize; + zPosition += zStepSize; //find the chain lengths for this step + // This section ~180us kinematics.inverse(whereXShouldBeAtThisStep,whereYShouldBeAtThisStep,&aChainLength,&bChainLength); //write to each axis + // This section ~180us leftAxis.write(aChainLength); rightAxis.write(bChainLength); if(zAxisAttached){ zAxis.write(zPosition); } + movementUpdate(); + //increment the number of steps taken numberOfStepsTaken++; //update position on display returnPoz(whereXShouldBeAtThisStep, whereYShouldBeAtThisStep, zPosition); + // This section ~10us //check for new serial commands readSerialCommands(); @@ -532,23 +545,23 @@ void singleAxisMove(Axis* axis, const float& endPos, const float& MMPerMin){ //the argument to abs should only be a variable -- splitting calc into 2 lines long finalNumberOfSteps = abs(moveDist/stepSizeMM); //number of steps taken in move finalNumberOfSteps = abs(finalNumberOfSteps); + stepSizeMM = stepSizeMM*direction; long numberOfStepsTaken = 0; //attach the axis we want to move axis->attach(); - unsigned long beginingOfLastStep = millis() - LOOPINTERVAL; - float whereAxisShouldBeAtThisStep; + float whereAxisShouldBeAtThisStep = startingPos; while(numberOfStepsTaken < finalNumberOfSteps){ - if (millis() - beginingOfLastStep > LOOPINTERVAL){ - beginingOfLastStep = millis(); + if (!movementUpdated) { //find the target point for this step - whereAxisShouldBeAtThisStep = startingPos + numberOfStepsTaken*stepSizeMM*direction; + whereAxisShouldBeAtThisStep += stepSizeMM; //write to axis axis->write(whereAxisShouldBeAtThisStep); + movementUpdate(); //update position on display returnPoz(xTarget, yTarget, zAxis.read()); @@ -746,12 +759,10 @@ int arc(const float& X1, const float& Y1, const float& X2, const float& Y2, co leftAxis.attach(); rightAxis.attach(); - unsigned long beginingOfLoop = millis(); - while(numberOfStepsTaken < abs(finalNumberOfSteps)){ - //if enough time has passed to take the next step - if (millis() - beginingOfLoop > (numberOfStepsTaken * LOOPINTERVAL)){ + //if last movement was performed start the next one + if (!movementUpdated){ degreeComplete = float(numberOfStepsTaken)/float(finalNumberOfSteps); @@ -763,7 +774,8 @@ int arc(const float& X1, const float& Y1, const float& X2, const float& Y2, co kinematics.inverse(whereXShouldBeAtThisStep,whereYShouldBeAtThisStep,&aChainLength,&bChainLength); leftAxis.write(aChainLength); - rightAxis.write(bChainLength); + rightAxis.write(bChainLength); + movementUpdate(); returnPoz(whereXShouldBeAtThisStep, whereYShouldBeAtThisStep, zAxis.read()); @@ -901,22 +913,19 @@ void G38(const String& readString) { finalNumberOfSteps = abs(finalNumberOfSteps); long numberOfStepsTaken = 0; - unsigned long beginingOfLastStep = millis() - LOOPINTERVAL; - float whereAxisShouldBeAtThisStep; + float whereAxisShouldBeAtThisStep = startingPos; axis->attach(); // zAxis->attach(); while (numberOfStepsTaken < finalNumberOfSteps) { - if (millis() - beginingOfLastStep > LOOPINTERVAL){ - //reset the counter - beginingOfLastStep = millis(); - + if (!movementUpdated){ //find the target point for this step - whereAxisShouldBeAtThisStep = startingPos + numberOfStepsTaken * stepSizeMM * direction; + whereAxisShouldBeAtThisStep += stepSizeMM * direction; //write to each axis axis->write(whereAxisShouldBeAtThisStep); + movementUpdate(); //update position on display returnPoz(xTarget, yTarget, zAxis.read()); diff --git a/cnc_ctrl_v1/cnc_ctrl_v1.ino b/cnc_ctrl_v1/cnc_ctrl_v1.ino index e6cb8025..9ae3b004 100644 --- a/cnc_ctrl_v1/cnc_ctrl_v1.ino +++ b/cnc_ctrl_v1/cnc_ctrl_v1.ino @@ -31,8 +31,7 @@ void setup(){ Serial.println(F("ready")); _signalReady(); - - Timer1.initialize(LOOPINTERVAL*1000); + Timer1.initialize(LOOPINTERVAL); Timer1.attachInterrupt(runsOnATimer); Serial.println(F("Grbl v1.00")); From 3506cbe60bb7c5972ed8ed435617f4361b65c0e4 Mon Sep 17 00:00:00 2001 From: Kevin R Keegan Date: Sun, 1 Oct 2017 16:29:59 -0700 Subject: [PATCH 4/7] Add Missed Movement Loop Debugging Option Very helpful for loop interval tuning. --- cnc_ctrl_v1/CNC_Functions.h | 33 +++++++++++++++++++++++++++++++++ cnc_ctrl_v1/cnc_ctrl_v1.ino | 6 ++++++ 2 files changed, 39 insertions(+) diff --git a/cnc_ctrl_v1/CNC_Functions.h b/cnc_ctrl_v1/CNC_Functions.h index a541ba93..df1a12de 100644 --- a/cnc_ctrl_v1/CNC_Functions.h +++ b/cnc_ctrl_v1/CNC_Functions.h @@ -23,6 +23,9 @@ libraries*/ #define VERSIONNUMBER 0.94 #define verboseDebug 0 // set to 0 for no debug messages, 1 for single-line messages, 2 to also output ring buffer contents +#define misloopDebug 0 // set to 1 for a arning evertime the movement loop fails + // to complete before being interrupted, helpful for loop + // LOOPINTERVAL tuning #include Servo myservo; // create servo object to control a servo @@ -152,6 +155,12 @@ bool encoderStepsChanged = false; bool zEncoderStepsChanged = false; volatile bool movementUpdated = false; +// Global variables for misloop tracking +#if misloopDebug > 1 +volatile bool inMovementLoop = false; +volatile bool movementFail = false; +#endif + // Commands that can safely be executed before machineReady String safeCommands[] = {"B01", "B03", "B04", "B05", "B07", "B12", "G20", "G21", "G90", "G91"}; String readyCommandString; //next command queued up and ready to send @@ -407,6 +416,12 @@ float computeStepSize(const float& MMPerMin){ } void movementUpdate(){ + #if misloopDebug > 1 + if (movementFail){ + Serial.println("Movement loop failed to complete before interrupt."); + movementFail = false; + } + #endif movementUpdated = true; } @@ -464,6 +479,9 @@ int cordinatedMove(const float& xEnd, const float& yEnd, const float& zEnd, fl while(numberOfStepsTaken < finalNumberOfSteps){ + #if misloopDebug > 1 + inMovementLoop = true; + #endif //if last movment was performed start the next if (!movementUpdated) { //find the target point for this step @@ -515,6 +533,9 @@ int cordinatedMove(const float& xEnd, const float& yEnd, const float& zEnd, fl } } } + #if misloopDebug > 1 + inMovementLoop = false; + #endif kinematics.inverse(xEnd,yEnd,&aChainLength,&bChainLength); leftAxis.endMove(aChainLength); @@ -554,6 +575,9 @@ void singleAxisMove(Axis* axis, const float& endPos, const float& MMPerMin){ float whereAxisShouldBeAtThisStep = startingPos; + #if misloopDebug > 1 + inMovementLoop = true; + #endif while(numberOfStepsTaken < finalNumberOfSteps){ if (!movementUpdated) { //find the target point for this step @@ -579,6 +603,9 @@ void singleAxisMove(Axis* axis, const float& endPos, const float& MMPerMin){ return; } } + #if misloopDebug > 1 + inMovementLoop = false; + #endif axis->endMove(endPos); @@ -760,6 +787,9 @@ int arc(const float& X1, const float& Y1, const float& X2, const float& Y2, co rightAxis.attach(); while(numberOfStepsTaken < abs(finalNumberOfSteps)){ + #if misloopDebug > 1 + inMovementLoop = true; + #endif //if last movement was performed start the next one if (!movementUpdated){ @@ -799,6 +829,9 @@ int arc(const float& X1, const float& Y1, const float& X2, const float& Y2, co } } } + #if misloopDebug > 1 + inMovementLoop = false; + #endif kinematics.inverse(X2,Y2,&aChainLength,&bChainLength); leftAxis.endMove(aChainLength); diff --git a/cnc_ctrl_v1/cnc_ctrl_v1.ino b/cnc_ctrl_v1/cnc_ctrl_v1.ino index 9ae3b004..acb2c962 100644 --- a/cnc_ctrl_v1/cnc_ctrl_v1.ino +++ b/cnc_ctrl_v1/cnc_ctrl_v1.ino @@ -39,6 +39,12 @@ void setup(){ } void runsOnATimer(){ + #if misloopDebug > 1 + if (inMovementLoop && !movementUpdated){ + movementFail = true; + } + #endif + movementUpdated = false; leftAxis.computePID(); rightAxis.computePID(); zAxis.computePID(); From 8a2ed39ac9036f492f164ec42957c688a6d04867 Mon Sep 17 00:00:00 2001 From: Kevin R Keegan Date: Sun, 1 Oct 2017 18:36:29 -0700 Subject: [PATCH 5/7] Fix Error in Misloop Debug Statements --- cnc_ctrl_v1/CNC_Functions.h | 17 ++++++++--------- cnc_ctrl_v1/cnc_ctrl_v1.ino | 2 +- 2 files changed, 9 insertions(+), 10 deletions(-) diff --git a/cnc_ctrl_v1/CNC_Functions.h b/cnc_ctrl_v1/CNC_Functions.h index df1a12de..fbd2e4ea 100644 --- a/cnc_ctrl_v1/CNC_Functions.h +++ b/cnc_ctrl_v1/CNC_Functions.h @@ -156,7 +156,7 @@ bool zEncoderStepsChanged = false; volatile bool movementUpdated = false; // Global variables for misloop tracking -#if misloopDebug > 1 +#if misloopDebug > 0 volatile bool inMovementLoop = false; volatile bool movementFail = false; #endif @@ -416,7 +416,7 @@ float computeStepSize(const float& MMPerMin){ } void movementUpdate(){ - #if misloopDebug > 1 + #if misloopDebug > 0 if (movementFail){ Serial.println("Movement loop failed to complete before interrupt."); movementFail = false; @@ -479,7 +479,7 @@ int cordinatedMove(const float& xEnd, const float& yEnd, const float& zEnd, fl while(numberOfStepsTaken < finalNumberOfSteps){ - #if misloopDebug > 1 + #if misloopDebug > 0 inMovementLoop = true; #endif //if last movment was performed start the next @@ -533,7 +533,7 @@ int cordinatedMove(const float& xEnd, const float& yEnd, const float& zEnd, fl } } } - #if misloopDebug > 1 + #if misloopDebug > 0 inMovementLoop = false; #endif @@ -574,8 +574,7 @@ void singleAxisMove(Axis* axis, const float& endPos, const float& MMPerMin){ axis->attach(); float whereAxisShouldBeAtThisStep = startingPos; - - #if misloopDebug > 1 + #if misloopDebug > 0 inMovementLoop = true; #endif while(numberOfStepsTaken < finalNumberOfSteps){ @@ -603,7 +602,7 @@ void singleAxisMove(Axis* axis, const float& endPos, const float& MMPerMin){ return; } } - #if misloopDebug > 1 + #if misloopDebug > 0 inMovementLoop = false; #endif @@ -787,7 +786,7 @@ int arc(const float& X1, const float& Y1, const float& X2, const float& Y2, co rightAxis.attach(); while(numberOfStepsTaken < abs(finalNumberOfSteps)){ - #if misloopDebug > 1 + #if misloopDebug > 0 inMovementLoop = true; #endif @@ -829,7 +828,7 @@ int arc(const float& X1, const float& Y1, const float& X2, const float& Y2, co } } } - #if misloopDebug > 1 + #if misloopDebug > 0 inMovementLoop = false; #endif diff --git a/cnc_ctrl_v1/cnc_ctrl_v1.ino b/cnc_ctrl_v1/cnc_ctrl_v1.ino index acb2c962..68c3540d 100644 --- a/cnc_ctrl_v1/cnc_ctrl_v1.ino +++ b/cnc_ctrl_v1/cnc_ctrl_v1.ino @@ -39,7 +39,7 @@ void setup(){ } void runsOnATimer(){ - #if misloopDebug > 1 + #if misloopDebug > 0 if (inMovementLoop && !movementUpdated){ movementFail = true; } From 50b38a3b483978318981c2779f9801d29198fd35 Mon Sep 17 00:00:00 2001 From: Kevin R Keegan Date: Fri, 13 Oct 2017 15:22:31 -0700 Subject: [PATCH 6/7] Adjust PID Timing Based on Loop Interval Value --- cnc_ctrl_v1/Axis.cpp | 2 +- cnc_ctrl_v1/MotorGearboxEncoder.cpp | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/cnc_ctrl_v1/Axis.cpp b/cnc_ctrl_v1/Axis.cpp index 1c2d72a6..eb1a2ad6 100644 --- a/cnc_ctrl_v1/Axis.cpp +++ b/cnc_ctrl_v1/Axis.cpp @@ -58,7 +58,7 @@ void Axis::loadPositionFromMemory(){ void Axis::initializePID(){ _pidController.SetMode(AUTOMATIC); _pidController.SetOutputLimits(-20, 20); - _pidController.SetSampleTime(10); + _pidController.SetSampleTime(LOOPINTERVAL / 1000); } void Axis::write(const float& targetPosition){ diff --git a/cnc_ctrl_v1/MotorGearboxEncoder.cpp b/cnc_ctrl_v1/MotorGearboxEncoder.cpp index 79f3f765..951514c7 100644 --- a/cnc_ctrl_v1/MotorGearboxEncoder.cpp +++ b/cnc_ctrl_v1/MotorGearboxEncoder.cpp @@ -53,7 +53,7 @@ void MotorGearboxEncoder::initializePID(){ //setup positive PID controller _PIDController.SetMode(AUTOMATIC); _PIDController.SetOutputLimits(-255, 255); - _PIDController.SetSampleTime(10); + _PIDController.SetSampleTime(LOOPINTERVAL / 1000); } void MotorGearboxEncoder::computePID(){ From cee73bc65e35bd43a98fdf2766d22210c75ce6c3 Mon Sep 17 00:00:00 2001 From: Kevin R Keegan Date: Fri, 13 Oct 2017 19:52:18 -0700 Subject: [PATCH 7/7] Pass LoopInterval to Axis and MotorGearboxEncoder Preprocessor defines don't seem to work as a global --- cnc_ctrl_v1/Axis.cpp | 10 +++++----- cnc_ctrl_v1/Axis.h | 5 ++--- cnc_ctrl_v1/CNC_Functions.h | 6 +++--- cnc_ctrl_v1/MotorGearboxEncoder.cpp | 8 ++++---- cnc_ctrl_v1/MotorGearboxEncoder.h | 4 ++-- 5 files changed, 16 insertions(+), 17 deletions(-) diff --git a/cnc_ctrl_v1/Axis.cpp b/cnc_ctrl_v1/Axis.cpp index eb1a2ad6..764561cd 100644 --- a/cnc_ctrl_v1/Axis.cpp +++ b/cnc_ctrl_v1/Axis.cpp @@ -23,9 +23,9 @@ #define SIZEOFFLOAT 4 #define SIZEOFLINSEG 17 -Axis::Axis(const int& pwmPin, const int& directionPin1, const int& directionPin2, const int& encoderPin1, const int& encoderPin2, const char& axisName, const int& eepromAdr) +Axis::Axis(const int& pwmPin, const int& directionPin1, const int& directionPin2, const int& encoderPin1, const int& encoderPin2, const char& axisName, const int& eepromAdr, const unsigned long& loopInterval) : -motorGearboxEncoder(pwmPin, directionPin1, directionPin2, encoderPin1, encoderPin2) +motorGearboxEncoder(pwmPin, directionPin1, directionPin2, encoderPin1, encoderPin2, loopInterval) { _pidController.setup(&_pidInput, &_pidOutput, &_pidSetpoint, 0, 0, 0, P_ON_E, REVERSE); @@ -34,7 +34,7 @@ motorGearboxEncoder(pwmPin, directionPin1, directionPin2, encoderPin1, encoderPi _axisName = axisName; _eepromAdr = eepromAdr; - initializePID(); + initializePID(loopInterval); motorGearboxEncoder.setName(_axisName); } @@ -55,10 +55,10 @@ void Axis::loadPositionFromMemory(){ } -void Axis::initializePID(){ +void Axis::initializePID(const unsigned long& loopInterval){ _pidController.SetMode(AUTOMATIC); _pidController.SetOutputLimits(-20, 20); - _pidController.SetSampleTime(LOOPINTERVAL / 1000); + _pidController.SetSampleTime( loopInterval / 1000); } void Axis::write(const float& targetPosition){ diff --git a/cnc_ctrl_v1/Axis.h b/cnc_ctrl_v1/Axis.h index 5f6d5945..a196104d 100644 --- a/cnc_ctrl_v1/Axis.h +++ b/cnc_ctrl_v1/Axis.h @@ -22,16 +22,15 @@ #include "PID_v1.h" #include #include "MotorGearboxEncoder.h" - class Axis{ public: - Axis(const int& pwmPin, const int& directionPin1, const int& directionPin2, const int& encoderPin1, const int& encoderPin2, const char& axisName, const int& eepromAdr); + Axis(const int& pwmPin, const int& directionPin1, const int& directionPin2, const int& encoderPin1, const int& encoderPin2, const char& axisName, const int& eepromAdr, const unsigned long& loopInterval); void write(const float& targetPosition); float read(); void set(const float& newAxisPosition); int updatePositionFromEncoder(); - void initializePID(); + void initializePID(const unsigned long& loopInterval); int detach(); int attach(); void hold(); diff --git a/cnc_ctrl_v1/CNC_Functions.h b/cnc_ctrl_v1/CNC_Functions.h index fbd2e4ea..01de27a5 100644 --- a/cnc_ctrl_v1/CNC_Functions.h +++ b/cnc_ctrl_v1/CNC_Functions.h @@ -134,9 +134,9 @@ int setupPins(){ int pinsSetup = setupPins(); -Axis leftAxis (ENC, IN6, IN5, ENCODER3B, ENCODER3A, 'L', LEFT_EEPROM_ADR); -Axis rightAxis(ENA, IN1, IN2, ENCODER1A, ENCODER1B, 'R', RIGHT_EEPROM_ADR); -Axis zAxis (ENB, IN3, IN4, ENCODER2B, ENCODER2A, 'Z', Z_EEPROM_ADR); +Axis leftAxis (ENC, IN6, IN5, ENCODER3B, ENCODER3A, 'L', LEFT_EEPROM_ADR, LOOPINTERVAL); +Axis rightAxis(ENA, IN1, IN2, ENCODER1A, ENCODER1B, 'R', RIGHT_EEPROM_ADR, LOOPINTERVAL); +Axis zAxis (ENB, IN3, IN4, ENCODER2B, ENCODER2A, 'Z', Z_EEPROM_ADR, LOOPINTERVAL); Kinematics kinematics; diff --git a/cnc_ctrl_v1/MotorGearboxEncoder.cpp b/cnc_ctrl_v1/MotorGearboxEncoder.cpp index 951514c7..6dfef1b5 100644 --- a/cnc_ctrl_v1/MotorGearboxEncoder.cpp +++ b/cnc_ctrl_v1/MotorGearboxEncoder.cpp @@ -24,7 +24,7 @@ to be a drop in replacement for a continuous rotation servo. #include "Arduino.h" #include "MotorGearboxEncoder.h" -MotorGearboxEncoder::MotorGearboxEncoder(const int& pwmPin, const int& directionPin1, const int& directionPin2, const int& encoderPin1, const int& encoderPin2) +MotorGearboxEncoder::MotorGearboxEncoder(const int& pwmPin, const int& directionPin1, const int& directionPin2, const int& encoderPin1, const int& encoderPin2, const unsigned long& loopInterval) : encoder(encoderPin1,encoderPin2) { @@ -35,7 +35,7 @@ encoder(encoderPin1,encoderPin2) //initialize the PID _PIDController.setup(&_currentSpeed, &_pidOutput, &_targetSpeed, _Kp, _Ki, _Kd, P_ON_E, DIRECT); - initializePID(); + initializePID(loopInterval); } @@ -49,11 +49,11 @@ void MotorGearboxEncoder::write(const float& speed){ } -void MotorGearboxEncoder::initializePID(){ +void MotorGearboxEncoder::initializePID(const unsigned long& loopInterval){ //setup positive PID controller _PIDController.SetMode(AUTOMATIC); _PIDController.SetOutputLimits(-255, 255); - _PIDController.SetSampleTime(LOOPINTERVAL / 1000); + _PIDController.SetSampleTime(loopInterval / 1000); } void MotorGearboxEncoder::computePID(){ diff --git a/cnc_ctrl_v1/MotorGearboxEncoder.h b/cnc_ctrl_v1/MotorGearboxEncoder.h index cdf64dfd..005c5199 100644 --- a/cnc_ctrl_v1/MotorGearboxEncoder.h +++ b/cnc_ctrl_v1/MotorGearboxEncoder.h @@ -25,7 +25,7 @@ class MotorGearboxEncoder{ public: - MotorGearboxEncoder(const int& pwmPin, const int& directionPin1, const int& directionPin2, const int& encoderPin1, const int& encoderPin2); + MotorGearboxEncoder(const int& pwmPin, const int& directionPin1, const int& directionPin2, const int& encoderPin1, const int& encoderPin2, const unsigned long& loopInterval); Encoder encoder; Motor motor; float cachedSpeed(); @@ -33,7 +33,7 @@ void computePID(); void setName(const char& newName); char name(); - void initializePID(); + void initializePID(const unsigned long& loopInterval); void setPIDAggressiveness(float aggressiveness); void setPIDValues(float KpV, float KiV, float KdV); void setEncoderResolution(float resolution);