try to make menus accessable from driveMode

src/SpecialMenus/Speed/menuSpeed.h

@@ -14,7 +14,7 @@
 
 #include <menuIntInput.h>
 
-class MenuSpeed : MenuIntInput {
+class MenuSpeed : public MenuIntInput {
     
 };
 

src/SpecialMenus/driveModi/Autopilot/menuAutopilot.cpp

@@ -201,7 +201,7 @@ void MenuAutopilot::printPage() const {
     this->print(lineOne, lineTwo);
 }
 
-void MenuAutopilot::runCommand() const {
+void MenuAutopilot::runCommand() {
     switch (this->getCurrentPage()) {
         case 5:
             this->autopilot->switchLoopMode();
@@ -226,7 +226,7 @@ void MenuAutopilot::runCommand() const {
     }
 }
 
-void MenuAutopilot::runCommandNo() const {
+void MenuAutopilot::runCommandNo() {
     switch (this->getCurrentPage()) {
     case 7:
         this->minDistance = this->autopilot->getNavigation()->decreaseMinDistanceToReachPoint();
@@ -254,4 +254,6 @@ void MenuAutopilot::init() {
     //dirty hack to get the val, can be better!!! TODO
     this->autopilot->getNavigation()->increaseMinDistanceToReachPoint();
     this->minDistance = this->autopilot->getNavigation()->decreaseMinDistanceToReachPoint();
+
+    MenuDriveMode::init();
 }

src/SpecialMenus/driveModi/Autopilot/menuAutopilot.h

@@ -47,8 +47,8 @@ class MenuAutopilot : public MenuDriveMode {
 
     private:
         void init() override;
-        void runCommand() const override;
-        void runCommandNo() const override;
+        void runCommand() override;
+        void runCommandNo() override;
 
         bool mutable targetFreezed = false;
         double mutable minDistance;

src/SpecialMenus/driveModi/CalibrateCompass/menuCalibrateCompass.cpp

@@ -117,7 +117,7 @@ void MenuCalibrateCompass::init() {
     this->updateDelay = 500; 
 }
 
-void MenuCalibrateCompass::runCommand() const {
+void MenuCalibrateCompass::runCommand() {
     switch (this->getCurrentPage()) {
         case 2:
             switch (this->caliCompass->getState()) {

src/SpecialMenus/driveModi/CalibrateCompass/menuCalibrateCompass.h

@@ -42,7 +42,7 @@ class MenuCalibrateCompass : public MenuDriveMode {
 
     protected:
         void init() override;
-        void runCommand() const override;
+        void runCommand() override;
 
     private:
         CalibrateCompassM* caliCompassMode;

src/SpecialMenus/driveModi/CaptureRoute/menuCaptureRoute.cpp

@@ -114,7 +114,7 @@ void MenuCaptureRoute::update() {
     this->printMenu();
 }
 
-void MenuCaptureRoute::runCommand() const {
+void MenuCaptureRoute::runCommand() {
     switch (this->getCurrentPage())
     {
     case 7:
@@ -134,5 +134,5 @@ void MenuCaptureRoute::init() {
     this->updateDelay = 500;
     this->captureRoute = new CaptureRoute();
     this->driveManager->changeModus(this->captureRoute);
-    
+    MenuDriveMode::init();
 }

src/SpecialMenus/driveModi/CaptureRoute/menuCaptureRoute.h

@@ -43,7 +43,7 @@ class MenuCaptureRoute : public MenuDriveMode {
          */
         void update() override;
 
-        void runCommand() const override;
+        void runCommand() override;
 
     private:
         void init() override;

src/SpecialMenus/driveModi/ManualDrive/menuManualDrive.cpp

@@ -11,7 +11,7 @@
 #include "menuManualDrive.h"
 
 MenuManualControl::MenuManualControl(DriveManager* driveManager) : MenuDriveMode(driveManager) {
-
+    this->noEqualLeft = true;
 }
 
 MenuManualControl::~MenuManualControl() {
@@ -35,15 +35,11 @@ void MenuManualControl::printPage() const {
             break;
 
         case 2:
-            lineOne = "Speed: ";
-            lineOne.concat(this->manualControl->getMaxSpeed());
-            lineTwo = "- dec 0.1 inc +";
+            lineOne = "Speed Menu";
             break;
 
         case 3:
-            lineOne = "RotSpeed: ";
-            lineOne.concat(this->manualControl->getMaxRotation());
-            lineTwo = "- dec 0.1 inc +";
+            lineOne = "Sensor Menu";
             break;
 
         default:
@@ -60,36 +56,21 @@ void MenuManualControl::init() {
     this->driveManager->changeModus(this->manualControl);
     this->setCountPages(4);
     this->updateDelay = 500; 
+    MenuDriveMode::init();
 }
 
-void MenuManualControl::runCommand() const {
+void MenuManualControl::runCommand() {
     switch (this->getCurrentPage()) {
         case 1:
             this->manualControl->switchInputMode();
             break;
 
         case 2:
-            this->manualControl->increaseMaxSpeed();
+            this->enterSpeedMenu();
             break;
 
         case 3:
-            this->manualControl->increaseMaxRotation();
-            break;
-        
-        default:
-            break;
-    }
-}
-
-void MenuManualControl::runCommandNo() const {
-    switch (this->getCurrentPage())
-    {
-    case 2:
-        this->manualControl->decreaseMaxSpeed();
-        break;
-
-    case 3:
-        this->manualControl->decreaseMaxRotation();
+            this->enterSensorMenu();
             break;
         
         default:

src/SpecialMenus/driveModi/ManualDrive/menuManualDrive.h

@@ -41,8 +41,7 @@ class MenuManualControl : public MenuDriveMode {
 
     protected:
         void init() override;
-        void runCommand() const override;
-        void runCommandNo() const override;
+        void runCommand() override;
 
     private:
         ManualControl* manualControl;

src/SpecialMenus/driveModi/menuDriveMode.cpp

@@ -16,11 +16,90 @@ MenuDriveMode::MenuDriveMode(DriveManager* driveManager) {
 }
 
 void MenuDriveMode::left() {
+    if (this->activeMenu) {
+        if (this->activeMenu)
+        this->activeMenu->left();
+        return;
+    }
+
     this->firstPrint = true;
     this->configureOnLeave();
     this->driveManager->changeModus();
-    if (parentMenu) {
-        this->parentMenu->printMenu();
-        this->leaved = true;
-    }
+    MenuInformationSites::left();
+}
+
+void MenuDriveMode::right() {
+    if (this->activeMenu) {
+        this->activeMenu->right();
+        return;
+    }
+    MenuInformationSites::right();
+}
+
+void MenuDriveMode::up() {
+    if (this->activeMenu) {
+        this->activeMenu->up();
+        return;
+    }
+    MenuInformationSites::up();
+}
+
+void MenuDriveMode::down() {
+    if (this->activeMenu) {
+        this->activeMenu->down();
+        return;
+    }
+    MenuInformationSites::down();
+}
+
+void MenuDriveMode::yes() {
+    if (this->activeMenu) {
+        this->activeMenu->yes();
+        return;
+    }
+    MenuInformationSites::yes();
+}
+
+void MenuDriveMode::no() {
+    if (this->activeMenu) {
+        this->activeMenu->no();
+        return;
+    }
+    MenuInformationSites::no();
+}
+
+void MenuDriveMode::prepareReenterMenu() {
+    this->activeMenu = nullptr;
+}
+
+void MenuDriveMode::printMenu() {
+    if (this->activeMenu) {
+        this->activeMenu->printMenu();
+        return;
+    }
+
+    MenuInformationSites::printMenu();
+}
+
+void MenuDriveMode::init() {
+    this->activeMenu = nullptr;
+}
+
+void MenuDriveMode::enterSpeedMenu() {
+    this->activeMenu = this->menuSpeed;
+    this->enterMenu();
+}
+
+void MenuDriveMode::enterSensorMenu() {
+    this->activeMenu = this->menuSensor;
+    this->enterMenu();
+}
+
+void MenuDriveMode::enterMenu() {
+    if (!this->activeMenu)
+        return
+    
+    this->activeMenu->printMenu();
+    std::cout << "Sensor Menu was theretically printed" << std::endl;
+    this->activeMenu->setParentMenu(this);
 }

src/SpecialMenus/driveModi/menuDriveMode.h

@@ -29,19 +29,37 @@ class MenuDriveMode : public MenuInformationSites {
          */
         MenuDriveMode(DriveManager* driveManager);
 
+        void addSensorMenu(MenuSensorData* menuSensor) { this->menuSensor = menuSensor; }
+        void addSpeedMenu(MenuSpeed* menuSpeed) { this->menuSpeed = menuSpeed; }
+
         /**
          * @brief Goes back to the parentMenu.
          */
         void left() override;
+        void right() override;
+        void up() override;
+        void down() override;
+        void yes() override;
+        void no() override;
+
+        void prepareReenterMenu() override;
+        void printMenu() override;
 
     protected:
-        void configureOnLeave() {}
+        void init() override;
+        virtual void configureOnLeave() {}
+        void enterSpeedMenu();
+        void enterSensorMenu();
 
         DriveManager* driveManager;
-        MenuSensorData* menuSensor;
-        MenuSpeed* menuSpeed;
         
         bool firstPrint = true;
-};
 
+    private:
+        void enterMenu();
+
+        MenuControl* activeMenu = nullptr;
+        MenuSensorData* menuSensor = nullptr;
+        MenuSpeed* menuSpeed = nullptr;
+};
 #endif // MENU_DRIVE_MODI_H

src/main.cpp

@@ -223,6 +223,8 @@ void makeMenu() {
     sys_m->setUpdateDelay(1500);
     sen_m->setUpdateDelay(500);
 
+    man_m->addSensorMenu(sen_m);
+
     // Entry for the main menu
     main_m->addEntry(new MenuAction("Mode", mode_m));
     main_m->addEntry(new MenuAction("Sensor", sen_m));