finished speed and sensor menu in modes

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.

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;

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;

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;
+}

src/SpecialMenus/Speed/menuSpeed.h

@@ -13,9 +13,17 @@
 #define MENU_SPEED_H
 
 #include <menuIntInput.h>
+#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

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();
 }
 

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" <<std::endl;

src/SpecialMenus/driveModi/menuDriveMode.cpp

@@ -24,6 +24,8 @@ void MenuDriveMode::left() {
 
     this->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);
 }

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

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);
     }
 }
 

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);
 

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;

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);
     }
 }

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};
 };
 
 

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();        

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);
 }
 

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() {