From aa9bf99349cfa0ba89cac99e1c73b2230b89294a Mon Sep 17 00:00:00 2001 From: Alexander Klein Date: Sat, 5 Feb 2022 13:46:52 +0100 Subject: [PATCH] refactor, added akku menu, finished autopilot menu --- lib/Navigation/navigation.cpp | 4 - src/SpecialMenus/Akku/menuAkku.cpp | 69 ++++++++++++++ src/SpecialMenus/Akku/menuAkku.h | 39 ++++++++ src/SpecialMenus/Autopilot/menuAutopilot.cpp | 25 ----- src/SpecialMenus/{ => GPS}/menuGPS.cpp | 0 src/SpecialMenus/{ => GPS}/menuGPS.h | 0 .../{ => PID}/menuPidSettings.cpp | 0 src/SpecialMenus/{ => PID}/menuPidSettings.h | 3 - .../driveModi/Autopilot/menuAutopilot.cpp | 94 +++++++++++++++++++ .../{ => driveModi}/Autopilot/menuAutopilot.h | 12 ++- .../CaptureRoute/menuCaptureRoute.cpp | 8 +- .../CaptureRoute/menuCaptureRoute.h | 7 +- .../ManualDrive/menuManualDrive.cpp | 0 .../ManualDrive/menuManualDrive.h | 2 +- .../{ => driveModi}/menuDriveMode.cpp | 0 .../{ => driveModi}/menuDriveMode.h | 0 src/driveModi/Modi/Autopilot/autopilot.cpp | 40 +++++++- src/driveModi/Modi/Autopilot/autopilot.h | 12 +++ src/main.cpp | 10 +- 19 files changed, 270 insertions(+), 55 deletions(-) create mode 100644 src/SpecialMenus/Akku/menuAkku.cpp create mode 100644 src/SpecialMenus/Akku/menuAkku.h delete mode 100644 src/SpecialMenus/Autopilot/menuAutopilot.cpp rename src/SpecialMenus/{ => GPS}/menuGPS.cpp (100%) rename src/SpecialMenus/{ => GPS}/menuGPS.h (100%) rename src/SpecialMenus/{ => PID}/menuPidSettings.cpp (100%) rename src/SpecialMenus/{ => PID}/menuPidSettings.h (91%) create mode 100644 src/SpecialMenus/driveModi/Autopilot/menuAutopilot.cpp rename src/SpecialMenus/{ => driveModi}/Autopilot/menuAutopilot.h (64%) rename src/SpecialMenus/{ => driveModi}/CaptureRoute/menuCaptureRoute.cpp (80%) rename src/SpecialMenus/{ => driveModi}/CaptureRoute/menuCaptureRoute.h (73%) rename src/SpecialMenus/{ => driveModi}/ManualDrive/menuManualDrive.cpp (100%) rename src/SpecialMenus/{ => driveModi}/ManualDrive/menuManualDrive.h (89%) rename src/SpecialMenus/{ => driveModi}/menuDriveMode.cpp (100%) rename src/SpecialMenus/{ => driveModi}/menuDriveMode.h (100%) diff --git a/lib/Navigation/navigation.cpp b/lib/Navigation/navigation.cpp index 415b191..ee999f9 100644 --- a/lib/Navigation/navigation.cpp +++ b/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; diff --git a/src/SpecialMenus/Akku/menuAkku.cpp b/src/SpecialMenus/Akku/menuAkku.cpp new file mode 100644 index 0000000..a30b0d5 --- /dev/null +++ b/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(); +} \ No newline at end of file diff --git a/src/SpecialMenus/Akku/menuAkku.h b/src/SpecialMenus/Akku/menuAkku.h new file mode 100644 index 0000000..9782bb7 --- /dev/null +++ b/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 + +#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 \ No newline at end of file diff --git a/src/SpecialMenus/Autopilot/menuAutopilot.cpp b/src/SpecialMenus/Autopilot/menuAutopilot.cpp deleted file mode 100644 index aaae2af..0000000 --- a/src/SpecialMenus/Autopilot/menuAutopilot.cpp +++ /dev/null @@ -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; -} \ No newline at end of file diff --git a/src/SpecialMenus/menuGPS.cpp b/src/SpecialMenus/GPS/menuGPS.cpp similarity index 100% rename from src/SpecialMenus/menuGPS.cpp rename to src/SpecialMenus/GPS/menuGPS.cpp diff --git a/src/SpecialMenus/menuGPS.h b/src/SpecialMenus/GPS/menuGPS.h similarity index 100% rename from src/SpecialMenus/menuGPS.h rename to src/SpecialMenus/GPS/menuGPS.h diff --git a/src/SpecialMenus/menuPidSettings.cpp b/src/SpecialMenus/PID/menuPidSettings.cpp similarity index 100% rename from src/SpecialMenus/menuPidSettings.cpp rename to src/SpecialMenus/PID/menuPidSettings.cpp diff --git a/src/SpecialMenus/menuPidSettings.h b/src/SpecialMenus/PID/menuPidSettings.h similarity index 91% rename from src/SpecialMenus/menuPidSettings.h rename to src/SpecialMenus/PID/menuPidSettings.h index 8109534..5452db4 100644 --- a/src/SpecialMenus/menuPidSettings.h +++ b/src/SpecialMenus/PID/menuPidSettings.h @@ -16,9 +16,6 @@ #include #include -// TODO: delete after installtion of display -#include - #define NUM_VAL 3 class MenuPidSettings : public MenuControl { diff --git a/src/SpecialMenus/driveModi/Autopilot/menuAutopilot.cpp b/src/SpecialMenus/driveModi/Autopilot/menuAutopilot.cpp new file mode 100644 index 0000000..8b6631e --- /dev/null +++ b/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(); +} \ No newline at end of file diff --git a/src/SpecialMenus/Autopilot/menuAutopilot.h b/src/SpecialMenus/driveModi/Autopilot/menuAutopilot.h similarity index 64% rename from src/SpecialMenus/Autopilot/menuAutopilot.h rename to src/SpecialMenus/driveModi/Autopilot/menuAutopilot.h index ad857a1..f02aff6 100644 --- a/src/SpecialMenus/Autopilot/menuAutopilot.h +++ b/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 \ No newline at end of file diff --git a/src/SpecialMenus/CaptureRoute/menuCaptureRoute.cpp b/src/SpecialMenus/driveModi/CaptureRoute/menuCaptureRoute.cpp similarity index 80% rename from src/SpecialMenus/CaptureRoute/menuCaptureRoute.cpp rename to src/SpecialMenus/driveModi/CaptureRoute/menuCaptureRoute.cpp index fc80893..562d043 100644 --- a/src/SpecialMenus/CaptureRoute/menuCaptureRoute.cpp +++ b/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() { diff --git a/src/SpecialMenus/CaptureRoute/menuCaptureRoute.h b/src/SpecialMenus/driveModi/CaptureRoute/menuCaptureRoute.h similarity index 73% rename from src/SpecialMenus/CaptureRoute/menuCaptureRoute.h rename to src/SpecialMenus/driveModi/CaptureRoute/menuCaptureRoute.h index 8c3e96a..42491e0 100644 --- a/src/SpecialMenus/CaptureRoute/menuCaptureRoute.h +++ b/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 \ No newline at end of file diff --git a/src/SpecialMenus/ManualDrive/menuManualDrive.cpp b/src/SpecialMenus/driveModi/ManualDrive/menuManualDrive.cpp similarity index 100% rename from src/SpecialMenus/ManualDrive/menuManualDrive.cpp rename to src/SpecialMenus/driveModi/ManualDrive/menuManualDrive.cpp diff --git a/src/SpecialMenus/ManualDrive/menuManualDrive.h b/src/SpecialMenus/driveModi/ManualDrive/menuManualDrive.h similarity index 89% rename from src/SpecialMenus/ManualDrive/menuManualDrive.h rename to src/SpecialMenus/driveModi/ManualDrive/menuManualDrive.h index 704e6c4..0d4bffe 100644 --- a/src/SpecialMenus/ManualDrive/menuManualDrive.h +++ b/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: diff --git a/src/SpecialMenus/menuDriveMode.cpp b/src/SpecialMenus/driveModi/menuDriveMode.cpp similarity index 100% rename from src/SpecialMenus/menuDriveMode.cpp rename to src/SpecialMenus/driveModi/menuDriveMode.cpp diff --git a/src/SpecialMenus/menuDriveMode.h b/src/SpecialMenus/driveModi/menuDriveMode.h similarity index 100% rename from src/SpecialMenus/menuDriveMode.h rename to src/SpecialMenus/driveModi/menuDriveMode.h diff --git a/src/driveModi/Modi/Autopilot/autopilot.cpp b/src/driveModi/Modi/Autopilot/autopilot.cpp index 41eb07f..d7c20d5 100644 --- a/src/driveModi/Modi/Autopilot/autopilot.cpp +++ b/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); @@ -68,4 +100,4 @@ void Autopilot::setRotInRelToDistance(int16_t course) { 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 bcbf1ec..28d6464 100644 --- a/src/driveModi/Modi/Autopilot/autopilot.h +++ b/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; diff --git a/src/main.cpp b/src/main.cpp index 41e1ba9..e5af01b 100644 --- a/src/main.cpp +++ b/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);