pwm smoothing fix for non-odometry

Ardumower · May 1, 2015 · 19596e0 · 19596e0
1 parent d8d248b
commit 19596e0
Show file tree

Hide file tree

Showing 3 changed files with 12 additions and 11 deletions.
diff --git a/ardumower/mower.cpp b/ardumower/mower.cpp
@@ -126,7 +126,7 @@ Mower robot;
 Mower::Mower(){
   name = "Ardumower";
   // ------- wheel motors -----------------------------
-  motorAccel       = 0.002;  // motor wheel acceleration - only functional when odometry is not in use (warning: do not set too high)
+  motorAccel       = 500;  // motor wheel acceleration - only functional when odometry is not in use (warning: do not set too low)
   motorSpeedMaxRpm       = 33;   // motor wheel max RPM
   motorSpeedMaxPwm    = 255;  // motor wheel max Pwm  (8-bit PWM=255, 10-bit PWM=1023)
   motorPowerMax     = 50;    // motor wheel max power (Watt)

diff --git a/ardumower/pfod.cpp b/ardumower/pfod.cpp
@@ -281,7 +281,7 @@ void RemoteControl::sendMotorMenu(boolean update){
   Bluetooth.print(robot->motorRightPWMCurr);   
   sendSlider("a06", F("Speed max in rpm"), robot->motorSpeedMaxRpm, "", 1, 100);    
   sendSlider("a15", F("Speed max in pwm"), robot->motorSpeedMaxPwm, "", 1, 255);    
-  sendSlider("a11", F("Accel"), robot->motorAccel, "", 0.01, 0.1);  
+  sendSlider("a11", F("Accel"), robot->motorAccel, "", 1, 1000);  
   sendSlider("a18", F("Power ignore time"), robot->motorPowerIgnoreTime, "", 1, 8000);     
   sendSlider("a07", F("Roll time max"), robot->motorRollTimeMax, "", 1, 8000);     
   sendSlider("a08", F("Reverse time"), robot->motorReverseTime, "", 1, 8000);     
@@ -326,7 +326,7 @@ void RemoteControl::processMotorMenu(String pfodCmd){
     else if (pfodCmd.startsWith("a07")) processSlider(pfodCmd, robot->motorRollTimeMax, 1); 
     else if (pfodCmd.startsWith("a08")) processSlider(pfodCmd, robot->motorReverseTime, 1);
     else if (pfodCmd.startsWith("a09")) processSlider(pfodCmd, robot->motorForwTimeMax, 10);
-    else if (pfodCmd.startsWith("a11")) processSlider(pfodCmd, robot->motorAccel, 0.01);    
+    else if (pfodCmd.startsWith("a11")) processSlider(pfodCmd, robot->motorAccel, 1);    
     else if (pfodCmd.startsWith("a12")) processSlider(pfodCmd, robot->motorBiDirSpeedRatio1, 0.01);    
     else if (pfodCmd.startsWith("a13")) processSlider(pfodCmd, robot->motorBiDirSpeedRatio2, 0.01);    
     else if (pfodCmd.startsWith("a14")) processPIDSlider(pfodCmd, "a14", robot->motorLeftPID, 0.01, 3.0);

diff --git a/ardumower/robot.cpp b/ardumower/robot.cpp
@@ -25,7 +25,7 @@
 
 #include "robot.h"
 
-#define MAGIC 38
+#define MAGIC 39
 
 char* stateNames[]={"OFF ", "RC  ", "FORW", "ROLL", "REV ", "CIRC", "ERR ", "PFND", "PTRK", "PROL", "PREV", "STAT", "CHARG", "STCHK",
   "CREV", "CROL", "CFOR", "MANU", "ROLW" };
@@ -525,11 +525,12 @@ void Robot::setMotorPWM(int pwmLeft, int pwmRight, boolean useAccel){
   //Console.print(pwmLeft);
   unsigned long TaC = millis() - lastSetMotorSpeedTime;    // sampling time in millis
   lastSetMotorSpeedTime = millis();  
-  if (TaC > 1000) TaC = 0;  
-  if (useAccel){
-    double accel = motorAccel * loopsTa;       
-    pwmLeft = (1.0 - accel) * motorLeftPWMCurr + accel * ((double)pwmLeft); 
-    pwmRight = (1.0 - accel) * motorRightPWMCurr + accel * ((double)pwmRight);  
+  if (TaC > 1000) TaC = 1;  
+  if (useAccel){    
+    // http://phrogz.net/js/framerate-independent-low-pass-filter.html
+    // value += (currentValue - value) / (smoothing / timeSinceLastSample);        
+    pwmLeft = motorLeftPWMCurr + (((float)pwmLeft)-motorLeftPWMCurr) / (motorAccel/((float)TaC));    
+    pwmRight = motorRightPWMCurr + (((float)pwmRight)-motorRightPWMCurr) / (motorAccel/((float)TaC));        
   }
   // ----- driver protection (avoids driver explosion) ----------
   if ( ((pwmLeft < 0) && (motorLeftPWMCurr >= 0)) ||
@@ -549,9 +550,9 @@ void Robot::setMotorPWM(int pwmLeft, int pwmRight, boolean useAccel){
       else motorRightZeroTimeout = 500;
   } else {
     if (pwmLeft == 0)  motorLeftZeroTimeout = max(0, ((int)(motorLeftZeroTimeout - TaC)) );
-      else motorLeftZeroTimeout = 2000;  
+      else motorLeftZeroTimeout = 700;  
     if (pwmRight == 0) motorRightZeroTimeout = max(0, ((int)(motorRightZeroTimeout - TaC)) );      
-      else motorRightZeroTimeout = 2000;  
+      else motorRightZeroTimeout = 700;  
   }
   // ---------------------------------
   motorLeftPWMCurr = pwmLeft;