From cc9e8a481e4c1c948a5bce2c19ec4a19a4184a9d Mon Sep 17 00:00:00 2001 From: Kevin R Keegan Date: Wed, 11 Oct 2017 17:46:08 -0700 Subject: [PATCH 1/7] Add Elapsed Time Feature to Allow for Accurate Slow Speed Detection Elapsedtime contains the number of microseconds that elapsed between when the most recent step was detected and when the step prior to that was detected. --- cnc_ctrl_v1/Encoder.h | 27 ++++++++++++++++++++++++++- 1 file changed, 26 insertions(+), 1 deletion(-) diff --git a/cnc_ctrl_v1/Encoder.h b/cnc_ctrl_v1/Encoder.h index 6745eabb..5349cf0b 100644 --- a/cnc_ctrl_v1/Encoder.h +++ b/cnc_ctrl_v1/Encoder.h @@ -64,6 +64,8 @@ typedef struct { IO_REG_TYPE pin2_bitmask; uint8_t state; int32_t position; + int32_t elapsedTime; + int32_t lastTime; } Encoder_internal_state_t; class Encoder @@ -112,6 +114,17 @@ class Encoder interrupts(); return ret; } + inline int32_t elapsedTime() { + if (interrupts_in_use < 2) { + noInterrupts(); + update(&encoder); + } else { + noInterrupts(); + } + int32_t ret = encoder.elapsedTime; + interrupts(); + return ret; + } inline void write(int32_t p) { noInterrupts(); encoder.position = p; @@ -183,7 +196,11 @@ class Encoder private: static void update(Encoder_internal_state_t *arg) { -#if defined(__AVR__) +#if defined(__AVR_DISABLED__) + // Changed from __AVR__ to force C code below, until someone has + // the chance to write the elapsedTime feature into assembly + // code. The time difference between the two options is + // minimal at best. // The compiler believes this is just 1 line of code, so // it will inline this function into each interrupt // handler. That's a tiny bit faster, but grows the code. @@ -277,15 +294,23 @@ class Encoder switch (state) { case 1: case 7: case 8: case 14: arg->position++; + arg->elapsedTime = (micros() - arg->lastTime); + arg->lastTime = micros(); return; case 2: case 4: case 11: case 13: arg->position--; + arg->elapsedTime = (arg->lastTime - micros()); + arg->lastTime = micros(); return; case 3: case 12: arg->position += 2; + arg->elapsedTime = (micros() - arg->lastTime) >> 1; + arg->lastTime = micros(); return; case 6: case 9: arg->position -= 2; + arg->elapsedTime = (arg->lastTime - micros()) >> 1; + arg->lastTime = micros(); return; } #endif From 3c963150395efcd8fddf75b19d6e988ceffb6b6f Mon Sep 17 00:00:00 2001 From: Kevin R Keegan Date: Wed, 11 Oct 2017 18:05:03 -0700 Subject: [PATCH 2/7] Use Elapsed Time to Determine Speed at Low RPMs At low speeds, less than 3 steps, the effect of quantization is noticable. This uses the new elapsedtime feature to avoid this quantization at these low speeds. At higher speeds, the count of steps becomes more reliable than looking at the elapsedtime. --- cnc_ctrl_v1/MotorGearboxEncoder.cpp | 28 +++++++++++++++++++++------- cnc_ctrl_v1/MotorGearboxEncoder.h | 7 +++---- 2 files changed, 24 insertions(+), 11 deletions(-) diff --git a/cnc_ctrl_v1/MotorGearboxEncoder.cpp b/cnc_ctrl_v1/MotorGearboxEncoder.cpp index b5e68ce4..67513ec5 100644 --- a/cnc_ctrl_v1/MotorGearboxEncoder.cpp +++ b/cnc_ctrl_v1/MotorGearboxEncoder.cpp @@ -101,7 +101,6 @@ void MotorGearboxEncoder::computePID(){ // 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(); @@ -168,16 +167,31 @@ float MotorGearboxEncoder::computeSpeed(){ Returns the motors speed in RPM since the last time this function was called */ - double timeElapsed = micros() - _lastTimeStamp; - float distMoved = encoder.read() - _lastPosition; //_runningAverage(encoder.read() - _lastPosition); //because of quantization noise it helps to average these + float currentPosition = encoder.read(); + float currentMicros = micros(); - //Compute the speed in RPM - float RPM = (_encoderStepsToRPMScaleFactor*distMoved)/float(timeElapsed); + float distMoved = currentPosition - _lastPosition; + float RPM = 0; + if (distMoved > 3 || distMoved < -3){ + + unsigned long timeElapsed = currentMicros - _lastTimeStamp; + //Compute the speed in RPM + RPM = (_encoderStepsToRPMScaleFactor*distMoved)/float(timeElapsed); + + } + else { + float elapsedTime = encoder.elapsedTime(); + + RPM = 0 ; + if (elapsedTime != 0){ + RPM = _encoderStepsToRPMScaleFactor / elapsedTime; + } + } //Store values for next time - _lastTimeStamp = micros(); - _lastPosition = encoder.read(); + _lastTimeStamp = currentMicros; + _lastPosition = currentPosition; return -1.0*RPM; } diff --git a/cnc_ctrl_v1/MotorGearboxEncoder.h b/cnc_ctrl_v1/MotorGearboxEncoder.h index e49011a8..fb8aeb89 100644 --- a/cnc_ctrl_v1/MotorGearboxEncoder.h +++ b/cnc_ctrl_v1/MotorGearboxEncoder.h @@ -41,14 +41,13 @@ private: double _targetSpeed; double _currentSpeed; - double _lastPosition; - double _lastTimeStamp; - char _motorName; + volatile double _lastPosition; + volatile double _lastTimeStamp; + char _motorName; double _pidOutput; PID _PIDController; double _Kp=0, _Ki=0, _Kd=0; float _encoderStepsToRPMScaleFactor = 7364.0; //6*10^7 us per minute divided by 8148 steps per revolution - float _minimumRPM = 0.5; }; #endif \ No newline at end of file From d6f32088ec42fda8a96740cdbaac4cfd6e535897 Mon Sep 17 00:00:00 2001 From: Kevin R Keegan Date: Wed, 11 Oct 2017 18:45:55 -0700 Subject: [PATCH 3/7] Moderately Dampen Quantization Dampens changes of 1 step, but has little effect on more significant changes --- cnc_ctrl_v1/MotorGearboxEncoder.cpp | 9 ++++++++- cnc_ctrl_v1/MotorGearboxEncoder.h | 3 ++- 2 files changed, 10 insertions(+), 2 deletions(-) diff --git a/cnc_ctrl_v1/MotorGearboxEncoder.cpp b/cnc_ctrl_v1/MotorGearboxEncoder.cpp index 67513ec5..a75ff251 100644 --- a/cnc_ctrl_v1/MotorGearboxEncoder.cpp +++ b/cnc_ctrl_v1/MotorGearboxEncoder.cpp @@ -188,12 +188,19 @@ float MotorGearboxEncoder::computeSpeed(){ RPM = _encoderStepsToRPMScaleFactor / elapsedTime; } } + RPM = RPM * -1.0; //Store values for next time _lastTimeStamp = currentMicros; _lastPosition = currentPosition; + _lastRPM = RPM; - return -1.0*RPM; + // This dampens some of the effects of quantization without having + // a big effect on other changes + if (RPM - _lastRPM < -1){ RPM + .5;} + else if (RPM - _lastRPM > 1){RPM - .5;} + + return RPM; } void MotorGearboxEncoder::setName(const char& newName){ diff --git a/cnc_ctrl_v1/MotorGearboxEncoder.h b/cnc_ctrl_v1/MotorGearboxEncoder.h index fb8aeb89..5e79814b 100644 --- a/cnc_ctrl_v1/MotorGearboxEncoder.h +++ b/cnc_ctrl_v1/MotorGearboxEncoder.h @@ -43,7 +43,8 @@ double _currentSpeed; volatile double _lastPosition; volatile double _lastTimeStamp; - char _motorName; + float _lastRPM; + char _motorName; double _pidOutput; PID _PIDController; double _Kp=0, _Ki=0, _Kd=0; From f486bf1cc6fd1ced9ac00589c2a9c5c701f4de62 Mon Sep 17 00:00:00 2001 From: Kevin R Keegan Date: Wed, 11 Oct 2017 19:13:01 -0700 Subject: [PATCH 4/7] Dampening of Quantization on the Steps Made a mistake, dampening should be on the steps not on the RPM --- cnc_ctrl_v1/MotorGearboxEncoder.cpp | 17 +++++++++-------- cnc_ctrl_v1/MotorGearboxEncoder.h | 2 +- 2 files changed, 10 insertions(+), 9 deletions(-) diff --git a/cnc_ctrl_v1/MotorGearboxEncoder.cpp b/cnc_ctrl_v1/MotorGearboxEncoder.cpp index a75ff251..9cc48d42 100644 --- a/cnc_ctrl_v1/MotorGearboxEncoder.cpp +++ b/cnc_ctrl_v1/MotorGearboxEncoder.cpp @@ -173,8 +173,15 @@ float MotorGearboxEncoder::computeSpeed(){ float distMoved = currentPosition - _lastPosition; float RPM = 0; - if (distMoved > 3 || distMoved < -3){ - + if (distMoved > 3 || distMoved < -3){ + + // This dampens some of the effects of quantization without having + // a big effect on other changes + float saveDistMoved = distMoved; + if (distMoved - _lastDistMoved <= -1){ distMoved + .5;} + else if (distMoved - _lastDistMoved >= 1){distMoved - .5;} + _lastDistMoved = saveDistMoved; + unsigned long timeElapsed = currentMicros - _lastTimeStamp; //Compute the speed in RPM RPM = (_encoderStepsToRPMScaleFactor*distMoved)/float(timeElapsed); @@ -193,12 +200,6 @@ float MotorGearboxEncoder::computeSpeed(){ //Store values for next time _lastTimeStamp = currentMicros; _lastPosition = currentPosition; - _lastRPM = RPM; - - // This dampens some of the effects of quantization without having - // a big effect on other changes - if (RPM - _lastRPM < -1){ RPM + .5;} - else if (RPM - _lastRPM > 1){RPM - .5;} return RPM; } diff --git a/cnc_ctrl_v1/MotorGearboxEncoder.h b/cnc_ctrl_v1/MotorGearboxEncoder.h index 5e79814b..0b6b5115 100644 --- a/cnc_ctrl_v1/MotorGearboxEncoder.h +++ b/cnc_ctrl_v1/MotorGearboxEncoder.h @@ -43,7 +43,7 @@ double _currentSpeed; volatile double _lastPosition; volatile double _lastTimeStamp; - float _lastRPM; + float _lastDistMoved; char _motorName; double _pidOutput; PID _PIDController; From 51068854300c84ece57ba15d7e6a87d3097a17db Mon Sep 17 00:00:00 2001 From: Kevin R Keegan Date: Wed, 11 Oct 2017 19:18:11 -0700 Subject: [PATCH 5/7] Use CachedSpeed in PID Velocity for Better Accuracy In order to see what effect the dampening has on the PID input we need to see the actual data from the Compute Speed routine without calling it again, which would actually cause computespeed to be called again. Likely need to make computespeed a private function to prevent causing issues as it should only be called by the PID process --- cnc_ctrl_v1/CNC_Functions.h | 6 +----- cnc_ctrl_v1/MotorGearboxEncoder.cpp | 18 ++++++++++++------ cnc_ctrl_v1/MotorGearboxEncoder.h | 2 ++ 3 files changed, 15 insertions(+), 11 deletions(-) diff --git a/cnc_ctrl_v1/CNC_Functions.h b/cnc_ctrl_v1/CNC_Functions.h index e58c11a2..677f95c7 100644 --- a/cnc_ctrl_v1/CNC_Functions.h +++ b/cnc_ctrl_v1/CNC_Functions.h @@ -1232,9 +1232,7 @@ void PIDTestVelocity(Axis* axis, const float start, const float stop, const floa double startTime; double print = micros(); double current = micros(); - float lastPosition = axis->motorGearboxEncoder.encoder.read(); float error; - float distMoved; float reportedSpeed; float span = stop - start; float speed; @@ -1253,9 +1251,7 @@ void PIDTestVelocity(Axis* axis, const float start, const float stop, const floa while (startTime + 2000000 > current){ if (current - print > 20000){ // Calculate and log error on same frequency as PID interrupt - distMoved = axis->motorGearboxEncoder.encoder.read() - lastPosition; - reportedSpeed= (7364.0*distMoved)/float(current - print); //6*10^7 us per minute, 8148 steps per revolution - lastPosition = axis->motorGearboxEncoder.encoder.read(); + reportedSpeed= axis->motorGearboxEncoder.cachedSpeed(); error = (-1.0 * reportedSpeed) - speed; print = current; Serial.println(error); diff --git a/cnc_ctrl_v1/MotorGearboxEncoder.cpp b/cnc_ctrl_v1/MotorGearboxEncoder.cpp index 9cc48d42..8bb5b583 100644 --- a/cnc_ctrl_v1/MotorGearboxEncoder.cpp +++ b/cnc_ctrl_v1/MotorGearboxEncoder.cpp @@ -172,7 +172,6 @@ float MotorGearboxEncoder::computeSpeed(){ float currentMicros = micros(); float distMoved = currentPosition - _lastPosition; - float RPM = 0; if (distMoved > 3 || distMoved < -3){ // This dampens some of the effects of quantization without having @@ -184,24 +183,31 @@ float MotorGearboxEncoder::computeSpeed(){ unsigned long timeElapsed = currentMicros - _lastTimeStamp; //Compute the speed in RPM - RPM = (_encoderStepsToRPMScaleFactor*distMoved)/float(timeElapsed); + _RPM = (_encoderStepsToRPMScaleFactor*distMoved)/float(timeElapsed); } else { float elapsedTime = encoder.elapsedTime(); - RPM = 0 ; + _RPM = 0 ; if (elapsedTime != 0){ - RPM = _encoderStepsToRPMScaleFactor / elapsedTime; + _RPM = _encoderStepsToRPMScaleFactor / elapsedTime; } } - RPM = RPM * -1.0; + _RPM = _RPM * -1.0; //Store values for next time _lastTimeStamp = currentMicros; _lastPosition = currentPosition; - return RPM; + return _RPM; +} + +float MotorGearboxEncoder::cachedSpeed(){ + /* + Returns the last result of computeSpeed + */ + return _RPM; } void MotorGearboxEncoder::setName(const char& newName){ diff --git a/cnc_ctrl_v1/MotorGearboxEncoder.h b/cnc_ctrl_v1/MotorGearboxEncoder.h index 0b6b5115..1eaa6b2a 100644 --- a/cnc_ctrl_v1/MotorGearboxEncoder.h +++ b/cnc_ctrl_v1/MotorGearboxEncoder.h @@ -29,6 +29,7 @@ Encoder encoder; Motor motor; float computeSpeed(); + float cachedSpeed(); void write(const float& speed); void computePID(); void setName(const char& newName); @@ -44,6 +45,7 @@ volatile double _lastPosition; volatile double _lastTimeStamp; float _lastDistMoved; + float _RPM; char _motorName; double _pidOutput; PID _PIDController; From 7c406dd96c3a87491db6ee4a9696e196f8acc6bb Mon Sep 17 00:00:00 2001 From: Kevin R Keegan Date: Wed, 11 Oct 2017 19:23:07 -0700 Subject: [PATCH 6/7] Make ComputeSpeed a Private Function It shouldn't be called outside PID loop otherwise changing the reference frame of elapsed time. --- cnc_ctrl_v1/MotorGearboxEncoder.cpp | 55 +++-------------------------- cnc_ctrl_v1/MotorGearboxEncoder.h | 2 +- 2 files changed, 6 insertions(+), 51 deletions(-) diff --git a/cnc_ctrl_v1/MotorGearboxEncoder.cpp b/cnc_ctrl_v1/MotorGearboxEncoder.cpp index 8bb5b583..79f3f765 100644 --- a/cnc_ctrl_v1/MotorGearboxEncoder.cpp +++ b/cnc_ctrl_v1/MotorGearboxEncoder.cpp @@ -60,57 +60,10 @@ void MotorGearboxEncoder::computePID(){ /* Recompute the speed control PID loop and command the motor to move. */ - _currentSpeed = computeSpeed(); - - /*if (millis() < 8000){ - _targetSpeed = 0; - } - else if (millis() < 12000){ - _targetSpeed = 10; - } - else if (millis() < 18000){ - _targetSpeed = 0; - } - else if(millis() < 24000){ - _targetSpeed = float((millis() - 18000))/400.0; - } - else if (millis() < 32000){ - _targetSpeed = 0; - } - else if (millis() < 40000){ - _targetSpeed = 10; - } - else if (millis() < 48000){ - _targetSpeed = 0; - } - else if (millis() < 56000){ - _targetSpeed = -10; - } - else if (millis() < 64000){ - _targetSpeed = 0; - } - else if (millis() < 72000){ - _targetSpeed = 10; - } - else if (millis() < 80000){ - _targetSpeed = 0; - } - else{ - _targetSpeed = 0; - }*/ - - // Between these speeds the motor is incapable of turning and it only - // causes the Iterm in the PID calculation to wind up + _currentSpeed = _computeSpeed(); _PIDController.Compute(); - - /*if(_motorName[0] == 'R'){ - //Serial.print(_currentSpeed); - //Serial.print(" "); - Serial.println(_targetSpeed); - }*/ - - //motor.attach(); + motor.write(_pidOutput); } @@ -161,10 +114,12 @@ void MotorGearboxEncoder::setEncoderResolution(float resolution){ } -float MotorGearboxEncoder::computeSpeed(){ +float MotorGearboxEncoder::_computeSpeed(){ /* Returns the motors speed in RPM since the last time this function was called + should only be called by the PID process otherwise we are calculating the + distance moved over a varying amount of time. */ diff --git a/cnc_ctrl_v1/MotorGearboxEncoder.h b/cnc_ctrl_v1/MotorGearboxEncoder.h index 1eaa6b2a..cdf64dfd 100644 --- a/cnc_ctrl_v1/MotorGearboxEncoder.h +++ b/cnc_ctrl_v1/MotorGearboxEncoder.h @@ -28,7 +28,6 @@ MotorGearboxEncoder(const int& pwmPin, const int& directionPin1, const int& directionPin2, const int& encoderPin1, const int& encoderPin2); Encoder encoder; Motor motor; - float computeSpeed(); float cachedSpeed(); void write(const float& speed); void computePID(); @@ -51,6 +50,7 @@ PID _PIDController; double _Kp=0, _Ki=0, _Kd=0; float _encoderStepsToRPMScaleFactor = 7364.0; //6*10^7 us per minute divided by 8148 steps per revolution + float _computeSpeed(); }; #endif \ No newline at end of file From bcfafcab21e45168a89e26c20995fa80ed55cf12 Mon Sep 17 00:00:00 2001 From: Kevin R Keegan Date: Wed, 11 Oct 2017 19:39:24 -0700 Subject: [PATCH 7/7] Cached Speed RPM is in the Correct Direction No need to flip directions anymore --- cnc_ctrl_v1/CNC_Functions.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/cnc_ctrl_v1/CNC_Functions.h b/cnc_ctrl_v1/CNC_Functions.h index 677f95c7..a866a8f3 100644 --- a/cnc_ctrl_v1/CNC_Functions.h +++ b/cnc_ctrl_v1/CNC_Functions.h @@ -1252,7 +1252,7 @@ void PIDTestVelocity(Axis* axis, const float start, const float stop, const floa if (current - print > 20000){ // Calculate and log error on same frequency as PID interrupt reportedSpeed= axis->motorGearboxEncoder.cachedSpeed(); - error = (-1.0 * reportedSpeed) - speed; + error = reportedSpeed - speed; print = current; Serial.println(error); }