- todos

- test driving

src/driveModi/Modi/TestMode/testMode.cpp

@@ -112,7 +112,7 @@ void TestMode::abortManeuver() {
 }
 
 uint8_t TestMode::getRemainingManeuverTime() const {
-    if (busy)
+    if (this->busy)
         return (uint8_t) ((this->maneuverTime - (millis() - this->actionStart)) / 1000);
     return 0;
 }
@@ -120,7 +120,7 @@ uint8_t TestMode::getRemainingManeuverTime() const {
 bool TestMode::engineInit(int16_t powerPercentage, int16_t seconds) {
     if (this->busy)
         return false;
-    if (powerPercentage >= 100 || powerPercentage <= -100)
+    if (powerPercentage > 100 || powerPercentage < -100)
         return false;
     if (seconds < 0)
         return false;

src/driveModi/Modi/TestMode/testMode.h

@@ -14,7 +14,6 @@
 #include <iostream>
 
 #include "moveControl.h"
-#include "navigation.h"
 #include "driveModi/driveModi.h"
 
 
@@ -97,7 +96,6 @@ class TestMode : public DriveModi {
         void run() override;
         bool engineInit(int16_t powerPercentage, int16_t seconds);
 
-        Navigation* navigation;
         Maneuver maneuver = Maneuver::None;
         int16_t maneuverValueOne = 0;
         int16_t maneuverValueTwo = 0;

src/moveControl.cpp

@@ -170,12 +170,12 @@ void MoveControl::calcTargetWheelSpeed() {
                \ 1 -b / \ T /   \ Xr /  */
 
     // (1 / r) * 1
-    constexpr double A = 15.82278481;
+    constexpr double A = 1.0 / (Settings::wheelDiameter / 2);
     // (1 / r) * b
-    constexpr double B = 2.096518987;
+    constexpr double B = (1.0 / (Settings::wheelDiameter / 2)) * (Settings::wheelDistance / 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);
+    this->wheelspeed_right_target = (A * this->drivingSpeeds.x + B * this->drivingSpeeds.rot);
+    this->wheelspeed_left_target  = (A * this->drivingSpeeds.x + (-B) * this->drivingSpeeds.rot);
 }
 
 void MoveControl::regulateMotors() {
@@ -208,6 +208,8 @@ void MoveControl::regulateMotors() {
 }
 
 void MoveControl::updateCurrentWheelSpeed() {
-    this->wheelspeed_left = this->left_speedometer->getSpeed();
-    this->wheelspeed_right = this->right_speedometer->getSpeed();
+    this->wheelspeed_left = this->left_speedometer->getSpeedRad();
+    this->wheelspeed_right = this->right_speedometer->getSpeedRad();
+    // this->wheelspeed_left = this->left_speedometer->getSpeed();
+    // this->wheelspeed_right = this->right_speedometer->getSpeed();
 }