diff --git a/sunray/comm.cpp b/sunray/comm.cpp index b392719fe..ae446f3c8 100644 --- a/sunray/comm.cpp +++ b/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(); diff --git a/sunray/config_example.h b/sunray/config_example.h index 9064b777e..6b768b903 100644 --- a/sunray/config_example.h +++ b/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 - diff --git a/sunray/motor.cpp b/sunray/motor.cpp index 8b0b9d118..c03f0567b 100644 --- a/sunray/motor.cpp +++ b/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); diff --git a/sunray/motor.h b/sunray/motor.h index f5a5f0c8c..3961c67be 100644 --- a/sunray/motor.h +++ b/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(); };