Skip to content

Commit

Permalink
Merge pull request #319 from krkeegan/loop_timing_improvements
Browse files Browse the repository at this point in the history 
Configurable Loop Timing; Improved Movement Loop Timing; and Debugging
  • Loading branch information
@BarbourSmith
BarbourSmith authored Oct 16, 2017
2 parents cdeede3 + cee73bc commit ecf7e23
Show file tree
Hide file tree
Showing 6 changed files with 96 additions and 69 deletions.
10 changes: 5 additions & 5 deletions cnc_ctrl_v1/Axis.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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);
Expand All @@ -34,7 +34,7 @@ motorGearboxEncoder(pwmPin, directionPin1, directionPin2, encoderPin1, encoderPi
_axisName = axisName;
_eepromAdr = eepromAdr;

initializePID();
initializePID(loopInterval);

motorGearboxEncoder.setName(_axisName);
}
Expand All @@ -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(10);
_pidController.SetSampleTime( loopInterval / 1000);
}

void Axis::write(const float& targetPosition){
Expand Down
5 changes: 2 additions & 3 deletions cnc_ctrl_v1/Axis.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -22,16 +22,15 @@
#include "PID_v1.h"
#include <EEPROM.h>
#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();
Expand Down
129 changes: 76 additions & 53 deletions cnc_ctrl_v1/CNC_Functions.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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.h>
Servo myservo; // create servo object to control a servo
Expand All @@ -43,7 +46,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 10000 // What is the frequency of the PID loop in microseconds

int ENCODER1A;
int ENCODER1B;
Expand Down Expand Up @@ -131,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;
Expand All @@ -150,6 +153,14 @@ bool rcvdKinematicSettings = false;
bool rcvdMotorSettings = false;
bool encoderStepsChanged = false;
bool zEncoderStepsChanged = false;
volatile bool movementUpdated = false;

// Global variables for misloop tracking
#if misloopDebug > 0
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
Expand Down Expand Up @@ -373,16 +384,10 @@ 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
*/

#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){
Expand All @@ -403,11 +408,21 @@ 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 .0001575*MMPerMin; //value found empirically by running loop until there were not spare cycles
return LOOPINTERVAL*(MMPerMin/(60 * 1000000));
}

void movementUpdate(){
#if misloopDebug > 0
if (movementFail){
Serial.println("Movement loop failed to complete before interrupt.");
movementFail = false;
}
#endif
movementUpdated = true;
}

int cordinatedMove(const float& xEnd, const float& yEnd, const float& zEnd, float MMPerMin){
Expand Down Expand Up @@ -458,37 +473,44 @@ 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 beginingOfLastStep = millis() - delayTime;

while(numberOfStepsTaken < finalNumberOfSteps){
//if enough time has passed to take the next step
if (millis() - beginingOfLastStep > delayTime){
//reset the counter
beginingOfLastStep = millis();

#if misloopDebug > 0
inMovementLoop = true;
#endif
//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();

Expand All @@ -511,6 +533,9 @@ int cordinatedMove(const float& xEnd, const float& yEnd, const float& zEnd, fl
}
}
}
#if misloopDebug > 0
inMovementLoop = false;
#endif

kinematics.inverse(xEnd,yEnd,&aChainLength,&bChainLength);
leftAxis.endMove(aChainLength);
Expand All @@ -536,30 +561,30 @@ 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);
stepSizeMM = stepSizeMM*direction;

long numberOfStepsTaken = 0;

//attach the axis we want to move
axis->attach();

unsigned long beginingOfLastStep = millis() - delayTime;
float whereAxisShouldBeAtThisStep;

float whereAxisShouldBeAtThisStep = startingPos;
#if misloopDebug > 0
inMovementLoop = true;
#endif
while(numberOfStepsTaken < finalNumberOfSteps){
if (millis() - beginingOfLastStep > delayTime){
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());
Expand All @@ -577,6 +602,9 @@ void singleAxisMove(Axis* axis, const float& endPos, const float& MMPerMin){
return;
}
}
#if misloopDebug > 0
inMovementLoop = false;
#endif

axis->endMove(endPos);

Expand Down Expand Up @@ -752,22 +780,18 @@ 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();
rightAxis.attach();

unsigned long beginingOfLastStep = millis() - delayTime;

while(numberOfStepsTaken < abs(finalNumberOfSteps)){
#if misloopDebug > 0
inMovementLoop = true;
#endif

//if enough time has passed to take the next step
if (millis() - beginingOfLastStep > delayTime){

//reset the counter
beginingOfLastStep = millis();
//if last movement was performed start the next one
if (!movementUpdated){

degreeComplete = float(numberOfStepsTaken)/float(finalNumberOfSteps);

Expand All @@ -779,7 +803,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());

Expand All @@ -803,6 +828,9 @@ int arc(const float& X1, const float& Y1, const float& X2, const float& Y2, co
}
}
}
#if misloopDebug > 0
inMovementLoop = false;
#endif

kinematics.inverse(X2,Y2,&aChainLength,&bChainLength);
leftAxis.endMove(aChainLength);
Expand Down Expand Up @@ -916,25 +944,20 @@ 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;
float whereAxisShouldBeAtThisStep;
float whereAxisShouldBeAtThisStep = startingPos;

axis->attach();
// zAxis->attach();

while (numberOfStepsTaken < finalNumberOfSteps) {
if (millis() - beginingOfLastStep > delayTime){
//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());
Expand Down
8 changes: 4 additions & 4 deletions cnc_ctrl_v1/MotorGearboxEncoder.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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)
{
Expand All @@ -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);


}
Expand All @@ -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(10);
_PIDController.SetSampleTime(loopInterval / 1000);
}

void MotorGearboxEncoder::computePID(){
Expand Down
4 changes: 2 additions & 2 deletions cnc_ctrl_v1/MotorGearboxEncoder.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -25,15 +25,15 @@

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();
void write(const float& speed);
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);
Expand Down
Loading

0 comments on commit ecf7e23

Please sign in to comment.