Skip to content

Commit

Permalink
Merge pull request #314 from krkeegan/Movement_Fixes
Browse files Browse the repository at this point in the history 
Improvements to the Movement Calculations and PID Controllers
  • Loading branch information
@BarbourSmith
BarbourSmith authored Sep 29, 2017
2 parents dfee525 + ad22f07 commit ee9bfa9
Show file tree
Hide file tree
Showing 5 changed files with 94 additions and 140 deletions.
41 changes: 7 additions & 34 deletions cnc_ctrl_v1/Axis.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -34,7 +34,6 @@ motorGearboxEncoder(pwmPin, directionPin1, directionPin2, encoderPin1, encoderPi
_pidController.setup(&_pidInput, &_pidOutput, &_pidSetpoint, 0, 0, 0, P_ON_E, REVERSE);

//initialize variables
_direction = FORWARD;
_axisName = axisName;
_eepromAdr = eepromAdr;

Expand All @@ -61,13 +60,18 @@ void Axis::loadPositionFromMemory(){

void Axis::initializePID(){
_pidController.SetMode(AUTOMATIC);
_pidController.SetOutputLimits(-17, 17);
_pidController.SetOutputLimits(-20, 20);
_pidController.SetSampleTime(10);
}

void Axis::write(const float& targetPosition){

_pidSetpoint = targetPosition/_mmPerRotation;
// Ensure that _pidSetpoint is equal to whole number of encoder steps
float steps = (targetPosition/_mmPerRotation) * _encoderSteps;
steps = steps * 2;
steps = round(steps);
steps = steps /2;
_pidSetpoint = steps/_encoderSteps;
return;
}

Expand Down Expand Up @@ -103,10 +107,6 @@ void Axis::computePID(){
return;
}

if (_detectDirectionChange(_pidSetpoint)){ //this determines if the axis has changed direction of movement and flushes the accumulator in the PID if it has
_pidController.FlipIntegrator();
}

_pidInput = motorGearboxEncoder.encoder.read()/_encoderSteps;

if (_pidController.Compute()){
Expand Down Expand Up @@ -255,33 +255,6 @@ void Axis::wipeEEPROM(){
Serial.println(F(" EEPROM erased"));
}

int Axis::_detectDirectionChange(const float& _pidSetpoint){

float difference = _pidSetpoint - _oldSetpoint;

if(difference == 0){
return 0;
}

int direction;
if(difference > 0){
direction = 1;
}
else{
direction = 0;
}

int retVal = 0;
if(direction != _oldDir){
retVal = 1;
}

_oldSetpoint = _pidSetpoint;
_oldDir = direction;

return retVal;
}

void Axis::test(){
/*
Test the axis by directly commanding the motor and observing if the encoder moves
Expand Down
3 changes: 0 additions & 3 deletions cnc_ctrl_v1/Axis.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -57,8 +57,6 @@
int _PWMread(int pin);
void _writeFloat(const unsigned int& addr, const float& x);
float _readFloat(const unsigned int& addr);
int _detectDirectionChange(const float& _pidSetpoint);
int _direction;
int _encoderPin;
float _axisTarget;
int _currentAngle;
Expand All @@ -73,7 +71,6 @@
float _mmPerRotation = 1;
float _encoderSteps = 100;
float _oldSetpoint;
float _oldDir;
bool _disableAxisForTesting = false;
String _axisName;
};
Expand Down
150 changes: 72 additions & 78 deletions cnc_ctrl_v1/CNC_Functions.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -432,46 +432,50 @@ int cordinatedMove(const float& xEnd, const float& yEnd, const float& MMPerMin
float aChainLength;
float bChainLength;
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();

//find the target point for this step
float whereXShouldBeAtThisStep = xStartingLocation + (numberOfStepsTaken*xStepSize);
float whereYShouldBeAtThisStep = yStartingLocation + (numberOfStepsTaken*yStepSize);

//find the target point for this step
float whereXShouldBeAtThisStep = xStartingLocation + (numberOfStepsTaken*xStepSize);
float whereYShouldBeAtThisStep = yStartingLocation + (numberOfStepsTaken*yStepSize);

//find the chain lengths for this step
kinematics.inverse(whereXShouldBeAtThisStep,whereYShouldBeAtThisStep,&aChainLength,&bChainLength);

//write to each axis
leftAxis.write(aChainLength);
rightAxis.write(bChainLength);

//increment the number of steps taken
numberOfStepsTaken++;

//update position on display
returnPoz(whereXShouldBeAtThisStep, whereYShouldBeAtThisStep, zAxis.read());

//check for new serial commands
readSerialCommands();

//check for a STOP command
if(checkForStopCommand()){

//set the axis positions to save
//find the chain lengths for this step
kinematics.inverse(whereXShouldBeAtThisStep,whereYShouldBeAtThisStep,&aChainLength,&bChainLength);
leftAxis.endMove(aChainLength);
rightAxis.endMove(bChainLength);

//make sure the positions are displayed correctly after stop
xTarget = whereXShouldBeAtThisStep;
yTarget = whereYShouldBeAtThisStep;
//write to each axis
leftAxis.write(aChainLength);
rightAxis.write(bChainLength);

return 1;
//increment the number of steps taken
numberOfStepsTaken++;

//update position on display
returnPoz(whereXShouldBeAtThisStep, whereYShouldBeAtThisStep, zAxis.read());

//check for new serial commands
readSerialCommands();

//check for a STOP command
if(checkForStopCommand()){

//set the axis positions to save
kinematics.inverse(whereXShouldBeAtThisStep,whereYShouldBeAtThisStep,&aChainLength,&bChainLength);
leftAxis.endMove(aChainLength);
rightAxis.endMove(bChainLength);

//make sure the positions are displayed correctly after stop
xTarget = whereXShouldBeAtThisStep;
yTarget = whereYShouldBeAtThisStep;

return 1;
}
}

//wait for the step to be completed
delay(delayTime);
}

kinematics.inverse(xEnd,yEnd,&aChainLength,&bChainLength);
Expand Down Expand Up @@ -508,22 +512,24 @@ void singleAxisMove(Axis* axis, const float& endPos, const float& MMPerMin){
//attach the axis we want to move
axis->attach();

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

while(numberOfStepsTaken < finalNumberOfSteps){

//find the target point for this step
float whereAxisShouldBeAtThisStep = startingPos + numberOfStepsTaken*stepSizeMM*direction;

//write to axis
axis->write(whereAxisShouldBeAtThisStep);

//update position on display
returnPoz(xTarget, yTarget, zAxis.read());

//calculate the correct delay between steps to set feedrate
delay(delayTime);

//increment the number of steps taken
numberOfStepsTaken++;
if (millis() - beginingOfLastStep > delayTime){
beginingOfLastStep = millis();
//find the target point for this step
whereAxisShouldBeAtThisStep = startingPos + numberOfStepsTaken*stepSizeMM*direction;

//write to axis
axis->write(whereAxisShouldBeAtThisStep);

//update position on display
returnPoz(xTarget, yTarget, zAxis.read());

//increment the number of steps taken
numberOfStepsTaken++;
}

//check for new serial commands
readSerialCommands();
Expand Down Expand Up @@ -717,7 +723,7 @@ int arc(const float& X1, const float& Y1, const float& X2, const float& Y2, co
leftAxis.attach();
rightAxis.attach();

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

while(numberOfStepsTaken < abs(finalNumberOfSteps)){

Expand Down Expand Up @@ -883,30 +889,29 @@ void G38(const String& readString) {
float delayTime = calculateDelay(stepSizeMM, MMPerMin);

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

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

while (numberOfStepsTaken < finalNumberOfSteps) {
if (millis() - beginingOfLastStep > delayTime){
//reset the counter
beginingOfLastStep = millis();

//reset the counter
beginingOfLastStep = millis();

//find the target point for this step
float whereAxisShouldBeAtThisStep = startingPos + numberOfStepsTaken * stepSizeMM * direction;

//write to each axis
axis->write(whereAxisShouldBeAtThisStep);
//find the target point for this step
whereAxisShouldBeAtThisStep = startingPos + numberOfStepsTaken * stepSizeMM * direction;

//update position on display
returnPoz(xTarget, yTarget, zAxis.read());
//write to each axis
axis->write(whereAxisShouldBeAtThisStep);

//calculate the correct delay between steps to set feedrate
delay(delayTime);
//update position on display
returnPoz(xTarget, yTarget, zAxis.read());

//increment the number of steps taken
numberOfStepsTaken++;
//increment the number of steps taken
numberOfStepsTaken++;
}

//check for new serial commands
readSerialCommands();
Expand Down Expand Up @@ -958,8 +963,7 @@ void calibrateChainLengths(){

//measure out the left chain
Serial.println(F("Measuring out left chain"));
leftAxis.setPIDAggressiveness(.1);
singleAxisMove(&leftAxis, ORIGINCHAINLEN, 500);
singleAxisMove(&leftAxis, ORIGINCHAINLEN, 800);

Serial.print(leftAxis.read());
Serial.println(F("mm"));
Expand All @@ -968,17 +972,13 @@ void calibrateChainLengths(){

//measure out the right chain
Serial.println(F("Measuring out right chain"));
rightAxis.setPIDAggressiveness(.1);
singleAxisMove(&rightAxis, ORIGINCHAINLEN, 500);
singleAxisMove(&rightAxis, ORIGINCHAINLEN, 800);

Serial.print(rightAxis.read());
Serial.println(F("mm"));

kinematics.forward(leftAxis.read(), rightAxis.read(), &xTarget, &yTarget);


leftAxis.setPIDAggressiveness(1);
rightAxis.setPIDAggressiveness(1);
}

void setInchesToMillimetersConversion(float newConversionFactor){
Expand Down Expand Up @@ -1397,10 +1397,7 @@ void executeBcodeLine(const String& gcodeLine){
//Directly command each axis to move to a given distance
float lDist = extractGcodeValue(gcodeLine, 'L', 0);
float rDist = extractGcodeValue(gcodeLine, 'R', 0);
float speed = extractGcodeValue(gcodeLine, 'F', 500);

leftAxis.setPIDAggressiveness(.1);
rightAxis.setPIDAggressiveness(.1);
float speed = extractGcodeValue(gcodeLine, 'F', 800);

if(useRelativeUnits){
if(abs(lDist) > 0){
Expand All @@ -1415,9 +1412,6 @@ void executeBcodeLine(const String& gcodeLine){
singleAxisMove(&rightAxis, rDist, speed);
}

leftAxis.setPIDAggressiveness(1);
rightAxis.setPIDAggressiveness(1);

return;
}

Expand Down
36 changes: 13 additions & 23 deletions cnc_ctrl_v1/MotorGearboxEncoder.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -34,8 +34,7 @@ encoder(encoderPin1,encoderPin2)
motor.write(0);

//initialize the PID
_posPIDController.setup(&_currentSpeed, &_pidOutput, &_targetSpeed, _Kp, _Ki, _Kd, P_ON_E, DIRECT);
_negPIDController.setup(&_currentSpeed, &_pidOutput, &_targetSpeed, _Kp, _Ki, _Kd, P_ON_E, DIRECT);
_PIDController.setup(&_currentSpeed, &_pidOutput, &_targetSpeed, _Kp, _Ki, _Kd, P_ON_E, DIRECT);
initializePID();


Expand All @@ -52,14 +51,9 @@ void MotorGearboxEncoder::write(const float& speed){

void MotorGearboxEncoder::initializePID(){
//setup positive PID controller
_posPIDController.SetMode(AUTOMATIC);
_posPIDController.SetOutputLimits(-255, 255);
_posPIDController.SetSampleTime(10);

//setup negative PID controller
_negPIDController.SetMode(AUTOMATIC);
_negPIDController.SetOutputLimits(-255, 255);
_negPIDController.SetSampleTime(10);
_PIDController.SetMode(AUTOMATIC);
_PIDController.SetOutputLimits(-255, 255);
_PIDController.SetSampleTime(10);
}

void MotorGearboxEncoder::computePID(){
Expand Down Expand Up @@ -105,14 +99,12 @@ void MotorGearboxEncoder::computePID(){
_targetSpeed = 0;
}*/


if(_targetSpeed > 0){
_posPIDController.Compute();
}
else{
_negPIDController.Compute();
}

// Between these speeds the motor is incapable of turning and it only
// causes the Iterm in the PID calculation to wind up
if (abs(_targetSpeed) <= _minimumRPM) _targetSpeed = 0;

_PIDController.Compute();

/*if(_motorName[0] == 'R'){
//Serial.print(_currentSpeed);
//Serial.print(" ");
Expand All @@ -134,8 +126,7 @@ void MotorGearboxEncoder::setPIDValues(float KpV, float KiV, float KdV){
_Ki = KiV;
_Kd = KdV;

_posPIDController.SetTunings(_Kp, _Ki, _Kd, P_ON_E);
_negPIDController.SetTunings(_Kp, _Ki, _Kd, P_ON_E);
_PIDController.SetTunings(_Kp, _Ki, _Kd, P_ON_E);
}

String MotorGearboxEncoder::getPIDString(){
Expand All @@ -156,8 +147,7 @@ void MotorGearboxEncoder::setPIDAggressiveness(float aggressiveness){
*/

_posPIDController.SetTunings(aggressiveness*_Kp, _Ki, _Kd, P_ON_E);
_negPIDController.SetTunings(aggressiveness*_Kp, _Ki, _Kd, P_ON_E);
_PIDController.SetTunings(aggressiveness*_Kp, _Ki, _Kd, P_ON_E);

}

Expand All @@ -180,7 +170,7 @@ float MotorGearboxEncoder::computeSpeed(){
*/
double timeElapsed = micros() - _lastTimeStamp;

float distMoved = _runningAverage(encoder.read() - _lastPosition); //because of quantization noise it helps to average these
float distMoved = encoder.read() - _lastPosition; //_runningAverage(encoder.read() - _lastPosition); //because of quantization noise it helps to average these

//Compute the speed in RPM
float RPM = (_encoderStepsToRPMScaleFactor*distMoved)/float(timeElapsed);
Expand Down
Loading

0 comments on commit ee9bfa9

Please sign in to comment.