ready to test capture Route and Autopiloz

src/SpecialMenus/Autopilot/menuAutopilot.cpp

@@ -1,24 +1,25 @@
 /**
- * @file menuCaptureRoute.cpp
+ * @file menuAutopilot.cpp
  * @author Alexander Klein (alex@kleiax.de)
  * @brief 
  * @version 0.1
- * @date 2022-01-31
+ * @date 2022-02-03
  * 
  * @copyright Copyright (c) 2022
  * 
  */
-#include "menuCaptureRoute.h"
 
-void MenuCaptureRoute::printMenu() {
-    this->driveManager->changeModus(Modi::CaptureRoute);
+#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("Jetzt kannst");
+        this->lcd->print("Autopilot");
         this->lcd->setCursor(0, 1);
         this->lcd->print("du fahren. :-)");
 
     } else
-        std::cout << "Jetzt kannst du\n fahren. :-)" << std::endl;
+        std::cout << "Autopilot du\n fahren. :-)" << std::endl;
 }

src/SpecialMenus/Autopilot/menuAutopilot.h

@@ -0,0 +1,23 @@
+/**
+ * @file menuAutopilot.h
+ * @author Alexander Klein (alex@kleiax.de)
+ * @brief 
+ * @version 0.1
+ * @date 2022-02-03
+ * 
+ * @copyright Copyright (c) 2022
+ * 
+ */
+
+#ifndef MENU_AUTOPILOT_DRIVE_H
+#define MENU_AUTOPILOT_DRIVE_H
+
+#include "SpecialMenus/menuDriveMode.h"
+
+class MenuAutopilot : public MenuDriveMode {
+    public:
+        MenuAutopilot(DriveManager* driveManager) : MenuDriveMode(driveManager) {}
+        void printMenu();
+};
+
+#endif // MENU_AUTOPILOT_DRIVE_H

src/SpecialMenus/CaptureRoute/menuCaptureRoute.cpp

@@ -0,0 +1,46 @@
+/**
+ * @file menuCaptureRoute.cpp
+ * @author Alexander Klein (alex@kleiax.de)
+ * @brief 
+ * @version 0.1
+ * @date 2022-01-31
+ * 
+ * @copyright Copyright (c) 2022
+ * 
+ */
+#include "menuCaptureRoute.h"
+
+MenuCaptureRoute::MenuCaptureRoute(DriveManager* driveManager)
+    : MenuDriveMode(driveManager) {
+        
+    }
+
+void MenuCaptureRoute::printMenu() {
+    if (this->firstPrint)
+        this->init();
+    
+    if (this->lcd) {
+        this->lcd->clear();
+        this->lcd->setCursor(0, 0);
+        this->lcd->printf("Points: %u", this->routeInfo.totalPoints);
+        this->lcd->setCursor(0, 1);
+        this->lcd->print("du fahren. :-)");
+
+    } else
+        std::cout << "Capture du\n fahren. :-)" << std::endl;
+}
+
+void MenuCaptureRoute::update() {
+    if (millis() - this->last_millis < this->delay && !this->captureRoute->shouldUpdate())
+        return;
+    this->routeInfo = this->captureRoute->getRouteInfo();
+    this->printMenu();
+    this->last_millis = millis();
+}
+
+void MenuCaptureRoute::init() {
+    this->firstPrint = false;
+    this->driveManager->changeModus(Modi::CaptureRoute);
+    this->captureRoute = (CaptureRoute*) this->driveManager->getDriveModiPtr();
+    this->routeInfo = this->captureRoute->getRouteInfo();
+}

src/SpecialMenus/CaptureRoute/menuCaptureRoute.h

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

src/SpecialMenus/ManualDrive/menuManualDrive.h

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

src/SpecialMenus/menuDriveMode.cpp

@@ -16,6 +16,7 @@ MenuDriveMode::MenuDriveMode(DriveManager* driveManager) {
 }
 
 void MenuDriveMode::left() {
+    this->firstPrint = true;
     this->driveManager->changeModus(Modi::Off);
     if (parentMenu)
         this->parentMenu->printMenu();

src/SpecialMenus/menuDriveMode.h

@@ -30,6 +30,7 @@ class MenuDriveMode : public MenuControl {
 
     protected:
         DriveManager* driveManager;
+        bool firstPrint = true;
 };
 
 #endif // MENU_DRIVE_MODI_H

src/driveModi/Modi/Autopilot/autopilot.cpp

@@ -11,10 +11,61 @@
 
 #include "driveModi/Modi/Autopilot/autopilot.h"
 
-Autopilot::Autopilot() {
+Autopilot::Autopilot(MoveControl* moveControl, Navigation* navigation)
+    : ManualControl(moveControl) {
+        this->navigation = navigation;
+        this->navigationStarted = this->navigation->startNavigation();
+        this->courseCorrection.correction = 0;
+        this->courseCorrection.distance = 0;
+}
 
+void Autopilot::loop() {
+    if (!this->selfDriving)
+        ManualControl::loop();
+
+    if (millis() - this->last_millis < delay) {
+        return;
+    }
+    this->runAutopilot();
+    this->last_millis = millis();
 }
 
 void Autopilot::runAutopilot() {
+    this->courseCorrection = this->navigation->getCourseCorrection();
+    if (selfDriving) {
+        this->setSpeedInRelToDistance(this->courseCorrection.distance);
+        this->setRotInRelToDistance(this->courseCorrection.correction);
+    }
+}
 
+void Autopilot::setSelfDriving(bool val) {
+    if (!this->navigationStarted)
+        return;
+
+    this->selfDriving = val;
+    this->moveControl->setSpeed(0);
+    this->moveControl->setRotationspeed(0);
+}
+
+void Autopilot::setSpeedInRelToDistance(double distance) {
+    // TODO: Delte Magic Numbers
+    if (distance > 2)
+        this->moveControl->setSpeed(1.5);
+    else if (distance < 0.5)
+        this->moveControl->setSpeed(1.5);
+    else
+        this->moveControl->setSpeed(0);
+}
+
+void Autopilot::setRotInRelToDistance(int16_t course) {
+    // TODO: Delte Magic Numbers
+    int8_t steps = course / 30;
+
+    if (steps == 0 && abs(course) >= 5)
+        steps = 1;
+
+    if (course == 0)
+        this->moveControl->setRotationspeed(0);
+    else
+        this->moveControl->setRotationspeed(steps);
 }

src/driveModi/Modi/Autopilot/autopilot.h

@@ -15,20 +15,29 @@
 
 #include "navigation.h"
 #include "moveControl.h"
-#include "driveModi/driveModi.h"
+#include "driveModi/Modi/ManualControl/manualControl.h"
 
-class Autopilot : DriveModi{
+class Autopilot : public ManualControl {
     public:
-        Autopilot();
+        Autopilot(MoveControl* moveControl, Navigation* navigation);
 
         void loop();
         void runAutopilot();
 
 
     private:
-        MoveControl* moveControl;
-        Navigation* navigation;
+        void setSelfDriving(bool val);
+        void setSpeedInRelToDistance(double distance);
+        void setRotInRelToDistance(int16_t course);
 
+        Navigation* navigation;
+        CourseCorrection courseCorrection;
+
+        bool navigationStarted = false;
+        bool selfDriving = false;
+
+        uint16_t last_millis = 0;
+        uint8_t delay = 40;
 };
 
 #endif // AUTOPILOT_H

src/driveModi/Modi/CaptureRoute/captureRoute.cpp

@@ -3,6 +3,7 @@
 CaptureRoute::CaptureRoute(MoveControl* moveControl, Navigation* navigation) 
     : ManualControl(moveControl) {
         this->navigation = navigation;
+        this->routeInfo = RouteInfo{0, 0};
 }
 
 void CaptureRoute::loop() {
@@ -18,5 +19,15 @@ void CaptureRoute::loop() {
 void CaptureRoute::runCaptureRoute() {
     if (Ps3.data.button.triangle) {
         this->navigation->addCurrentPosToRoute();
+        this->routeInfo.totalPoints++;
+        this->updateDisplay = true;
     }
+}
+
+bool CaptureRoute::shouldUpdate() {
+    if (this->updateDisplay) {
+        this->updateDisplay = false;
+        return true;
+    }
+    return false;
 }

src/driveModi/Modi/CaptureRoute/captureRoute.h

@@ -15,14 +15,17 @@ class CaptureRoute : public ManualControl {
         void runCaptureRoute();
 
         Navigation* getNavigation() const { return this->navigation; }
+        RouteInfo getRouteInfo() const { return this->routeInfo; }
 
+        bool shouldUpdate();
     private:
         Navigation* navigation;
-
-        uint16_t savedPoints = 0;
+        RouteInfo routeInfo;
 
         uint32_t last_millis = 0;
         uint16_t delay = 200;
+
+        bool updateDisplay = false;
 };
 
 #endif // CAPTURE_ROUTE_H

src/driveModi/Modi/ManualControl/manualControl.h

@@ -69,9 +69,10 @@ class ManualControl : public DriveModi{
          */
         void setMaxRotation(double maxRotation) { max_rotation = maxRotation; }
 
-    private:
+    protected:
         MoveControl *moveControl;
 
+    private:
         uint32_t last_millis = 0;
         uint8_t delay = 10;
         double max_speed = 1;

src/driveModi/driveManager.cpp

@@ -60,8 +60,9 @@ void DriveManager::changeModus(Modi modus) {
             }
             break;
 
-        case Modi::Autopilot:
-            this->currentModusPtr = nullptr;
+        case Modi::Autopilot: {
+            this->currentModusPtr = new Autopilot(this->moveControl, this->navigation);
+            }
             break;
 
         case Modi::ConsolControl:

src/main.cpp

@@ -15,8 +15,9 @@
 #include "menu.h"
 #include "menuAction.h"
 #include "SpecialMenus/menuPidSettings.h"
-#include "SpecialMenus/menuManualDrive.h"
-#include "SpecialMenus/menuCaptureRoute.h"
+#include "SpecialMenus/ManualDrive/menuManualDrive.h"
+#include "SpecialMenus/CaptureRoute/menuCaptureRoute.h"
+#include "SpecialMenus/Autopilot/menuAutopilot.h"
 #include "SpecialMenus/menuGPS.h"
 
 MoveControl moveController;
@@ -34,12 +35,11 @@ void controllerPrintBattery();
 void i2cScanner();
 void restart();
 
-void exitDriveMode() {driveManager.changeModus(Modi::Off);}
 
 void setup() {
     Serial.begin(115200);
     std::cout << "Welcome to Kleiax-Rover" << std::endl;
-    std::cout << "All actions from the main programm run on" << xPortGetCoreID() << std::endl;
+    std::cout << "All actions from the main programm run on Core -> " << xPortGetCoreID() << std::endl;
 
     Wire.begin(21, 19);
     // i2cScanner();
@@ -67,6 +67,7 @@ void setup() {
     MenuPidSettings* pidr_m = new MenuPidSettings(moveController.getPID(1));
     MenuManualControl* man_m = new MenuManualControl(&driveManager);
     MenuCaptureRoute* cap_m = new MenuCaptureRoute(&driveManager);
+    MenuAutopilot* auto_m = new MenuAutopilot(&driveManager);
     MenuGPS* gps_m = new MenuGPS(driveManager.getNavigation()->getGPS());
 
     // Set Menus on LCD
@@ -77,6 +78,7 @@ void setup() {
     pidr_m->setLcd(lcd);
     man_m->setLcd(lcd);
     cap_m->setLcd(lcd);
+    auto_m->setLcd(lcd);
     gps_m->setLcd(lcd);
 
 
@@ -91,7 +93,7 @@ void setup() {
     main_m->addEntry(restart_e);
 
     // Entry for the mode Menu
-    MenuAction* autopilot_e = new MenuAction("Autopilot", dummy);
+    MenuAction* autopilot_e = new MenuAction("Autopilot", auto_m);
     MenuAction* captureRoute_e = new MenuAction("Capture Route", cap_m);
     MenuAction* consolControl_e = new MenuAction("Consol Control", dummy);
     MenuAction* manualControl_e = new MenuAction("Manual Control", man_m);
@@ -153,7 +155,7 @@ void callbackControllerAction() {
 
 void callbackControllerConnect() {
     std::cout << "Controller connected to ESP32" << std::endl;
-    std::cout << "All actions from the Controller run on" << xPortGetCoreID() << std::endl;
+    std::cout << "All actions from the Controller run on Core -> " << xPortGetCoreID() << std::endl;
 
     // TODO: Display is not functional without this seconde init, first init is in setup()
     lcd->init();