finished speed and sensor menu in modes

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