From bc37abcb6630d4e2df770f5d1ba7cceba1cca57d Mon Sep 17 00:00:00 2001 From: Alexander Klein Date: Thu, 3 Feb 2022 19:50:48 +0100 Subject: [PATCH] ready to test capture Route and Autopiloz --- doc/PinBelegungen.txt | 14 ++--- lib/Navigation/navigation.cpp | 14 ++++- lib/Navigation/navigation.h | 5 +- lib/Navigation/route.cpp | 9 ++++ lib/Navigation/route.h | 9 +++- .../menuAutopilot.cpp} | 15 +++--- src/SpecialMenus/Autopilot/menuAutopilot.h | 23 ++++++++ .../CaptureRoute/menuCaptureRoute.cpp | 46 ++++++++++++++++ .../{ => CaptureRoute}/menuCaptureRoute.h | 14 ++++- .../{ => ManualDrive}/menuManualDrive.cpp | 0 .../{ => ManualDrive}/menuManualDrive.h | 2 +- src/SpecialMenus/menuDriveMode.cpp | 1 + src/SpecialMenus/menuDriveMode.h | 1 + src/driveModi/Modi/Autopilot/autopilot.cpp | 53 ++++++++++++++++++- src/driveModi/Modi/Autopilot/autopilot.h | 19 +++++-- .../Modi/CaptureRoute/captureRoute.cpp | 11 ++++ .../Modi/CaptureRoute/captureRoute.h | 7 ++- .../Modi/ManualControl/manualControl.h | 3 +- src/driveModi/driveManager.cpp | 5 +- src/main.cpp | 14 ++--- 20 files changed, 226 insertions(+), 39 deletions(-) rename src/SpecialMenus/{menuCaptureRoute.cpp => Autopilot/menuAutopilot.cpp} (51%) create mode 100644 src/SpecialMenus/Autopilot/menuAutopilot.h create mode 100644 src/SpecialMenus/CaptureRoute/menuCaptureRoute.cpp rename src/SpecialMenus/{ => CaptureRoute}/menuCaptureRoute.h (54%) rename src/SpecialMenus/{ => ManualDrive}/menuManualDrive.cpp (100%) rename src/SpecialMenus/{ => ManualDrive}/menuManualDrive.h (91%) diff --git a/doc/PinBelegungen.txt b/doc/PinBelegungen.txt index c423d79..628d56e 100644 --- a/doc/PinBelegungen.txt +++ b/doc/PinBelegungen.txt @@ -4,14 +4,14 @@ ESP32 Rover Pinbelegung x 23 DirR1 x 22 PWMR x TX PC - x RX PC -AKKU 35 21 SDA -INTL1 32 GND -INTL2 33 19 SCL -INTB1 25 18 + x RX PC Connector I2C +AKKU 35 21 SDA SDA +INTL1 32 GND GND +INTL2 33 19 SCL SCL +INTB1 25 18 VCC INTB2 26 5 -DirL1 27 17 GPS TX -DirR2 14 16 GPS RX +DirL1 27 17 GPS RX +DirR2 14 16 GPS TX DirL2 12 4 SD-Card GND x PWML 13 2 SD-Card diff --git a/lib/Navigation/navigation.cpp b/lib/Navigation/navigation.cpp index f298ca4..d00143b 100644 --- a/lib/Navigation/navigation.cpp +++ b/lib/Navigation/navigation.cpp @@ -11,13 +11,19 @@ #include "navigation.h" -Navigation::Navigation(uint8_t rx, uint8_t tx) { +Navigation::Navigation(uint8_t rx, uint8_t tx, Route* route) { this->gps = new TinyGPSPlus; Serial1.begin(9600, SERIAL_8N1, rx, tx); + if (route) + this->route = route; + else + this->route = new Route(); } Navigation::~Navigation() { delete this->gps; + if (this->route) + delete this->route; } void Navigation::loop() { @@ -25,6 +31,12 @@ void Navigation::loop() { gps->encode(Serial1.read()); } +void Navigation::newRoute() { + if (this->route) + delete this->route; + this->route = new Route(); +} + bool Navigation::startNavigation() { Point newTargetPoint = this->route->startRoute(); this->navigationStarted = this->setTargetPoint(newTargetPoint); diff --git a/lib/Navigation/navigation.h b/lib/Navigation/navigation.h index 8c504cf..c01263f 100644 --- a/lib/Navigation/navigation.h +++ b/lib/Navigation/navigation.h @@ -26,7 +26,7 @@ struct CourseCorrection { }; class Navigation { public: - Navigation(uint8_t rx, uint8_t tx); + Navigation(uint8_t rx, uint8_t tx, Route* route = nullptr); ~Navigation(); void loop(); @@ -38,6 +38,7 @@ class Navigation { bool addCurrentPosToRoute(); TinyGPSPlus* getGPS() { return this->gps; } + RouteInfo getRouteInfo() const { return this->route->getRouteInfo(); } private: Point currentLocation(); @@ -47,7 +48,7 @@ class Navigation { static double courseTo(Point p1, Point p2); TinyGPSPlus* gps; - Route* route; + Route* route = nullptr; Point lastPoint; Point targetPoint; diff --git a/lib/Navigation/route.cpp b/lib/Navigation/route.cpp index 346075c..d9af119 100644 --- a/lib/Navigation/route.cpp +++ b/lib/Navigation/route.cpp @@ -23,14 +23,23 @@ Point Route::startRoute() { if (this->points.size() < 1) return Point(0, 0); *this->it = this->points.begin(); + this->currentPoint = 1; return **this->it; } Point Route::getNextPoint() { if (*this->it != this->points.end()) { this->it++; + this->currentPoint++; return **this->it; } else return Point(0, 0); +} + +RouteInfo Route::getRouteInfo() { + RouteInfo info; + info.totalPoints = this->points.size(); + info.currentPoint = this->currentPoint; + return info; } \ No newline at end of file diff --git a/lib/Navigation/route.h b/lib/Navigation/route.h index 4442f09..e1df5db 100644 --- a/lib/Navigation/route.h +++ b/lib/Navigation/route.h @@ -31,6 +31,11 @@ class Point{ bool isValid() const { return this->lat + this->lon; } }; +struct RouteInfo{ + uint16_t currentPoint; + uint16_t totalPoints; +}; + class Route { public: Route(); @@ -38,10 +43,10 @@ class Route { Point startRoute(); Point getNextPoint(); - uint16_t getNumberOfPoints() { return this->count_points; } + RouteInfo getRouteInfo(); private: - uint16_t count_points = 0; + uint16_t currentPoint = 0; std::list points; std::list::iterator* it; diff --git a/src/SpecialMenus/menuCaptureRoute.cpp b/src/SpecialMenus/Autopilot/menuAutopilot.cpp similarity index 51% rename from src/SpecialMenus/menuCaptureRoute.cpp rename to src/SpecialMenus/Autopilot/menuAutopilot.cpp index 8fb6928..aaae2af 100644 --- a/src/SpecialMenus/menuCaptureRoute.cpp +++ b/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; } \ No newline at end of file diff --git a/src/SpecialMenus/Autopilot/menuAutopilot.h b/src/SpecialMenus/Autopilot/menuAutopilot.h new file mode 100644 index 0000000..ad857a1 --- /dev/null +++ b/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 \ No newline at end of file diff --git a/src/SpecialMenus/CaptureRoute/menuCaptureRoute.cpp b/src/SpecialMenus/CaptureRoute/menuCaptureRoute.cpp new file mode 100644 index 0000000..fc80893 --- /dev/null +++ b/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(); +} \ No newline at end of file diff --git a/src/SpecialMenus/menuCaptureRoute.h b/src/SpecialMenus/CaptureRoute/menuCaptureRoute.h similarity index 54% rename from src/SpecialMenus/menuCaptureRoute.h rename to src/SpecialMenus/CaptureRoute/menuCaptureRoute.h index a0ff636..8c3e96a 100644 --- a/src/SpecialMenus/menuCaptureRoute.h +++ b/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 \ No newline at end of file diff --git a/src/SpecialMenus/menuManualDrive.cpp b/src/SpecialMenus/ManualDrive/menuManualDrive.cpp similarity index 100% rename from src/SpecialMenus/menuManualDrive.cpp rename to src/SpecialMenus/ManualDrive/menuManualDrive.cpp diff --git a/src/SpecialMenus/menuManualDrive.h b/src/SpecialMenus/ManualDrive/menuManualDrive.h similarity index 91% rename from src/SpecialMenus/menuManualDrive.h rename to src/SpecialMenus/ManualDrive/menuManualDrive.h index 6ee2ecc..704e6c4 100644 --- a/src/SpecialMenus/menuManualDrive.h +++ b/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: diff --git a/src/SpecialMenus/menuDriveMode.cpp b/src/SpecialMenus/menuDriveMode.cpp index c8361ca..88bc4f9 100644 --- a/src/SpecialMenus/menuDriveMode.cpp +++ b/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(); diff --git a/src/SpecialMenus/menuDriveMode.h b/src/SpecialMenus/menuDriveMode.h index 1f27151..d282dfe 100644 --- a/src/SpecialMenus/menuDriveMode.h +++ b/src/SpecialMenus/menuDriveMode.h @@ -30,6 +30,7 @@ class MenuDriveMode : public MenuControl { protected: DriveManager* driveManager; + bool firstPrint = true; }; #endif // MENU_DRIVE_MODI_H \ No newline at end of file diff --git a/src/driveModi/Modi/Autopilot/autopilot.cpp b/src/driveModi/Modi/Autopilot/autopilot.cpp index 5bd9589..41eb07f 100644 --- a/src/driveModi/Modi/Autopilot/autopilot.cpp +++ b/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); } \ No newline at end of file diff --git a/src/driveModi/Modi/Autopilot/autopilot.h b/src/driveModi/Modi/Autopilot/autopilot.h index 8200c93..bcbf1ec 100644 --- a/src/driveModi/Modi/Autopilot/autopilot.h +++ b/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 \ No newline at end of file diff --git a/src/driveModi/Modi/CaptureRoute/captureRoute.cpp b/src/driveModi/Modi/CaptureRoute/captureRoute.cpp index 67a0f3b..afa198b 100644 --- a/src/driveModi/Modi/CaptureRoute/captureRoute.cpp +++ b/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; } \ No newline at end of file diff --git a/src/driveModi/Modi/CaptureRoute/captureRoute.h b/src/driveModi/Modi/CaptureRoute/captureRoute.h index 1e58412..96bc10a 100644 --- a/src/driveModi/Modi/CaptureRoute/captureRoute.h +++ b/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 \ No newline at end of file diff --git a/src/driveModi/Modi/ManualControl/manualControl.h b/src/driveModi/Modi/ManualControl/manualControl.h index 9c52b69..74123d0 100644 --- a/src/driveModi/Modi/ManualControl/manualControl.h +++ b/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; diff --git a/src/driveModi/driveManager.cpp b/src/driveModi/driveManager.cpp index ee5da36..2ed9bb1 100644 --- a/src/driveModi/driveManager.cpp +++ b/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: diff --git a/src/main.cpp b/src/main.cpp index 87095bc..41e1ba9 100644 --- a/src/main.cpp +++ b/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();