Odometry: RPM estimation improvements

sunray/config_example.h

@@ -130,6 +130,8 @@ Also, you may choose the serial port below for serial monitor output (CONSOLE).
 
 // #define TICKS_PER_REVOLUTION  304     // odometry ticks per wheel revolution (RM18)
 
+#define RESPONSIVE_RPM false // use tick time for odometry RPM calculations
+
 
 // ----- gear motors --------------------------------------------------
 // for brushless motors, study the sections (drivers, adapter, protection etc.) in the Wiki (https://wiki.ardumower.de/index.php?title=DIY_Brushless_Driver_Board)

sunray/motor.cpp

@@ -296,12 +296,33 @@ void Motor::run() {
   float deltaControlTimeSec =  ((float)(currTime - lastControlTime)) / 1000.0;
   lastControlTime = currTime;
 
+#if RESPONSIVE_RPM
+  unsigned long timeLeft;
+  unsigned long timeRight;
+  unsigned long timeMow;
+  motorDriver.getMotorTickTime(timeLeft, timeRight, timeMow);  
+
+  float fTimeLeft = timeLeft / 1000000.0;
+  float fTimeRight = timeRight / 1000000.0;
+  float fTimeMow = timeMow /1000000.0;
+
+  if (motorLeftPWMCurr < 0) fTimeLeft *= -1;
+  if (motorRightPWMCurr < 0) fTimeRight *= -1;
+  if (motorMowPWMCurr < 0) fTimeMow *= -1;
+
+  // calculat speed via tick time
+  motorLeftRpmCurr = (1.0 / (fTimeLeft * (float)ticksPerRevolution)) * 60.0 * (float)(motorLeftPWMCurr != 0);
+  motorRightRpmCurr = (1.0 / (fTimeRight * (float)ticksPerRevolution)) * 60.0 * (float)(motorRightPWMCurr != 0);
+  motorMowRpmCurr = (1.0 / (fTimeMow * 6.0)) * 60.0 * (float)(motorMowPWMCurr != 0);
+#else
   // calculate speed via tick count
   // 2000 ticksPerRevolution: @ 30 rpm  => 0.5 rps => 1000 ticksPerSec
   // 20 ticksPerRevolution: @ 30 rpm => 0.5 rps => 10 ticksPerSec
   motorLeftRpmCurr = 60.0 * ( ((float)ticksLeft) / ((float)ticksPerRevolution) ) / deltaControlTimeSec;
   motorRightRpmCurr = 60.0 * ( ((float)ticksRight) / ((float)ticksPerRevolution) ) / deltaControlTimeSec;
   motorMowRpmCurr = 60.0 * ( ((float)ticksMow) / ((float)6.0) ) / deltaControlTimeSec; // assuming 6 ticks per revolution
+#endif
+
   float lp = 0.9; // 0.995
   motorLeftRpmCurrLP = lp * motorLeftRpmCurrLP + (1.0-lp) * motorLeftRpmCurr;
   motorRightRpmCurrLP = lp * motorRightRpmCurrLP + (1.0-lp) * motorRightRpmCurr;

sunray/src/driver/AmRobotDriver.cpp

@@ -21,8 +21,15 @@
 #endif
 
 
-#define SUPER_SPIKE_ELIMINATOR 1  // advanced spike elimination  (experimental, comment out to disable)
+//#define SUPER_SPIKE_ELIMINATOR 1  // advanced spike elimination  (experimental, comment out to disable)
 
+volatile unsigned long motorLeftTickTime = 0;
+volatile unsigned long motorRightTickTime = 0;
+volatile unsigned long motorMowTickTime = 0;
+
+volatile unsigned long motorLeftThen = 0;
+volatile unsigned long motorRightThen = 0;
+volatile unsigned long motorMowThen = 0;
 
 volatile int odomTicksLeft  = 0;
 volatile int odomTicksRight = 0;
@@ -32,10 +39,6 @@ volatile unsigned long motorLeftTicksTimeout = 0;
 volatile unsigned long motorRightTicksTimeout = 0;
 volatile unsigned long motorMowTicksTimeout = 0;
 
-volatile unsigned long motorLeftTransitionTime = 0;
-volatile unsigned long motorRightTransitionTime = 0;
-volatile unsigned long motorMowTransitionTime = 0;
-
 volatile float motorLeftDurationMax = 0;
 volatile float motorRightDurationMax = 0;
 volatile float motorMowDurationMax = 0;
@@ -101,47 +104,50 @@ float AmRobotDriver::getCpuTemperature(){
 // odometry signal change interrupt
 
 void OdometryMowISR(){			  
+  unsigned long now = micros();
   if (digitalRead(pinMotorMowRpm) == LOW) return;
-  if (millis() < motorMowTicksTimeout) return; // eliminate spikes  
+  if (now - motorMowThen < motorMowTicksTimeout) return; // eliminate spikes 
+  motorMowTickTime = now - motorMowThen;
+  motorMowThen = now; 
   #ifdef SUPER_SPIKE_ELIMINATOR
-    unsigned long duration = millis() - motorMowTransitionTime;
-    if (duration > 5) duration = 0;
-    motorMowTransitionTime = millis();
-    motorMowDurationMax = 0.7 * max(motorMowDurationMax, ((float)duration));
-    motorMowTicksTimeout = millis() + motorMowDurationMax;
+    unsigned long duration = motorMowTickTime;
+    if (duration > 5000) duration = 0;
+    motorMowTicksTimeout = 0.7 * max(motorMowTicksTimeout, ((float)duration));
   #else
-    motorMowTicksTimeout = millis() + 1;
+    motorMowTicksTimeout = 1000;
   #endif
-  odomTicksMow++;    
+  odomTicksMow++;
 }
 
 
 void OdometryLeftISR(){			  
+  unsigned long now = micros();
   if (digitalRead(pinOdometryLeft) == LOW) return;
-  if (millis() < motorLeftTicksTimeout) return; // eliminate spikes  
+  if (now - motorLeftThen < motorLeftTicksTimeout) return; // eliminate spikes 
+  motorLeftTickTime = now - motorLeftThen;
+  motorLeftThen = now; 
   #ifdef SUPER_SPIKE_ELIMINATOR
-    unsigned long duration = millis() - motorLeftTransitionTime;
-    if (duration > 5) duration = 0;
-    motorLeftTransitionTime = millis();
-    motorLeftDurationMax = 0.7 * max(motorLeftDurationMax, ((float)duration));
-    motorLeftTicksTimeout = millis() + motorLeftDurationMax;
+    unsigned long duration = motorLeftTickTime;
+    if (duration > 5000) duration = 0;
+    motorLeftTicksTimeout = 0.7 * max(motorLeftTicksTimeout, ((float)duration));
   #else
-    motorLeftTicksTimeout = millis() + 1;
+    motorLeftTicksTimeout = 1000;
   #endif
   odomTicksLeft++;    
 }
 
 void OdometryRightISR(){			
+  unsigned long now = micros();
   if (digitalRead(pinOdometryRight) == LOW) return;  
-  if (millis() < motorRightTicksTimeout) return; // eliminate spikes
+  if (now - motorRightThen < motorRightTicksTimeout) return; // eliminate spikes
+  motorRightTickTime = now - motorRightThen;
+  motorRightThen = now;
   #ifdef SUPER_SPIKE_ELIMINATOR
-    unsigned long duration = millis() - motorRightTransitionTime;
-    if (duration > 5) duration = 0;  
-    motorRightTransitionTime = millis();
-    motorRightDurationMax = 0.7 * max(motorRightDurationMax, ((float)duration));  
-    motorRightTicksTimeout = millis() + motorRightDurationMax;
+    unsigned long duration = motorRightTickTime;
+    if (duration > 5000) duration = 0;  
+    motorRightTicksTimeout = 0.7 * max(motorRightTicksTimeout, ((float)duration));  
   #else
-    motorRightTicksTimeout = millis() + 1;
+    motorRightTicksTimeout = 1000;
   #endif
   odomTicksRight++;        
 
@@ -554,6 +560,13 @@ void AmMotorDriver::getMotorEncoderTicks(int &leftTicks, int &rightTicks, int &m
   odomTicksLeft = odomTicksRight = odomTicksMow = 0;
 }    
 
+void AmMotorDriver::getMotorTickTime(unsigned long &leftTime, unsigned long &rightTime, unsigned long &mowTime){
+  unsigned long now = micros();
+  leftTime = max(motorLeftTickTime, now - motorLeftThen);
+  rightTime = max(motorRightTickTime, now - motorRightThen);
+  mowTime = max(motorMowTickTime, now - motorMowThen);
+}     
+
 
 
 

sunray/src/driver/AmRobotDriver.h

@@ -72,6 +72,7 @@ class AmMotorDriver: public MotorDriver {
     void resetMotorFaults()  override;
     void getMotorCurrent(float &leftCurrent, float &rightCurrent, float &mowCurrent) override;
     void getMotorEncoderTicks(int &leftTicks, int &rightTicks, int &mowTicks) override;
+    void getMotorTickTime(unsigned long &leftTime, unsigned long &rightTime, unsigned long &mowTime);
   protected:
     int lastLeftPwm;
     int lastRightPwm;