add test for mowmotor

sunray/comm.cpp

@@ -204,6 +204,15 @@ void cmdMotorTest(){
   motor.test();  
 }
 
+//Svol0 TestMowMotor
+// To find optimal value for pwmMaxMow
+void cmdMowMotorTest(){
+  String s = F("D");
+  cmdAnswer(s);
+  motor.testMow();  
+}
+//END Svol0 TestMowMotor
+
 void cmdMotorPlot(){
   String s = F("Q");
   cmdAnswer(s);
@@ -789,6 +798,7 @@ void processCmd(bool checkCrc, bool decrypt){
   if (cmd[3] == 'P') cmdPosMode();  
   if (cmd[3] == 'T') cmdStats();
   if (cmd[3] == 'L') cmdClearStats();
+  if (cmd[3] == 'D') cmdMowMotorTest(); //Svol0 TestMowMotor															  
   if (cmd[3] == 'E') cmdMotorTest();  
   if (cmd[3] == 'Q') cmdMotorPlot();  
   if (cmd[3] == 'O') cmdObstacle();  

sunray/config_example.h

@@ -186,6 +186,9 @@ Also, you may choose the serial port below for serial monitor output (CONSOLE).
 // certain time (normally a few seconds) and the mower will try again and set a virtual obstacle after too many tries
 // On the other hand, the overload detection will detect situations the fault signal cannot detect: slightly higher current for a longer time 
 
+// choose ticks per cutting disc revolution :
+#define MOW_TICKS_PER_REVOLUTION  12 / 2   // odometry ticks per cutting disc revolution 
+
 #define MOW_FAULT_CURRENT 8.0       // mowing motor fault current (amps)
 #define MOW_OVERLOAD_CURRENT 2.0    // mowing motor overload current (amps)
 
@@ -604,4 +607,3 @@ Also, you may choose the serial port below for serial monitor output (CONSOLE).
 #ifdef BNO055
   #define MPU9250   // just to make mpu driver happy to compile something
 #endif
-

sunray/motor.cpp

@@ -11,6 +11,8 @@
 
 
 
+bool mowTestActiv = false;
+int pwmMowTest  = 0;
 
 void Motor::begin() {
 	pwmMax = 255;
@@ -30,6 +32,15 @@ void Motor::begin() {
 
   //ticksPerRevolution = 1060/2;
   ticksPerRevolution = TICKS_PER_REVOLUTION;
+
+  // check for MOW_TICKS_PER_REVOLUTION value
+  #ifdef MOW_TICKS_PER_REVOLUTION
+    ticksPerMowMotorRevolution = MOW_TICKS_PER_REVOLUTION;
+    if (ticksPerMowMotorRevolution == 0) ticksPerMowMotorRevolution = 1;
+  #else
+    ticksPerMowMotorRevolution = 1; // avoid div by zero
+  #endif
+
 	wheelBaseCm = WHEEL_BASE_CM;    // wheel-to-wheel distance (cm) 36
   wheelDiameter = WHEEL_DIAMETER; // wheel diameter (mm)
   ticksPerCm         = ((float)ticksPerRevolution) / (((float)wheelDiameter)/10.0) / 3.1415;    // computes encoder ticks per cm (do not change)  
@@ -405,7 +416,7 @@ void Motor::run() {
   // 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
+  motorMowRpmCurr = 60.0 * ( ((float)ticksMow) / ((float)ticksPerMowMotorRevolution) ) / deltaControlTimeSec;
   float lp = 0.9; // 0.995
   motorLeftRpmCurrLP = lp * motorLeftRpmCurrLP + (1.0-lp) * motorLeftRpmCurr;
   motorRightRpmCurrLP = lp * motorRightRpmCurrLP + (1.0-lp) * motorRightRpmCurr;
@@ -712,6 +723,34 @@ void Motor::dumpOdoTicks(int seconds){
   CONSOLE.println();               
 }
 
+void Motor::dumpOdoMowTicks(){
+  int ticksLeft=0;
+  int ticksRight=0;
+  int ticksMow=0;
+  motorDriver.getMotorEncoderTicks(ticksLeft, ticksRight, ticksMow);  
+  motorMowTicks += ticksMow;
+
+  unsigned long currTime = millis();
+  float deltaControlTimeSec =  ((float)(currTime - lastControlTime)) / 1000.0;
+  lastControlTime = currTime;
+
+  motorMowRpmCurr = 60.0 * ( ((float)ticksMow) / ((float)ticksPerMowMotorRevolution) ) / deltaControlTimeSec;
+
+  float lp = 0.9; // 0.995
+  motorMowRpmCurrLP = lp * motorMowRpmCurrLP + (1.0-lp) * motorMowRpmCurr;
+
+  CONSOLE.print(" pwmMow: ");
+  CONSOLE.print(pwmMowTest);
+  CONSOLE.print(" | RpmCurr: ");
+  CONSOLE.print(motorMowRpmCurr);
+  CONSOLE.print("U/min | RpmCurrLP: ");
+  CONSOLE.print(motorMowRpmCurrLP);
+  CONSOLE.print("U/min | Sense: ");
+  CONSOLE.print(motorMowSense); // motorstrom
+  CONSOLE.print("A | SenseLP: ");        
+  CONSOLE.print(motorMowSenseLP); // motorstrom
+  CONSOLE.println("A");
+}
 
 void Motor::test(){
   CONSOLE.println("motor test - 10 revolutions");
@@ -756,7 +795,205 @@ void Motor::test(){
   CONSOLE.println("motor test done - please ignore any IMU/GPS errors");
 }
 
+//Svol0 TestMowMotor
+void Motor::testMow(){
+  unsigned long nextMowSpeedChange = 0;
+  int MowTestStep = 0;
+  int RepeatCounter = 0;
+  motorMowTicks = 0;
+  pwmMowTest    = 0;
+  int pwmMowMem = 0;
+  unsigned long nextValueOut = 0; 
+
+  mowTestActiv  = true; // to disable speedlimitation
+  // test, if 'start/stop button' is bridged
+  if (digitalRead(pinButton) == HIGH) {
+    while (mowTestActiv == true){  
+      // abbruch, wenn die Start-Taste während des Tests gedrückt wird
+      if (MowTestStep >= 4 && MowTestStep < 10){
+        if (digitalRead(pinButton) == LOW){
+          nextMowSpeedChange = millis();
+          MowTestStep = 10;
+        }
+      }
+      if (MowTestStep == 12 || MowTestStep == 13){
+        if (digitalRead(pinButton) == LOW){
+          nextMowSpeedChange = millis();
+          MowTestStep = 14;
+        }
+      }
+
+      sense();          
+      buzzer.run();
+      watchdogReset();
 
+      // Infos im Sekundentakt rausschreiben
+      if (MowTestStep >= 6 && MowTestStep < 10){
+        if (millis() > nextValueOut){
+          nextValueOut += 1000;
+          dumpOdoMowTicks();
+        }
+      } else nextValueOut = millis();
+
+      switch (MowTestStep) {
+        case 0: // Infos out
+          CONSOLE.println("*****************************************************************************************************************************");
+          CONSOLE.println("motor.cpp Motor::testMow:");
+          CONSOLE.println( "TEST STARTS WITH pwmMow = 100; VALUE INCREASES EVERY 10 SECONDS BY 5 TILL 'pwmMow = 255':");
+          CONSOLE.println(" ATTENTION! TO START THE MOWMOTOR KEEP THE 'START/STOP BUTTON' PRESSED FOR AT LEAST 5 SECONDS!");
+          CONSOLE.println(" THE MOWMOTOR CAN BE STOPPED BY PRESSING AGAIN THE 'START/STOP BUTTON'");
+          MowTestStep++;
+          nextMowSpeedChange = millis();
+          break;
+
+        case 1: // Warten, dass die Starttaste für min. 5 Sek gedrückt wird.
+          if (digitalRead(pinButton) == HIGH) nextMowSpeedChange = millis();
+          RepeatCounter = 0;
+          delay(20);
+          if (millis() - nextMowSpeedChange > 5000) MowTestStep++;
+          break;
+
+        case 2: // Bestätigungston Abspielen
+          nextMowSpeedChange = millis() + 1000;
+          CONSOLE.println(" PLEASE RELEASE THE 'START/STOP' TO GO ON WITH THE TEST");
+          buzzer.sound(SND_READY, true);
+          MowTestStep++;
+          break;
+
+        case 3: // warten, dass die Taste wieder losgelassen wird
+          if (millis() - nextMowSpeedChange > 1000){
+            if (digitalRead(pinButton) == HIGH){
+              CONSOLE.println(" ATTENTION! MOWMOTOR WILL START SPINN UP IN LESS THAN 10 SECONDS!");
+              MowTestStep++;
+            }
+          }
+          break;
+
+        case 4: // Warnton laden
+          if (RepeatCounter <= 3){
+            nextMowSpeedChange = millis() + 2000;
+            buzzer.sound(SND_ERROR, true);
+          } else {
+            nextMowSpeedChange = millis() + 250;
+            buzzer.sound(SND_READY, true);
+          }
+          RepeatCounter++;
+          MowTestStep++;
+          break;
+
+        case 5: // Warnton abspielen
+          if (millis() > nextMowSpeedChange){
+            if (RepeatCounter <= 12) MowTestStep--;
+            else {
+              CONSOLE.println(" ATTENTION! MOWMOTOR IS SPINNING UP!");
+              MowTestStep++;
+            }
+          }          
+          break;
+
+        case 6: // spin up mow motor
+          if (millis() > nextMowSpeedChange) MowTestStep++;
+          break;
+
+        case 7: // spin up mow motor
+          if (pwmMowTest < 100) {
+            nextMowSpeedChange = millis() + 50; // erhöhung alle 50ms
+            pwmMowTest += 1;
+            speedPWM(0, 0, pwmMowTest);
+            MowTestStep--;
+          } else {
+              CONSOLE.println(" MOWMOTOR SPINN UP COMPLETED. PWM-VALUE WILL INCREASE EVERY 10 SECOUNDS BY 5 TILL pwmMow = 255");    
+              nextMowSpeedChange = millis() + 10000;
+              MowTestStep++; // fertig hochgelaufen
+          }
+          break;
+
+        case 8: // Alle 10 Sekunden wird der Mähmotor pwm-Wert um 5 erhöht
+          buzzer.sound(SND_READY, true);
+          pwmMowTest = pwmMowTest + 5;
+          if (pwmMowTest > 255){
+            pwmMowTest = 255;
+            MowTestStep = 10;
+          } else {
+            speedPWM(0, 0, pwmMowTest);
+            MowTestStep++;
+          }
+          break;
+
+        case 9:
+          if (millis() > nextMowSpeedChange){
+            nextMowSpeedChange = millis() + 10000;
+            MowTestStep--;
+          }
+          break;
+
+        case 10:  // reduziere die Geschwindigkeit
+          pwmMowMem = pwmMowTest;  // store last pwm-value
+          CONSOLE.println(" MOWMOTOR IS SLOWING DOWN.");
+          MowTestStep++;
+          break;
+
+        case 11:
+          if (pwmMowTest > 0){
+            if (millis() > nextMowSpeedChange){
+              pwmMowTest = pwmMowTest - 1;
+              speedPWM(0, 0, pwmMowTest);
+              nextMowSpeedChange = millis() + 10;
+            }
+          } else {
+            nextMowSpeedChange = millis();
+            RepeatCounter = 0;
+            MowTestStep++;
+          }
+          break;
+
+        case 12: // Info wiederholt ausgeben
+          CONSOLE.println(" YOU CAN ABORT THE DELAY OF 120 SEC BY PRESSING THE START/STOP BUTTON");
+          nextMowSpeedChange = millis() + 5000;
+          buzzer.sound(SND_READY, true);
+          RepeatCounter++;
+          MowTestStep++;
+          break;
+
+        case 13: // Warten
+          if (millis() > nextMowSpeedChange){
+            if (RepeatCounter <= 24) MowTestStep--;
+            else {
+              MowTestStep++;
+            }
+          }          
+          break;
+
+        case 14:
+          CONSOLE.println(" MOWMOTOR-TEST DONE - please ignore any IMU/GPS errors.");
+          CONSOLE.print(" LAST PWM VALUE BEFORE STOP WAS: ");
+          CONSOLE.println(pwmMowMem);
+          CONSOLE.println("*****************************************************************************************************************************");
+          pwmMowTest = MIN_MOW_RPM;
+          MowTestStep++;
+          nextMowSpeedChange = millis() + 5000;
+          break;
+
+        case 15:
+          if (millis() > nextMowSpeedChange){
+            mowTestActiv  = false; // enable speedlimitation
+          }
+          break;
+
+      }    
+    } // while (mowTestActiv == true)
+  }
+  else {
+    speedPWM(0, 0, 0);
+    mowTestActiv  = false; // enable speedlimitation
+    pwmMowTest = MIN_MOW_RPM;
+    CONSOLE.println("motor.cpp Motor::testMow: START/STOP BUTTON SEEMS TO BE BRIDGED. END OF TEST");
+    CONSOLE.println("motor.cpp Motor::testMow: please ignore any IMU/GPS errors");
+    CONSOLE.println("*****************************************************************************************************************************");
+    delay(4000);
+  }
+
+} 
 void Motor::plot(){
   CONSOLE.println("motor plot (left,right,mow) - NOTE: Start Arduino IDE Tools->Serial Plotter (CTRL+SHIFT+L)");
   delay(5000);

sunray/motor.h

@@ -20,6 +20,7 @@ class Motor {
     float wheelBaseCm;  // wheel-to-wheel diameter
     int wheelDiameter;   // wheel diameter (mm)
     int ticksPerRevolution; // ticks per revolution
+    int ticksPerMowMotorRevolution; // ticks per revolution of the Mowmotor
     float ticksPerCm;  // ticks per cm
     bool activateLinearSpeedRamp;  // activate ramp to accelerate/slow down linear speed?
     bool toggleMowDir; // toggle mowing motor direction each mow motor start?    
@@ -57,6 +58,7 @@ class Motor {
     void begin();
     void run();      
     void test();
+    void testMow(); //Svol0 TestMowMotor
     void plot();
     void enableTractionMotors(bool enable);
     void setLinearAngularSpeed(float linear, float angular, bool useLinearRamp = true);
@@ -102,6 +104,7 @@ class Motor {
     bool checkCurrentTooLowError();
     void sense();
     void dumpOdoTicks(int seconds);    
+    void dumpOdoMowTicks();    
 };