Merge pull request #320 from krkeegan/velo_calc_improvements

Fix Error in Quantization Buffer; Allow RPM to Approach 0
MaslowCNC · Oct 17, 2017 · 99dabad · 99dabad
2 parents ecf7e23 + f328de5
commit 99dabad
Show file tree

Hide file tree

Showing 2 changed files with 23 additions and 2 deletions.
diff --git a/cnc_ctrl_v1/Encoder.h b/cnc_ctrl_v1/Encoder.h
@@ -125,6 +125,17 @@ class Encoder
 		interrupts();
 		return ret;
 	}
+	inline int32_t lastStepTime() {
+		if (interrupts_in_use < 2) {
+			noInterrupts();
+			update(&encoder);
+		} else {
+			noInterrupts();
+		}
+		int32_t ret = encoder.lastTime;
+		interrupts();
+		return ret;
+	}
 	inline void write(int32_t p) {
 		noInterrupts();
 		encoder.position = p;

diff --git a/cnc_ctrl_v1/MotorGearboxEncoder.cpp b/cnc_ctrl_v1/MotorGearboxEncoder.cpp
@@ -132,8 +132,8 @@ float MotorGearboxEncoder::_computeSpeed(){
         // 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;}
+        if (distMoved - _lastDistMoved <= -1){ distMoved += .5;}
+        else if (distMoved - _lastDistMoved >= 1){distMoved -= .5;}
         _lastDistMoved = saveDistMoved;
 
         unsigned long timeElapsed =  currentMicros - _lastTimeStamp;
@@ -143,6 +143,16 @@ float MotorGearboxEncoder::_computeSpeed(){
     }
     else {
         float elapsedTime = encoder.elapsedTime();
+        float lastTime = micros() - encoder.lastStepTime();  // no direction associated with this
+        if (lastTime > abs(elapsedTime)) {
+            // This allows the RPM to approach 0
+            if (elapsedTime < 0){
+                elapsedTime = -lastTime;
+            }
+            else {
+                elapsedTime = lastTime;
+            }
+        };
 
         _RPM = 0 ;
         if (elapsedTime != 0){