diff --git a/cnc_ctrl_v1/CNC_Functions.h b/cnc_ctrl_v1/CNC_Functions.h index e58c11a2..a866a8f3 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,10 +1251,8 @@ 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(); - error = (-1.0 * reportedSpeed) - speed; + reportedSpeed= axis->motorGearboxEncoder.cachedSpeed(); + error = reportedSpeed - speed; print = current; Serial.println(error); } 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 diff --git a/cnc_ctrl_v1/MotorGearboxEncoder.cpp b/cnc_ctrl_v1/MotorGearboxEncoder.cpp index b5e68ce4..79f3f765 100644 --- a/cnc_ctrl_v1/MotorGearboxEncoder.cpp +++ b/cnc_ctrl_v1/MotorGearboxEncoder.cpp @@ -60,58 +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 - if (abs(_targetSpeed) <= _minimumRPM) _targetSpeed = 0; + _currentSpeed = _computeSpeed(); _PIDController.Compute(); - - /*if(_motorName[0] == 'R'){ - //Serial.print(_currentSpeed); - //Serial.print(" "); - Serial.println(_targetSpeed); - }*/ - - //motor.attach(); + motor.write(_pidOutput); } @@ -162,24 +114,55 @@ 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. */ - 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(); + + float distMoved = currentPosition - _lastPosition; + 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); - //Compute the speed in RPM - float RPM = (_encoderStepsToRPMScaleFactor*distMoved)/float(timeElapsed); + } + else { + float elapsedTime = encoder.elapsedTime(); + + _RPM = 0 ; + if (elapsedTime != 0){ + _RPM = _encoderStepsToRPMScaleFactor / elapsedTime; + } + } + _RPM = _RPM * -1.0; //Store values for next time - _lastTimeStamp = micros(); - _lastPosition = encoder.read(); + _lastTimeStamp = currentMicros; + _lastPosition = currentPosition; - return -1.0*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 e49011a8..cdf64dfd 100644 --- a/cnc_ctrl_v1/MotorGearboxEncoder.h +++ b/cnc_ctrl_v1/MotorGearboxEncoder.h @@ -28,7 +28,7 @@ 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(); void setName(const char& newName); @@ -41,14 +41,16 @@ private: double _targetSpeed; double _currentSpeed; - double _lastPosition; - double _lastTimeStamp; + volatile double _lastPosition; + volatile double _lastTimeStamp; + float _lastDistMoved; + float _RPM; 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; + float _computeSpeed(); }; #endif \ No newline at end of file