From 45264c6481fd8bba88d2b9ea96e6801ef375e846 Mon Sep 17 00:00:00 2001 From: Alexander Klein Date: Fri, 29 Sep 2023 11:30:13 +0200 Subject: [PATCH] finished speed and sensor menu in modes --- doc/TODO allgemein.txt | 5 +--- include/moveControl.h | 10 +++++--- src/SpecialMenus/PID/menuPidSettings.h | 2 +- src/SpecialMenus/Speed/menuSpeed.cpp | 6 +++++ src/SpecialMenus/Speed/menuSpeed.h | 12 +++++++-- .../driveModi/ManualDrive/menuManualDrive.cpp | 1 + .../driveModi/TestMode/menuTestMode.cpp | 3 +-- src/SpecialMenus/driveModi/menuDriveMode.cpp | 19 ++++++++++++-- src/SpecialMenus/driveModi/menuDriveMode.h | 5 ++-- src/driveModi/Modi/Autopilot/autopilot.cpp | 20 +++++++-------- .../Modi/ManualControl/manualControl.cpp | 18 +++++-------- src/driveModi/Modi/TestMode/testMode.cpp | 16 ++++++------ src/driveModi/driveManager.cpp | 1 + src/driveModi/driveManager.h | 3 ++- src/driveModi/driveModi.h | 25 +++---------------- src/main.cpp | 11 +++++--- src/moveControl.cpp | 17 ++++++++----- 17 files changed, 96 insertions(+), 78 deletions(-) diff --git a/doc/TODO allgemein.txt b/doc/TODO allgemein.txt index 89e0b07..b1d53b8 100644 --- a/doc/TODO allgemein.txt +++ b/doc/TODO allgemein.txt @@ -42,10 +42,8 @@ Do later: Do now: Code: Doxygen Kommentare aktualisieren - sensor data gnss daten aktualisieren - Menübaum korrigieren, Verlinkung Sensor in Mode etc. esp und sensoren in den deepsleep - battery Prozent ausgabe wirkt komisch + magic numbers etc Latex: Anhang: Liste mit allen Komponenten und Kurzbeschreibung @@ -57,4 +55,3 @@ Latex: 3D: Rover: - Spannungsteiler neu kalibrieren. diff --git a/include/moveControl.h b/include/moveControl.h index 8c6d7b8..9fa948c 100644 --- a/include/moveControl.h +++ b/include/moveControl.h @@ -22,6 +22,10 @@ #include "debugTimes.h" #include "component.h" +struct DrivingSpeeds { + double x; + double rot; +}; /** * @brief This class manages the motors and the encoders @@ -91,6 +95,8 @@ class MoveControl : public Component { */ void setRotationSpeed(double speed); + void setSpeeds(DrivingSpeeds drivingSpeeds); + /** * @brief Set Raw Power Left * @@ -159,9 +165,7 @@ class MoveControl : public Component { PID *right_pid; Status driving_status = Status::Stop; - - double x_speed = 0; - double rotation_speed = 0; + DrivingSpeeds drivingSpeeds = {0, 0}; double wheelspeed_left_target = 0; double wheelspeed_right_target = 0; diff --git a/src/SpecialMenus/PID/menuPidSettings.h b/src/SpecialMenus/PID/menuPidSettings.h index 61d5c08..b8fbb44 100644 --- a/src/SpecialMenus/PID/menuPidSettings.h +++ b/src/SpecialMenus/PID/menuPidSettings.h @@ -29,7 +29,7 @@ class MenuPidSettings : public MenuIntInputWrapper { */ MenuPidSettings(PID* pid); - void action(int16_t* values, uint8_t length); + void action(int16_t* values, uint8_t length) override; private: PID* pid; diff --git a/src/SpecialMenus/Speed/menuSpeed.cpp b/src/SpecialMenus/Speed/menuSpeed.cpp index e267bc2..1cb1736 100644 --- a/src/SpecialMenus/Speed/menuSpeed.cpp +++ b/src/SpecialMenus/Speed/menuSpeed.cpp @@ -11,3 +11,9 @@ #include "menuSpeed.h" +void MenuSpeed::action(int16_t* values, uint8_t length) { + if (length != 2) + return; + this->speeds.x = (double) values[0] / 10.0; + this->speeds.rot = (double) values[1] / 10.0; +} diff --git a/src/SpecialMenus/Speed/menuSpeed.h b/src/SpecialMenus/Speed/menuSpeed.h index 65b3ccd..2bdf127 100644 --- a/src/SpecialMenus/Speed/menuSpeed.h +++ b/src/SpecialMenus/Speed/menuSpeed.h @@ -13,9 +13,17 @@ #define MENU_SPEED_H #include +#include "moveControl.h" + +class MenuSpeed : public MenuIntInputWrapper { + public: + MenuSpeed(DrivingSpeeds& speeds) : speeds(speeds){}; + + void action(int16_t* values, uint8_t length) override; + + private: + DrivingSpeeds& speeds; -class MenuSpeed : public MenuIntInput { - }; #endif // MENU_SPEED_H diff --git a/src/SpecialMenus/driveModi/ManualDrive/menuManualDrive.cpp b/src/SpecialMenus/driveModi/ManualDrive/menuManualDrive.cpp index 72bdeef..7f3cb5c 100644 --- a/src/SpecialMenus/driveModi/ManualDrive/menuManualDrive.cpp +++ b/src/SpecialMenus/driveModi/ManualDrive/menuManualDrive.cpp @@ -56,6 +56,7 @@ void MenuManualControl::init() { this->driveManager->changeModus(this->manualControl); this->setCountPages(4); this->updateDelay = 500; + this->activateSpeedMenu(); MenuDriveMode::init(); } diff --git a/src/SpecialMenus/driveModi/TestMode/menuTestMode.cpp b/src/SpecialMenus/driveModi/TestMode/menuTestMode.cpp index 4d5d76c..f5157df 100644 --- a/src/SpecialMenus/driveModi/TestMode/menuTestMode.cpp +++ b/src/SpecialMenus/driveModi/TestMode/menuTestMode.cpp @@ -77,8 +77,7 @@ void MenuTestMode::init() { this->testMode = new TestMode(); this->driveManager->changeModus(this->testMode); - testMode->setMaxSpeed(1); - testMode->setMaxRotation(8); + this->testMode->setSpeeds(DrivingSpeeds{1, 8}); auto dummy = []() { std::cout << "Dummy in Action" <firstPrint = true; this->configureOnLeave(); + if (this->menuSpeed) + delete this->menuSpeed; this->driveManager->changeModus(); MenuInformationSites::left(); } @@ -85,6 +87,20 @@ void MenuDriveMode::init() { this->activeMenu = nullptr; } +void MenuDriveMode::activateSpeedMenu() { + if (!this->driveManager->isActive()) + return; + + DrivingSpeeds& speeds = this->driveManager->getDriveModiPtr()->getSpeedsRef(); + MenuIntInput* menu = new MenuIntInput(2, new MenuSpeed(speeds)); + + menu->setMinMax(5, UINT8_MAX); + menu->setEntry(0, "x m/s e-1", driveManager->getDrivingSpeedsRef().x * 10); + menu->setEntry(1, "z rad/s e-1", driveManager->getDrivingSpeedsRef().rot * 10); + + this->addSpeedMenu(menu); +} + void MenuDriveMode::enterSpeedMenu() { this->activeMenu = this->menuSpeed; this->enterMenu(); @@ -97,9 +113,8 @@ void MenuDriveMode::enterSensorMenu() { void MenuDriveMode::enterMenu() { if (!this->activeMenu) - return + return; this->activeMenu->printMenu(); - std::cout << "Sensor Menu was theretically printed" << std::endl; this->activeMenu->setParentMenu(this); } diff --git a/src/SpecialMenus/driveModi/menuDriveMode.h b/src/SpecialMenus/driveModi/menuDriveMode.h index 82a67e1..c74861e 100644 --- a/src/SpecialMenus/driveModi/menuDriveMode.h +++ b/src/SpecialMenus/driveModi/menuDriveMode.h @@ -30,7 +30,7 @@ class MenuDriveMode : public MenuInformationSites { MenuDriveMode(DriveManager* driveManager); void addSensorMenu(MenuSensorData* menuSensor) { this->menuSensor = menuSensor; } - void addSpeedMenu(MenuSpeed* menuSpeed) { this->menuSpeed = menuSpeed; } + void addSpeedMenu(MenuIntInput* menuSpeed) { this->menuSpeed = menuSpeed; } /** * @brief Goes back to the parentMenu. @@ -48,6 +48,7 @@ class MenuDriveMode : public MenuInformationSites { protected: void init() override; virtual void configureOnLeave() {} + void activateSpeedMenu(); void enterSpeedMenu(); void enterSensorMenu(); @@ -60,6 +61,6 @@ class MenuDriveMode : public MenuInformationSites { MenuControl* activeMenu = nullptr; MenuSensorData* menuSensor = nullptr; - MenuSpeed* menuSpeed = nullptr; + MenuIntInput* menuSpeed = nullptr; }; #endif // MENU_DRIVE_MODI_H diff --git a/src/driveModi/Modi/Autopilot/autopilot.cpp b/src/driveModi/Modi/Autopilot/autopilot.cpp index 9a1da0c..49b0cf2 100644 --- a/src/driveModi/Modi/Autopilot/autopilot.cpp +++ b/src/driveModi/Modi/Autopilot/autopilot.cpp @@ -125,17 +125,13 @@ void Autopilot::init() { this->courseCorrection.correction = 0; this->courseCorrection.distance = 0; this->updateDisplay = true; - - this->maxRotationSpeed = 7; - this->maxForwardSpeed = 1.5; } void Autopilot::drive() { - this->moveControl->setRotationSpeed(0); - if (this->courseCorrection.distance >= this->minRemainingDistance) - this->moveControl->setSpeed(this->maxForwardSpeed); - else - this,moveControl->setSpeed(0); + DrivingSpeeds speeds = {0, 0}; + if (this->courseCorrection.distance >= this->minRemainingDistance) + speeds.x = this->maxSpeeds.x; + this->moveControl->setSpeeds(speeds); } void Autopilot::beginRotate() { @@ -145,11 +141,13 @@ void Autopilot::beginRotate() { this->rotationAimAzimuth = this->getSensorData()->getRealAzimuth() + this->courseCorrection.correction; this->rotationAimAzimuth = Navigation::fixDegree(this->rotationAimAzimuth); - this->moveControl->setSpeed(0); + DrivingSpeeds speeds = {0, 0}; if (this->courseCorrection.correction > 0) - this->moveControl->setRotationSpeed(-this->maxRotationSpeed); + speeds.rot = -this->maxSpeeds.rot; else - this->moveControl->setRotationSpeed(this->maxRotationSpeed); + speeds.rot = this->maxSpeeds.rot; + + this->moveControl->setSpeeds(speeds); } } diff --git a/src/driveModi/Modi/ManualControl/manualControl.cpp b/src/driveModi/Modi/ManualControl/manualControl.cpp index a59fa3f..8a81978 100644 --- a/src/driveModi/Modi/ManualControl/manualControl.cpp +++ b/src/driveModi/Modi/ManualControl/manualControl.cpp @@ -34,17 +34,11 @@ void ManualControl::analogControl() { int16_t x = this->input->x - 127; int16_t y = this->input->y - 127; - // static int counter = 0; - // if (counter % 60 == 0) { - // std::cout << "Input: x: " << (int) this->input->x << " y: " << (int) this->input->y << std::endl; - // std::cout << "Output: x: " << (int) x << " y: " << (int) y << std::endl; - // } - // counter++; - double value_per_step = this->maxForwardSpeed * 2 / UINT8_MAX; + double value_per_step = this->maxSpeeds.x * 2 / UINT8_MAX; this->moveControl->setSpeed(-x * value_per_step); - value_per_step = this->maxRotationSpeed * 2 / UINT8_MAX; + value_per_step = this->maxSpeeds.rot * 2 / UINT8_MAX; this->moveControl->setRotationSpeed(y * value_per_step); } @@ -53,16 +47,16 @@ void ManualControl::digitalControl() { int16_t x = this->input->y - 127; if (y > 120) - this->moveControl->setSpeed(-this->maxForwardSpeed); + this->moveControl->setSpeed(-this->maxSpeeds.x); else if (y < -120) - this->moveControl->setSpeed(this->maxForwardSpeed); + this->moveControl->setSpeed(this->maxSpeeds.rot); else this->moveControl->setSpeed(0); if (x > 120) - this->moveControl->setRotationSpeed(this->maxRotationSpeed); + this->moveControl->setRotationSpeed(this->maxSpeeds.rot); else if (x < -120) - this->moveControl->setRotationSpeed(-this->maxRotationSpeed); + this->moveControl->setRotationSpeed(-this->maxSpeeds.rot); else this->moveControl->setRotationSpeed(0); diff --git a/src/driveModi/Modi/TestMode/testMode.cpp b/src/driveModi/Modi/TestMode/testMode.cpp index 5f5cada..cb067d1 100644 --- a/src/driveModi/Modi/TestMode/testMode.cpp +++ b/src/driveModi/Modi/TestMode/testMode.cpp @@ -37,9 +37,9 @@ bool TestMode::drive(int16_t cm, int16_t degree) { this->moveControl->setSpeed(0); if (degree < 0) - this->moveControl->setRotationSpeed(-this->maxRotationSpeed); + this->moveControl->setRotationSpeed(-this->maxSpeeds.rot); else if (degree > 0) - this->moveControl->setRotationSpeed(this->maxRotationSpeed); + this->moveControl->setRotationSpeed(this->maxSpeeds.rot); this->azimuth = this->getSensorData()->getRealAzimuth(); this->degree = degree; @@ -53,11 +53,11 @@ bool TestMode::drive(int16_t cm, int16_t degree) { this->moveControl->setRotationSpeed(0); if (cm < 0) - this->moveControl->setSpeed(-this->maxForwardSpeed); + this->moveControl->setSpeed(-this->maxSpeeds.x); else if (cm > 0) - this->moveControl->setSpeed(this->maxForwardSpeed); + this->moveControl->setSpeed(this->maxSpeeds.x); - this->maneuverTime = (uint32_t) (((double) abs(cm) / 100.0) / this->maxForwardSpeed) * 1000; + this->maneuverTime = (uint32_t) (((double) abs(cm) / 100.0) / this->maxSpeeds.x) * 1000; this->busy = true; this->maneuver = Maneuver::Drive; return true; @@ -65,11 +65,11 @@ bool TestMode::drive(int16_t cm, int16_t degree) { //forward or backward and left or right // TODO: Calculate roationspeed if (cm < 0) - this->moveControl->setSpeed(-this->maxForwardSpeed); + this->moveControl->setSpeed(-this->maxSpeeds.x); else if (cm > 0) - this->moveControl->setSpeed(this->maxForwardSpeed); + this->moveControl->setSpeed(this->maxSpeeds.x); - this->maneuverTime = (uint32_t) (((double) cm / 100.0) / this->maxForwardSpeed) * 1000; + this->maneuverTime = (uint32_t) (((double) cm / 100.0) / this->maxSpeeds.x) * 1000; this->busy = true; this->maneuver = Maneuver::Drive; return true; diff --git a/src/driveModi/driveManager.cpp b/src/driveModi/driveManager.cpp index dc948ee..aa371b4 100644 --- a/src/driveModi/driveManager.cpp +++ b/src/driveModi/driveManager.cpp @@ -36,6 +36,7 @@ void DriveManager::changeModus(DriveModi* modus) { if (this->currentModusPtr) { this->currentModusPtr->activate(this->driveModiParams); + this->currentModusPtr->setSpeeds(this->drivingSpeeds); this->addChildComponent(this->currentModusPtr); } } diff --git a/src/driveModi/driveManager.h b/src/driveModi/driveManager.h index ab70f61..8c7f5f9 100644 --- a/src/driveModi/driveManager.h +++ b/src/driveModi/driveManager.h @@ -53,7 +53,7 @@ class DriveManager : public Component { * @return DriveModi* */ DriveModi* getDriveModiPtr() const { return this->currentModusPtr; } - + DrivingSpeeds& getDrivingSpeedsRef() { return this->drivingSpeeds; } bool isActive() const { return this->currentModusPtr; } @@ -63,6 +63,7 @@ class DriveManager : public Component { DriveModi *currentModusPtr = nullptr; DriveModiParams driveModiParams; + DrivingSpeeds drivingSpeeds = {1, 7}; }; diff --git a/src/driveModi/driveModi.h b/src/driveModi/driveModi.h index 33d9f70..adc9db5 100644 --- a/src/driveModi/driveModi.h +++ b/src/driveModi/driveModi.h @@ -39,34 +39,17 @@ class DriveModi : public Component { void activate(MoveControl* moveControl, ControlPadInput* input, SensorData* sensorData); void activate(DriveModiParams params); - /** - * @brief Set the max speed - * - * @param maxSpeed in m/s - */ - void setMaxSpeed(double maxForwardSpeed) { maxForwardSpeed = maxForwardSpeed; } - void increaseMaxSpeed(double increase = 0.1) { maxForwardSpeed += increase; } - void decreaseMaxSpeed(double increase = 0.1) { maxForwardSpeed -= increase; } - double getMaxSpeed() const { return this->maxForwardSpeed; } - - /** - * @brief Set the max rotation - * - * @param maxRotation in rad/s (maybe) - */ - void setMaxRotation(double maxRotation) { maxRotationSpeed = maxRotation; } - void increaseMaxRotation(double increase = 0.1) { maxRotationSpeed += increase; } - void decreaseMaxRotation(double increase = 0.1) { maxRotationSpeed -= increase; } - double getMaxRotation() const { return this->maxRotationSpeed; } + void setSpeeds(DrivingSpeeds speeds) { this->maxSpeeds = speeds; } + DrivingSpeeds getSpeeds() const { return this->maxSpeeds; } + DrivingSpeeds& getSpeedsRef() { return this->maxSpeeds; } protected: virtual void afterActivate() {} MoveControl *moveControl; + DrivingSpeeds maxSpeeds = {1, 7}; const ControlPadInput* input; const SensorData* sensorData; - double maxForwardSpeed = 1; - double maxRotationSpeed = 7; private: void init(); diff --git a/src/main.cpp b/src/main.cpp index 997da5c..161f8f4 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -208,6 +208,7 @@ void makeMenu() { Menu* pid_m = new Menu(); MenuIntInput* pidl_m = new MenuIntInput(3, new MenuPidSettings(moveController.getPID(0))); MenuIntInput* pidr_m = new MenuIntInput(3, new MenuPidSettings(moveController.getPID(1))); + MenuIntInput* speed_m = new MenuIntInput(2, new MenuSpeed(driveManager->getDrivingSpeedsRef())); MenuManualControl* man_m = new MenuManualControl(driveManager); MenuCaptureRoute* cap_m = new MenuCaptureRoute(driveManager); MenuAutopilot* auto_m = new MenuAutopilot(driveManager); @@ -223,8 +224,6 @@ void makeMenu() { sys_m->setUpdateDelay(1500); sen_m->setUpdateDelay(500); - man_m->addSensorMenu(sen_m); - // Entry for the main menu main_m->addEntry(new MenuAction("Mode", mode_m)); main_m->addEntry(new MenuAction("Sensor", sen_m)); @@ -243,7 +242,7 @@ void makeMenu() { // Entry for the setting menu set_m->addEntry(new MenuAction("PID", pid_m)); - set_m->addEntry(new MenuAction("Speed", dummy)); + set_m->addEntry(new MenuAction("Speed", speed_m)); set_m->addEntry(new MenuAction("Distance", dummy)); set_m->addEntry(new MenuAction("WiFi", dummy)); set_m->addEntry(new MenuAction("Battery", bat_m)); @@ -263,6 +262,12 @@ void makeMenu() { pidr_m->setEntry(1, "I-Part", (uint8_t) moveController.getPID(1)->GetKi()); pidr_m->setEntry(2, "D-Part", (uint8_t) moveController.getPID(1)->GetKd()); + speed_m->setMinMax(5, UINT8_MAX); + speed_m->setEntry(0, "x m/s e-1", driveManager->getDrivingSpeedsRef().x * 10); + speed_m->setEntry(1, "z rad/s e-1", driveManager->getDrivingSpeedsRef().rot * 10); + + man_m->addSensorMenu(sen_m); + controlPad->setMenuControl(main_m); } diff --git a/src/moveControl.cpp b/src/moveControl.cpp index eb87aa2..20c9248 100644 --- a/src/moveControl.cpp +++ b/src/moveControl.cpp @@ -105,20 +105,25 @@ void MoveControl::emergencyStop() { void MoveControl::setSpeed(double speed) { if (speed < 0.2 && speed > -0.2) { - this->x_speed = 0; + this->drivingSpeeds.x = 0; return; } - this->x_speed = speed; + this->drivingSpeeds.x = speed; } void MoveControl::setRotationSpeed(double speed) { if (speed < 0.1 && speed > -0.1){ - this->rotation_speed = 0; + this->drivingSpeeds.rot = 0; return; } - this->rotation_speed = speed; + this->drivingSpeeds.rot = speed; +} + +void MoveControl::setSpeeds(DrivingSpeeds drivingSpeeds) { + this->setSpeed(drivingSpeeds.x); + this->setRotationSpeed(drivingSpeeds.rot); } void MoveControl::setRawPowerLeft(int16_t power) { @@ -169,8 +174,8 @@ void MoveControl::calcTargetWheelSpeed() { // (1 / r) * b constexpr double B = 2.096518987; - this->wheelspeed_right_target = (A * this->x_speed + B * this->rotation_speed) * (Settings::wheelDiameter / 2); - this->wheelspeed_left_target = (A * this->x_speed + (-B) * this->rotation_speed) * (Settings::wheelDiameter / 2); + this->wheelspeed_right_target = (A * this->drivingSpeeds.x + B * this->drivingSpeeds.rot) * (Settings::wheelDiameter / 2); + this->wheelspeed_left_target = (A * this->drivingSpeeds.x + (-B) * this->drivingSpeeds.rot) * (Settings::wheelDiameter / 2); } void MoveControl::regulateMotors() {