diff --git a/sunray/config_example.h b/sunray/config_example.h index fe0bfbdd8..ce26e6781 100644 --- a/sunray/config_example.h +++ b/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) diff --git a/sunray/motor.cpp b/sunray/motor.cpp index 78fd37531..67ecf648e 100644 --- a/sunray/motor.cpp +++ b/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; diff --git a/sunray/src/driver/AmRobotDriver.cpp b/sunray/src/driver/AmRobotDriver.cpp index e6408e726..3dcd7f280 100644 --- a/sunray/src/driver/AmRobotDriver.cpp +++ b/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); +} + diff --git a/sunray/src/driver/AmRobotDriver.h b/sunray/src/driver/AmRobotDriver.h index 7e436a4e9..4ee304dd8 100644 --- a/sunray/src/driver/AmRobotDriver.h +++ b/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;