Implement 75% off menu

src/main.cpp

@@ -8,13 +8,23 @@
 #include "driveModi/driveManager.h"
 #include "moveControl.h"
 #include "network.h"
+#include "menu.h"
+#include "menuEntry.h"
+
+// Platzhalter, muss irgendwann weg
+void dummy(){Serial.println("Dummy in Action");}
 
 void callbackControllerAction();
 void callbackControllerConnect();
 void controllerPrintBattery();
+void p_f();
+void i_f();
+void d_f();
+int32_t displaySelectValue();
 
 MoveControl moveController;
 DriveManager driveManager(&moveController);
+Menu* main_m;
 
 void setup() {
     Serial.begin(115200);
@@ -29,9 +39,40 @@ void setup() {
     Serial.println("\nReady to connect a PS3 Controller... \n");
     Ps3.begin();
 
-    driveManager.changeModus(Modi::ManualControl);
-
     Serial1.begin(GPS_BAUD, SERIAL_8N1, GPS_RX, GPS_TX);
+
+    // Create Menu
+    main_m = new Menu();
+    Menu* mode_m = new Menu();
+    Menu* pid_m = new Menu();
+
+    // Entry for the main menu
+    MenuEntry* mode_e = new MenuEntry("Mode", mode_m);
+    MenuEntry* gps_e = new MenuEntry("GPS", dummy);
+    MenuEntry* pid_e = new MenuEntry("PID", pid_m);
+    main_m->addEntry(mode_e);
+    main_m->addEntry(gps_e);
+    main_m->addEntry(pid_e);
+
+    // Entry for the mode Menu
+    MenuEntry* autopilot_e = new MenuEntry("Autopilot", dummy);
+    MenuEntry* captureRoute_e = new MenuEntry("Capture Route", dummy);
+    MenuEntry* consolControl_e = new MenuEntry("Consol Control", dummy);
+    MenuEntry* manualControl_e = new MenuEntry("Manual Control", dummy);
+    MenuEntry* testMode_e = new MenuEntry("Test Mode", dummy);
+    mode_m->addEntry(autopilot_e);
+    mode_m->addEntry(captureRoute_e);
+    mode_m->addEntry(consolControl_e);
+    mode_m->addEntry(manualControl_e);
+    mode_m->addEntry(testMode_e);
+
+    // Entry for the PID Menu
+    MenuEntry* p_e = new MenuEntry("P", dummy);
+    MenuEntry* i_e = new MenuEntry("I", dummy);
+    MenuEntry* d_e = new MenuEntry("D", dummy);
+    pid_m->addEntry(p_e);
+    pid_m->addEntry(i_e);
+    pid_m->addEntry(d_e);
 }
 
 void loop() {
@@ -41,8 +82,36 @@ void loop() {
         Serial.print(Serial1.read());
 }
 
-void callbackControllerAction() {
+bool isDelayReached() {
+    static uint32_t lastMillis = 0;
+    const uint16_t delay = 200;
+    bool res = false;
 
+    if (millis() - lastMillis > delay) {
+        res = true;
+        lastMillis = millis();
+    }
+    
+    return res;
+}
+
+void callbackControllerAction() {
+    // Menu controlling
+
+    if (isDelayReached()) {
+        if (Ps3.data.button.up)
+            main_m->prevEntry();
+        else if (Ps3.data.button.down)
+            main_m->nextEntry();
+        else if (Ps3.data.button.left)
+            main_m->enterParentMenu();
+        else if (Ps3.data.button.right)
+            main_m->enterEntry();
+        else if (Ps3.data.button.select)
+            main_m->printMenu();
+        else if (Ps3.data.button.ps)
+            controllerPrintBattery();
+    }
 }
 
 void callbackControllerConnect() {
@@ -64,3 +133,19 @@ void controllerPrintBattery() {
     else if( controller_battery == ps3_status_battery_shutdown ) Serial.println("SHUTDOWN");
     else Serial.println("UNDEFINED");
 }
+
+void p_f() {
+
+}
+
+void i_f() {
+
+}
+
+void d_f() {
+
+}
+
+int32_t displaySelectValue() {
+    
+}