put testmode into operation

src/SpecialMenus/driveModi/TestMode/menuTestMode.cpp

@@ -12,12 +12,7 @@
 
 MenuTestMode::MenuTestMode(DriveManager* driveManager)
     : MenuDriveMode(driveManager) {
-        // this->testMode = testMode;
-        // this->values[0] = 0;
-        // this->values[1] = 0;
 
-        // strcpy(this->names[0], "Centimeter");
-        // strcpy(this->names[1], "Degree");
     }
 
 MenuTestMode::~MenuTestMode() {
@@ -36,31 +31,45 @@ void MenuTestMode::printMenu() {
 }
 
 void MenuTestMode::down() {
-    this->mainMenu->down();
+    if (this->checkInput())
+        this->mainMenu->down();
 }
 
 void MenuTestMode::up() {
-    this->mainMenu->up();
+    if (this->checkInput())
+        this->mainMenu->up();
 }
 
 void MenuTestMode::right() {
-    this->mainMenu->right();
+    if (this->checkInput())
+        this->mainMenu->right();
 }
 
 void MenuTestMode::left() {
     if (this->mainMenu->isInSubmenu())
-        this->mainMenu->left();
+        if (this->checkInput())
+            this->mainMenu->left();
+        else {
+            this->testMode->abortManeuver();
+            this->mainMenu->left();
+        }
     else
         this->parentMenu->printMenu();
 }
 
 void MenuTestMode::yes() {
-    this->mainMenu->yes();
+    if (this->checkInput())
+        this->mainMenu->yes();
 }
 
 void MenuTestMode::no() {
     if (this->mainMenu->isInSubmenu())
-        this->mainMenu->no();
+        if (this->checkInput())
+            this->mainMenu->no();
+        else {
+            this->testMode->abortManeuver();
+            this->mainMenu->no();
+        }
     else
         this->left();
 }
@@ -81,10 +90,10 @@ void MenuTestMode::init() {
     // Ping google
     // WheelEncoder
 
-    MenuIntInput* drivingMenu = new MenuIntInput(2, new MenuTestModeWrapper(this->testMode, &TestMode::drive));
+    MenuIntInput* drivingMenu = new MenuIntInput(2, new MenuTestModeWrapper(this, &TestMode::drive));
-    MenuIntInput* engineLeftMenu = new MenuIntInput(2, new MenuTestModeWrapper(this->testMode, &TestMode::leftEngine));
+    MenuIntInput* engineLeftMenu = new MenuIntInput(2, new MenuTestModeWrapper(this, &TestMode::leftEngine));
-    MenuIntInput* engineRightMenu = new MenuIntInput(2, new MenuTestModeWrapper(this->testMode, &TestMode::rightEngine));
+    MenuIntInput* engineRightMenu = new MenuIntInput(2, new MenuTestModeWrapper(this, &TestMode::rightEngine));
-    MenuIntInput* engineBothMenu = new MenuIntInput(2, new MenuTestModeWrapper(this->testMode, &TestMode::bothEngine));
+    MenuIntInput* engineBothMenu = new MenuIntInput(2, new MenuTestModeWrapper(this, &TestMode::bothEngine));
 
 
     // Set menus on LCD
@@ -101,18 +110,22 @@ void MenuTestMode::init() {
     drivingMenu->setMinMaxSteps(0, -1000, 1000, 25);
     drivingMenu->setEntry(1, "Degree");
     drivingMenu->setMinMaxSteps(1, -360, 360, 15);
+    drivingMenu->setPrintParentMenu(false);
     engineLeftMenu->setEntry(0, "Percentage");
     engineLeftMenu->setMinMaxSteps(0, -100, 100, 5);
-    engineLeftMenu->setEntry(1, "Seconds", 10);
+    engineLeftMenu->setEntry(1, "Seconds", 5);
     engineLeftMenu->setMinMaxSteps(1, 0, 120, 1);
+    engineLeftMenu->setPrintParentMenu(false);
     engineRightMenu->setEntry(0, "Percentage");
     engineRightMenu->setMinMaxSteps(0, -100, 100, 5);
-    engineRightMenu->setEntry(1, "Seconds", 10);
+    engineRightMenu->setEntry(1, "Seconds", 5);
     engineRightMenu->setMinMaxSteps(1, 0, 120, 1);
+    engineRightMenu->setPrintParentMenu(false);
     engineBothMenu->setEntry(0, "Percentage");
     engineBothMenu->setMinMaxSteps(0, -100, 100, 5);
-    engineBothMenu->setEntry(1, "Seconds", 10);
+    engineBothMenu->setEntry(1, "Seconds", 5);
     engineBothMenu->setMinMaxSteps(1, 0, 120, 1);
+    engineBothMenu->setPrintParentMenu(false);
 
 
     // Entrys for the menus
@@ -148,12 +161,46 @@ void MenuTestMode::init() {
     lightMenu->addEntry(lightOffAction);    
 }
 
-MenuTestModeWrapper::MenuTestModeWrapper(TestMode* testMode, TestModeFunctionSingle testModeFunction) {
+void MenuTestMode::update() {
-    this->testMode = testMode;
+    if (this->maneuverInAction)
+        if (millis() - this->lastUpdateTime > updateDelay) {
+            this->printManeuverTime();
+            this->lastUpdateTime = millis();
+        }
+}
+
+bool MenuTestMode::checkInput() {
+    if (this->testMode->getBusy()) {
+        this->printManeuverTime();
+        this->maneuverInAction = true;
+        return false;
+    }
+
+    if (this->maneuverInAction)
+        this->maneuverInAction = false;
+    
+    return true; 
+}
+
+void MenuTestMode::printManeuverTime() {
+    std::cout << "MenuTestMode::printManeuverTime: 1" << std::endl;
+    String lineTwo = "left ";
+    std::cout << "MenuTestMode::printManeuverTime: 2" << std::endl;
+    lineTwo.concat(this->testMode->getRemainingManeuverTime());
+    std::cout << "MenuTestMode::printManeuverTime: 3" << std::endl;
+    lineTwo.concat("secs");
+    std::cout << "MenuTestMode::printManeuverTime: 4" << std::endl;
+    this->print("Maneuver time", lineTwo);
+    std::cout << "MenuTestMode::printManeuverTime: 5" << std::endl;
+}
+
+MenuTestModeWrapper::MenuTestModeWrapper(MenuTestMode* menu, TestModeFunctionSingle testModeFunction) {
+    this->menu = menu;
+    this->testMode = this->menu->getTestMode();
     this->testModeFunctionSingle = testModeFunction;
 }
 
-MenuTestModeWrapper::MenuTestModeWrapper(TestMode* testMode, TestModeFunctionDouble testModeFunction) {
+MenuTestModeWrapper::MenuTestModeWrapper(MenuTestMode* menu, TestModeFunctionDouble testModeFunction) {
     this->testMode = testMode;
     this->testModeFunctionDouble = testModeFunction;
 }
@@ -162,8 +209,13 @@ void MenuTestModeWrapper::action(int16_t* values, uint8_t length) {
     // This function calls a member function with a pointer
     // Very helpful site:
     // https://isocpp.org/wiki/faq/pointers-to-members#fnptr-vs-memfnptr-types
+    std::cout << "MenuTestModeWrapper::action: 0" << std::endl;
     if (length == 2 && this->testModeFunctionDouble)
         (this->testMode->*this->testModeFunctionDouble)(values[0], values[1]);
     else if (length == 1 && this->testModeFunctionSingle)
         (this->testMode->*this->testModeFunctionSingle)(values[0]);
+
+    std::cout << "MenuTestModeWrapper::action: 1" << std::endl;
+    this->menu->printManeuverTime();
+    std::cout << "MenuTestModeWrapper::action: 2" << std::endl;
 }   

src/SpecialMenus/driveModi/TestMode/menuTestMode.h

@@ -22,6 +22,7 @@ class MenuTestMode : public MenuDriveMode {
         MenuTestMode(DriveManager *driveManager);
         ~MenuTestMode();
         void printMenu();
+        void printManeuverTime();
 
         void down() override;
         void up() override;
@@ -30,25 +31,35 @@ class MenuTestMode : public MenuDriveMode {
         void yes() override;
         void no() override;
 
+        void update() override;
+
+        TestMode* getTestMode() const { return this->testMode; }
+
     private:
         void init();
+        bool checkInput();
 
         TestMode* testMode;
         Menu* mainMenu;
 
         bool isInit = false;
+        bool maneuverInAction = false;
+
+        uint32_t lastUpdateTime = 0;
+        uint16_t updateDelay = 1000;
 };
 
 typedef  bool (TestMode::*TestModeFunctionSingle)(int16_t);
 typedef  bool (TestMode::*TestModeFunctionDouble)(int16_t, int16_t);
 class MenuTestModeWrapper : public MenuIntInputWrapper {
     public:
-        MenuTestModeWrapper(TestMode* testMode, TestModeFunctionSingle testModeFunction);
+        MenuTestModeWrapper(MenuTestMode* menu, TestModeFunctionSingle testModeFunction);
-        MenuTestModeWrapper(TestMode* testMode, TestModeFunctionDouble testModeFunction);
+        MenuTestModeWrapper(MenuTestMode* menu, TestModeFunctionDouble testModeFunction);
 
         void action(int16_t* values, uint8_t length) override;
 
     private:
+        MenuTestMode* menu;
         TestMode* testMode;
         TestModeFunctionSingle testModeFunctionSingle = nullptr;
         TestModeFunctionDouble testModeFunctionDouble = nullptr;

src/driveModi/Modi/TestMode/testMode.cpp

@@ -22,8 +22,9 @@ void TestMode::loop() {
     if (millis() - this->lastMillis < this->delay)
         return;
 
-    if (millis() - this->actionStart > this->maneuverTime) {
+    if (millis() - this->actionStart > this->maneuverTime || this->abort) {
         this->busy = false;
+        this->abort = false;
         this->moveControl->setDrivingStatus(DrivingStatus::stop);
     }
 }
@@ -82,59 +83,58 @@ bool TestMode::drive(int16_t cm, int16_t degree) {
 }
 
 bool TestMode::leftEngine(int16_t powerPercentage, int16_t seconds) {
-    if (this->busy)
+    std::cout << "TestMode::leftEngine: 1" << std::endl;
-        return false;
+    if (this->engineInit(powerPercentage, seconds)) {
-
+        this->moveControl->setRawPowerLeft(powerPercentage);
-    if (powerPercentage >= 100 || powerPercentage <= -100)
+        return true;
-        return false;
-
-    if (true) {
-        std::cout << "It works... %: " << powerPercentage << " s: " << seconds << std::endl;
-        return false;
     }
-
+    return false;
-    this->actionStart = millis();
-    this->moveControl->setDrivingStatus(DrivingStatus::raw);
-    this->moveControl->setRawPowerLeft(powerPercentage);
-    this->maneuverTime = seconds * 1000;
-    this->busy = true;
 }
 
 bool TestMode::rightEngine(int16_t powerPercentage, int16_t seconds) {
-    if (this->busy)
+    if (this->engineInit(powerPercentage, seconds)) {
-        return false;
+        this->moveControl->setRawPowerRight(powerPercentage);
-
+        return true;
-    if (powerPercentage >= 100 || powerPercentage <= -100)
-        return false;
-
-    if (true) {
-        std::cout << "It works... %: " << powerPercentage << " s: " << seconds << std::endl;
-        return false;
     }
-
+    return false;
-    this->actionStart = millis();
-    this->moveControl->setDrivingStatus(DrivingStatus::raw);
-    this->moveControl->setRawPowerRight(powerPercentage);
-    this->maneuverTime = seconds * 1000;
-    this->busy = true;
 }
 
 bool TestMode::bothEngine(int16_t powerPercentage, int16_t seconds) {
+    if (this->engineInit(powerPercentage, seconds)) {
+        this->moveControl->setRawPowerLeft(powerPercentage);
+        this->moveControl->setRawPowerRight(powerPercentage);;
+        return true;
+    }
+    return false;
+}
+
+void TestMode::abortManeuver() {
+    this->abort = true;
+}
+
+uint8_t TestMode::getRemainingManeuverTime() {
+    if (busy)
+        return (uint8_t) ((this->maneuverTime - (millis() - this->actionStart)) / 1000);
+    return 0;
+}
+
+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;
-        return false;
     }
 
     this->actionStart = millis();
     this->moveControl->setDrivingStatus(DrivingStatus::raw);
-    this->moveControl->setRawPowerLeft(powerPercentage);
-    this->moveControl->setRawPowerRight(powerPercentage);
     this->maneuverTime = seconds * 1000;
     this->busy = true;
+    return true;
 }

src/driveModi/Modi/TestMode/testMode.h

@@ -30,13 +30,21 @@ class TestMode : public DriveModi {
         bool rightEngine(int16_t powerPercentage, int16_t seconds);
         bool bothEngine(int16_t powerPercentage, int16_t seconds);
 
+        void abortManeuver();
+
+        uint8_t getRemainingManeuverTime();
+        bool getBusy() const { return this->busy; }
+
     private:
+        bool engineInit(int16_t powerPercentage, int16_t seconds);
+
         MoveControl *moveControl;
 
         double speed = 0;
         double rotationSpeed = 0;
 
         bool busy = false;
+        bool abort = false;
         
         uint8_t delay = 10;