diff --git a/cnc_ctrl_v1/Axis.cpp b/cnc_ctrl_v1/Axis.cpp index dcfb5179..5bdeb1c5 100644 --- a/cnc_ctrl_v1/Axis.cpp +++ b/cnc_ctrl_v1/Axis.cpp @@ -172,6 +172,9 @@ void Axis::changeEncoderResolution(const int& newResolution){ */ _encoderSteps = newResolution; + //push to the gearbox for calculating RPM + motorGearboxEncoder.setEncoderResolution(newResolution); + } int Axis::detach(){ diff --git a/cnc_ctrl_v1/MotorGearboxEncoder.cpp b/cnc_ctrl_v1/MotorGearboxEncoder.cpp index 729af92a..019993c6 100644 --- a/cnc_ctrl_v1/MotorGearboxEncoder.cpp +++ b/cnc_ctrl_v1/MotorGearboxEncoder.cpp @@ -151,6 +151,17 @@ void MotorGearboxEncoder::setPIDAggressiveness(float aggressiveness){ } +void MotorGearboxEncoder::setEncoderResolution(float resolution){ + /* + + Change the encoder resolution + + */ + + _encoderStepsToRPMScaleFactor = 60000000.0/resolution; //6*10^7 us per minute divided by 8148 steps per revolution + +} + float MotorGearboxEncoder::computeSpeed(){ /* @@ -162,7 +173,7 @@ float MotorGearboxEncoder::computeSpeed(){ float distMoved = _runningAverage(encoder.read() - _lastPosition); //because of quantization noise it helps to average these //Compute the speed in RPM - float RPM = (7364.0*distMoved)/float(timeElapsed); //6*10^7 us per minute, 8148 steps per revolution + float RPM = (_encoderStepsToRPMScaleFactor*distMoved)/float(timeElapsed); //Store values for next time _lastTimeStamp = micros(); diff --git a/cnc_ctrl_v1/MotorGearboxEncoder.h b/cnc_ctrl_v1/MotorGearboxEncoder.h index 5409d901..885b7dc1 100644 --- a/cnc_ctrl_v1/MotorGearboxEncoder.h +++ b/cnc_ctrl_v1/MotorGearboxEncoder.h @@ -35,6 +35,7 @@ void initializePID(); void setPIDAggressiveness(float aggressiveness); void setPIDValues(float KpV, float KiV, float KdV); + void setEncoderResolution(float resolution); private: double _targetSpeed; double _currentSpeed; @@ -46,6 +47,7 @@ PID _posPIDController; PID _negPIDController; double _Kp=0, _Ki=0, _Kd=0; + float _encoderStepsToRPMScaleFactor = 7364.0; //6*10^7 us per minute divided by 8148 steps per revolution int _oldValue1; int _oldValue2; int _oldValue3;