refactor, added akku menu, finished autopilot menu

lib/Navigation/navigation.cpp

@@ -78,14 +78,11 @@ CourseCorrection Navigation::getCourseCorrection() {
 }
 
 bool Navigation::addCurrentPosToRoute() {
-    std::cout << "Navigation::addCurrentPosToRoute 1" << std::endl;
     Point p = this->currentLocation();
-    std::cout << "Navigation::addCurrentPosToRoute " << p.lat << " " << p.lon << std::endl;
     if (!p.isValid()) { return false; }
 
     // First Point
     if (this->route->getRouteInfo().totalPoints == 0) {
-        std::cout << "Navigation::addCurrentPosToRoute 2" << std::endl;
         this->route->addPointToRoute(p);
         this->lastPoint = p;
         return true;
@@ -93,7 +90,6 @@ bool Navigation::addCurrentPosToRoute() {
 
     // Ever Point after the first
     if (MIN_DISTANCE_BETWEEN_POINTS <= this->distanceBetwenn(p, this->lastPoint)) {
-        std::cout << "Navigation::addCurrentPosToRoute 3" << std::endl;
         this->route->addPointToRoute(p);
         this->lastPoint = p;
         return true;

src/SpecialMenus/Akku/menuAkku.cpp

@@ -0,0 +1,69 @@
+/**
+ * @file menuAkku.cpp
+ * @author Alexander Klein (alex@kleiax.de)
+ * @brief 
+ * @version 0.1
+ * @date 2022-02-05
+ * 
+ * @copyright Copyright (c) 2022
+ * 
+ */
+
+#include "menuAkku.h"
+
+MenuAkku::MenuAkku() {
+
+}
+
+void MenuAkku::right() {
+    this->printMenu();
+}
+
+void MenuAkku::left() {
+    if (this->parentMenu)
+        this->parentMenu->printMenu();
+}
+
+void MenuAkku::no() {
+    this->left();
+}
+
+void MenuAkku::yes() {
+    this->right();
+}
+
+void MenuAkku::printMenu() {
+    char buf[17];
+    uint8_t controllerBattery = -1;
+    uint8_t mainBattery = 77;
+
+    if( controllerBattery != Ps3.data.status.battery ){
+        controllerBattery = Ps3.data.status.battery;
+    }
+
+    if( controllerBattery == ps3_status_battery_charging )      sprintf(buf, "LOAD");
+    else if( controllerBattery == ps3_status_battery_full )     sprintf(buf, "FULL");
+    else if( controllerBattery == ps3_status_battery_high )     sprintf(buf, "HIGH");
+    else if( controllerBattery == ps3_status_battery_low)       sprintf(buf, "LOW");
+    else if( controllerBattery == ps3_status_battery_dying )    sprintf(buf, "BAD");
+    else if( controllerBattery == ps3_status_battery_shutdown ) sprintf(buf, "OFF");
+    else sprintf(buf, "UNDEF");
+
+    if (this->lcd) {
+
+    } else {
+        lcd->clear();
+        lcd->setCursor(0, 0);
+        lcd->printf("Controller: %s", buf);
+        lcd->setCursor(0, 1);
+        lcd->printf("Main: %u%%", mainBattery);
+    }
+}
+
+void MenuAkku::update() {
+    if (millis() - this->last_millis < this->delay) {
+        return;
+    }
+    this->printMenu();
+    this->last_millis = millis();
+}

src/SpecialMenus/Akku/menuAkku.h

@@ -0,0 +1,39 @@
+/**
+ * @file menuAkku.h
+ * @author Alexander Klein (alex@kleiax.de)
+ * @brief 
+ * @version 0.1
+ * @date 2022-02-05
+ * 
+ * @copyright Copyright (c) 2022
+ * 
+ */
+
+#ifndef MENU_AKKU_H
+#define MENU_AKKU_H
+
+#include <Ps3Controller.h>
+
+#include "menuControl.h"
+
+class MenuAkku : public MenuControl {
+    public:
+        MenuAkku();
+
+        void down(){}
+        void up(){}
+        void right();
+        void left();
+        void no();
+        void yes();
+
+        void printMenu();
+        void update();
+
+    private:
+        const uint16_t delay = 5000;
+        uint32_t last_millis = 0;
+
+};
+
+#endif // MENU_AKKU_H

src/SpecialMenus/Autopilot/menuAutopilot.cpp

@@ -1,25 +0,0 @@
-/**
- * @file menuAutopilot.cpp
- * @author Alexander Klein (alex@kleiax.de)
- * @brief 
- * @version 0.1
- * @date 2022-02-03
- * 
- * @copyright Copyright (c) 2022
- * 
- */
-
-#include "menuAutopilot.h"
-
-void MenuAutopilot::printMenu() {
-    this->driveManager->changeModus(Modi::Autopilot);
-    if (this->lcd) {
-        this->lcd->clear();
-        this->lcd->setCursor(0, 0);
-        this->lcd->print("Autopilot");
-        this->lcd->setCursor(0, 1);
-        this->lcd->print("du fahren. :-)");
-
-    } else
-        std::cout << "Autopilot du\n fahren. :-)" << std::endl;
-}

src/SpecialMenus/PID/menuPidSettings.h

@@ -16,9 +16,6 @@
 #include <PID_v1.h>
 #include <stdint.h>
 
-// TODO: delete after installtion of display
-#include <Arduino.h>
-
 #define NUM_VAL 3
 
 class MenuPidSettings : public MenuControl {

src/SpecialMenus/driveModi/Autopilot/menuAutopilot.cpp

@@ -0,0 +1,94 @@
+/**
+ * @file menuAutopilot.cpp
+ * @author Alexander Klein (alex@kleiax.de)
+ * @brief 
+ * @version 0.1
+ * @date 2022-02-03
+ * 
+ * @copyright Copyright (c) 2022
+ * 
+ */
+
+#include "menuAutopilot.h"
+
+void MenuAutopilot::printMenu() {
+    if (this->firstPrint)
+        this->init();
+
+    char buf1[17];
+    char buf2[17];
+    char unit[3];
+
+    bool navigationStarted = this->autopilot->getNavigationStarted();
+    bool navigationEnded = this->autopilot->getNavigationEnded();
+    bool selfDriving = this->autopilot->getSelfDriving();
+    bool selfDrivingAvailable = this->autopilot->getSelfDrivingAvailable();
+
+    // 999m -360°
+    CourseCorrection correction = this->autopilot->getCourseCorrection();
+    uint16_t distance = 0;
+
+    if (correction.distance < 1) {
+        distance = (uint16_t) (correction.distance * 100);
+        sprintf(unit, "cm");
+    } else if (correction.distance < 1000) {
+        distance = (uint16_t) correction.distance;
+        sprintf(unit, "m");
+    } else {
+        distance = (uint16_t) (correction.distance / 1000);
+        sprintf(unit, "km");
+    }
+
+    if (navigationStarted) {
+        sprintf(buf1 ,"Points %u/%u",this->routeInfo.currentPoint ,this->routeInfo.totalPoints);
+        if (selfDriving) {
+            sprintf(buf2, "Auto: %u%s %d", distance, unit, correction.correction);
+        } else {
+            if (selfDrivingAvailable) {
+                sprintf(buf2, "Self -> Auto");
+            } else {
+                sprintf(buf2, "Self: %u%s %d", distance, unit, correction.correction);
+            }
+        }
+    } else {
+        if (navigationEnded) {
+            sprintf(buf1, "Am Ziel");
+            sprintf(buf2, "angekommen.");
+        } else {
+            sprintf(buf1, "Keine Route");
+            sprintf(buf2, "vorhanden.");
+        }
+    } 
+        if (!navigationStarted && !navigationEnded){
+        
+    }
+        if (this->lcd) {
+            this->lcd->clear();
+            this->lcd->setCursor(0, 0);
+            this->lcd->print(buf1);
+            this->lcd->setCursor(0, 1);
+            this->lcd->print(buf2);
+
+        } else
+            std::cout << buf1 << " " << buf2 << std::endl;
+    
+}
+
+void MenuAutopilot::update() {
+    if (millis() - this->last_millis < this->minDelay)
+        return;
+    this->last_millis = millis();
+
+    if (!this->autopilot->shouldUpdate())
+        return;
+
+    this->routeInfo = this->autopilot->getRouteInfo();
+    this->printMenu();
+}
+
+void MenuAutopilot::init() {
+    this->firstPrint = false;
+    this->driveManager->changeModus(Modi::Autopilot);
+    this->autopilot = (Autopilot*) this->driveManager->getDriveModiPtr();
+    this->routeInfo = this->autopilot->getRouteInfo();
+}

src/SpecialMenus/driveModi/Autopilot/menuAutopilot.h

@@ -12,12 +12,22 @@
 #ifndef MENU_AUTOPILOT_DRIVE_H
 #define MENU_AUTOPILOT_DRIVE_H
 
-#include "SpecialMenus/menuDriveMode.h"
+#include "SpecialMenus/driveModi/menuDriveMode.h"
 
 class MenuAutopilot : public MenuDriveMode {
     public:
         MenuAutopilot(DriveManager* driveManager) : MenuDriveMode(driveManager) {}
         void printMenu();
+        void update();
+
+    private:
+        void init();
+
+        Autopilot* autopilot;
+        RouteInfo routeInfo;
+
+        uint32_t last_millis = 0;
+        uint16_t minDelay = 500;
 };
 
 #endif // MENU_AUTOPILOT_DRIVE_H

src/SpecialMenus/driveModi/CaptureRoute/menuCaptureRoute.cpp

@@ -10,11 +10,6 @@
  */
 #include "menuCaptureRoute.h"
 
-MenuCaptureRoute::MenuCaptureRoute(DriveManager* driveManager)
-    : MenuDriveMode(driveManager) {
-        
-    }
-
 void MenuCaptureRoute::printMenu() {
     if (this->firstPrint)
         this->init();
@@ -31,11 +26,10 @@ void MenuCaptureRoute::printMenu() {
 }
 
 void MenuCaptureRoute::update() {
-    if (millis() - this->last_millis < this->delay && !this->captureRoute->shouldUpdate())
+    if (!this->captureRoute->shouldUpdate())
         return;
     this->routeInfo = this->captureRoute->getRouteInfo();
     this->printMenu();
-    this->last_millis = millis();
 }
 
 void MenuCaptureRoute::init() {

src/SpecialMenus/driveModi/CaptureRoute/menuCaptureRoute.h

@@ -11,11 +11,11 @@
 #ifndef MENU_CAPTURE_ROUTE_H
 #define MENU_MANUAL_DRIVE_H
 
-#include "SpecialMenus/menuDriveMode.h"
+#include "SpecialMenus/driveModi/menuDriveMode.h"
 
 class MenuCaptureRoute : public MenuDriveMode {
     public:
-        MenuCaptureRoute(DriveManager* driveManager);
+        MenuCaptureRoute(DriveManager* driveManager) : MenuDriveMode(driveManager) {}
         void printMenu();
         void update();
 
@@ -24,9 +24,6 @@ class MenuCaptureRoute : public MenuDriveMode {
 
         CaptureRoute* captureRoute;
         RouteInfo routeInfo;
-
-        uint32_t last_millis = 0;
-        const uint16_t delay = 5000;
 };
 
 #endif // MENU_MANUAL_DRIVE_H

src/SpecialMenus/driveModi/ManualDrive/menuManualDrive.h

@@ -12,7 +12,7 @@
 #ifndef MENU_MANUAL_DRIVE_H
 #define MENU_MANUAL_DRIVE_H
 
-#include "SpecialMenus/menuDriveMode.h"
+#include "SpecialMenus/driveModi/menuDriveMode.h"
 
 class MenuManualControl : public MenuDriveMode {
     public:

src/driveModi/Modi/Autopilot/autopilot.cpp

@@ -15,33 +15,65 @@ Autopilot::Autopilot(MoveControl* moveControl, Navigation* navigation)
     : ManualControl(moveControl) {
         this->navigation = navigation;
         this->navigationStarted = this->navigation->startNavigation();
+        this->navigation->getRouteInfo();
         this->courseCorrection.correction = 0;
         this->courseCorrection.distance = 0;
+        this->updateDisplay = true;
 }
 
 void Autopilot::loop() {
-    if (!this->selfDriving)
+    if (!this->selfDriving && this->navigationStarted)
         ManualControl::loop();
 
-    if (millis() - this->last_millis < delay) {
+    if (millis() - this->last_millis < delay)
         return;
-    }
+
     this->runAutopilot();
     this->last_millis = millis();
 }
 
 void Autopilot::runAutopilot() {
+    if (!this->navigationStarted || this->navigationEnded)
+        return;
+
     this->courseCorrection = this->navigation->getCourseCorrection();
+    this->routeInfo = this->navigation->getRouteInfo();
+    this->updateDisplay = true;
+
+    // Check if start Point is near to current Location
+    if (this->routeInfo.currentPoint >= 2)
+        this->selfDrivingAvailable = true;
+
+    if (!this->selfDriving && this->selfDrivingAvailable)
+        this->setSelfDriving(Ps3.data.button.triangle);
+
+    if (this->routeInfo.currentPoint == this->routeInfo.totalPoints) {
+        this->navigationEnded = true;
+        this->navigationStarted = false;
+        this->selfDrivingAvailable = false;
+        this->setSelfDriving(false);
+    }
+
     if (selfDriving) {
         this->setSpeedInRelToDistance(this->courseCorrection.distance);
         this->setRotInRelToDistance(this->courseCorrection.correction);
     }   
 }
 
+bool Autopilot::shouldUpdate() {
+    if (this->updateDisplay) {
+        this->updateDisplay = false;
+        return true;
+    }
+    return false;
+}
+
 void Autopilot::setSelfDriving(bool val) {
     if (!this->navigationStarted)
         return;
 
+    this->updateDisplay = true;
+
     this->selfDriving = val;
     this->moveControl->setSpeed(0);
     this->moveControl->setRotationspeed(0);

src/driveModi/Modi/Autopilot/autopilot.h

@@ -24,6 +24,14 @@ class Autopilot : public ManualControl {
         void loop();
         void runAutopilot();
 
+        RouteInfo getRouteInfo() const { return this->routeInfo; }
+        CourseCorrection getCourseCorrection() const { return this->courseCorrection; }
+        bool getNavigationStarted() const { return this->navigationStarted; }
+        bool getNavigationEnded() const { return this->navigationEnded; }
+        bool getSelfDriving() const { return this->selfDriving; }
+        bool getSelfDrivingAvailable() const { return this->selfDrivingAvailable; }
+        
+        bool shouldUpdate();
 
     private:
         void setSelfDriving(bool val);
@@ -32,9 +40,13 @@ class Autopilot : public ManualControl {
 
         Navigation* navigation;
         CourseCorrection courseCorrection;
+        RouteInfo routeInfo;
 
         bool navigationStarted = false;
+        bool navigationEnded = false;
         bool selfDriving = false;
+        bool selfDrivingAvailable = false;
+        bool updateDisplay = false;
 
         uint16_t last_millis = 0;
         uint8_t delay = 40;

src/main.cpp

@@ -14,11 +14,11 @@
 
 #include "menu.h"
 #include "menuAction.h"
-#include "SpecialMenus/menuPidSettings.h"
-#include "SpecialMenus/ManualDrive/menuManualDrive.h"
-#include "SpecialMenus/CaptureRoute/menuCaptureRoute.h"
-#include "SpecialMenus/Autopilot/menuAutopilot.h"
-#include "SpecialMenus/menuGPS.h"
+#include "SpecialMenus/PID/menuPidSettings.h"
+#include "SpecialMenus/driveModi/ManualDrive/menuManualDrive.h"
+#include "SpecialMenus/driveModi/CaptureRoute/menuCaptureRoute.h"
+#include "SpecialMenus/driveModi/Autopilot/menuAutopilot.h"
+#include "SpecialMenus/GPS/menuGPS.h"
 
 MoveControl moveController;
 DriveManager driveManager(&moveController);