bugFix testModeMenu implement duration print

src/driveModi/Modi/TestMode/testMode.cpp

@@ -10,8 +10,9 @@
  */
 #include "testMode.h"
 
-TestMode::TestMode(MoveControl *moveControl) {
+TestMode::TestMode(MoveControl *moveControl,  Navigation* navigation) {
     this->moveControl = moveControl;
+    this->navigation = navigation;
 }
 
 TestMode::~TestMode() {
@@ -22,11 +23,20 @@ void TestMode::loop() {
     if (millis() - this->lastMillis < this->delay)
         return;
 
+    if (this->maneuver == Maneuver::Turn) {
+        uint16_t delta = abs(this->azimuth - this->navigation->getAzimuth());
+        if (delta > this->degree)
+            this->abort = true;
+    }
+
     if (millis() - this->actionStart > this->maneuverTime || this->abort) {
         this->busy = false;
         this->abort = false;
         this->moveControl->setDrivingStatus(DrivingStatus::stop);
+        this->maneuver = Maneuver::None;
     }
+
+    this->lastMillis = millis();
 }
 
 void TestMode::setSpeed(int16_t speed) {
@@ -41,13 +51,9 @@ bool TestMode::drive(int16_t cm, int16_t degree) {
     if (this->busy)
         return false;
 
-    if (true) {
-        std::cout << "It works... cm: " << cm << " degree: " << degree << std::endl;
-        return false;
-    }
-
     this->actionStart = millis();
     this->moveControl->setDrivingStatus(DrivingStatus::drive);
+
     if (cm == 0) {
         //Only left or right
         this->moveControl->setSpeed(0);
@@ -57,8 +63,10 @@ bool TestMode::drive(int16_t cm, int16_t degree) {
         else if (degree > 0)
             this->moveControl->setRotationSpeed(this->rotationSpeed);
         
-        //TODO: calc this shit
-        this->maneuverTime = 10;
+        this->azimuth = this->navigation->getAzimuth();
+        this->degree = degree;
+
+        this->maneuverTime = 5 * 1000;
         this->busy = true;
         return true;
 
@@ -73,9 +81,20 @@ bool TestMode::drive(int16_t cm, int16_t degree) {
         
         this->maneuverTime = (uint32_t) (((double) cm / 100.0) / this->speed) * 1000;
         this->busy = true;
+        this->maneuver = Maneuver::Drive;
         return true;
     } else {
         //forward or backward and left or right
+
+        if (cm < 0)
+            this->moveControl->setSpeed(-this->speed);
+        else if (cm > 0)
+            this->moveControl->setSpeed(this->speed);
+        
+        this->maneuverTime = (uint32_t) (((double) cm / 100.0) / this->speed) * 1000;
+        this->busy = true;
+        this->maneuver = Maneuver::Drive;
+        return true;
     }
 
     this->moveControl->setDrivingStatus(DrivingStatus::stop);    
@@ -83,9 +102,9 @@ bool TestMode::drive(int16_t cm, int16_t degree) {
 }
 
 bool TestMode::leftEngine(int16_t powerPercentage, int16_t seconds) {
-    std::cout << "TestMode::leftEngine: 1" << std::endl;
     if (this->engineInit(powerPercentage, seconds)) {
         this->moveControl->setRawPowerLeft(powerPercentage);
+        this->maneuver = Maneuver::LeftEngine;
         return true;
     }
     return false;
@@ -94,6 +113,7 @@ bool TestMode::leftEngine(int16_t powerPercentage, int16_t seconds) {
 bool TestMode::rightEngine(int16_t powerPercentage, int16_t seconds) {
     if (this->engineInit(powerPercentage, seconds)) {
         this->moveControl->setRawPowerRight(powerPercentage);
+        this->maneuver = Maneuver::RightEngine;
         return true;
     }
     return false;
@@ -102,7 +122,8 @@ bool TestMode::rightEngine(int16_t powerPercentage, int16_t seconds) {
 bool TestMode::bothEngine(int16_t powerPercentage, int16_t seconds) {
     if (this->engineInit(powerPercentage, seconds)) {
         this->moveControl->setRawPowerLeft(powerPercentage);
-        this->moveControl->setRawPowerRight(powerPercentage);;
+        this->moveControl->setRawPowerRight(powerPercentage);
+        this->maneuver = Maneuver::BothEngine;
         return true;
     }
     return false;
@@ -121,17 +142,11 @@ uint8_t TestMode::getRemainingManeuverTime() {
 bool TestMode::engineInit(int16_t powerPercentage, int16_t seconds) {
     if (this->busy)
         return false;
-
     if (powerPercentage >= 100 || powerPercentage <= -100)
         return false;
-
     if (seconds < 0)
         return false;
 
-    if (true) {
-        std::cout << "It works... %: " << powerPercentage << " s: " << seconds << std::endl;
-    }
-
     this->actionStart = millis();
     this->moveControl->setDrivingStatus(DrivingStatus::raw);
     this->maneuverTime = seconds * 1000;

src/driveModi/Modi/TestMode/testMode.h

@@ -11,12 +11,24 @@
 #ifndef TEST_MODE_H 
 #define TEST_MODE_H
 
+#include <iostream>
+
 #include "moveControl.h"
+#include "navigation.h"
 #include "driveModi/driveModi.h"
 
 class TestMode : public DriveModi {
     public:
-        TestMode(MoveControl *MoveControl);
+        enum Maneuver {
+            None,
+            LeftEngine,
+            RightEngine,
+            BothEngine,
+            Turn,
+            Drive
+        };
+
+        TestMode(MoveControl *moveControl, Navigation* navigation);
         ~TestMode();
 
         void loop() override;
@@ -34,11 +46,16 @@ class TestMode : public DriveModi {
 
         uint8_t getRemainingManeuverTime();
         bool getBusy() const { return this->busy; }
+        Maneuver getManeuver() const { return this->maneuver; }
 
     private:
         bool engineInit(int16_t powerPercentage, int16_t seconds);
 
         MoveControl *moveControl;
+        Navigation* navigation;
+        Maneuver maneuver = Maneuver::None;
+        int16_t maneuverValueOne = 0;
+        int16_t maneuverValueTwo = 0;
 
         double speed = 0;
         double rotationSpeed = 0;
@@ -47,11 +64,13 @@ class TestMode : public DriveModi {
         bool abort = false;
         
         uint8_t delay = 10;
-
+        uint16_t azimuth;
+        int16_t degree;
         uint32_t maneuverTime = 0;
         uint32_t lastMillis = 0;
         uint32_t actionStart = 0;
 
+
 };
 
 #endif // TEST_MODE_H