- todos

- test driving

doc/TODO allgemein.txt

@@ -37,20 +37,24 @@ Do later:
     -> Check speration between Ui and Route (RouteMenu)
     -> update sparkfun gnss auf v3 für SPI korrekturdatenüvbetragung
     -> Rover Objekt mit Error zustand freeze, damit das wenn möglich auch auf dem Display angezeigt wird.
+    -> ESP und Sensoren in den DeepSleep für Auschalten oder Akkuschutz
 
 
 Do now:
 Code:
     Doxygen Kommentare aktualisieren
-    esp und sensoren in den deepsleep
     magic numbers etc
+    speedometer geschwindigkeit auch als rad/s
+    Menu in Lib Folder packen
+    testMode einzelner Motor testen klappt nicht
+    Fernbedienung!
 
 Latex:
     Anhang: Liste mit allen Komponenten und Kurzbeschreibung
     Unterschied funktionale und nicht funktionale Anforderungen
-    Strom nicht über Zeit sondern Rampe
-    Formel für Zeit anfahrrampe Einheiten
-    Überall den Durchmesser der Räder auf 12cm und Pulse auf 384 / 3 = 128
+    Overfull H-Boxen
+    Test Motor, Zeiten kontrolieren. Wenn einfach so dann bezug auf fehler bei Timing und freeRtos
+    Bild mit RPY einbinden
 
 3D:
     

include/config.h

@@ -35,18 +35,19 @@ namespace PinNumbers {
 }
 
 namespace Settings {
-    constexpr float wheelDiameter = 0.1263;
-    constexpr uint16_t encoderSteps = 360;
+    constexpr float wheelDiameter = 0.105;
+    constexpr float wheelDistance = 0.255;
+    constexpr uint16_t encoderSteps = 384;
 
     namespace Pid {
         namespace Left {
-            constexpr uint8_t P = 75;
+            constexpr uint8_t P = 5;
             constexpr uint8_t I = 0;
             constexpr uint8_t D = 0;
         }
 
         namespace Right {
-            constexpr uint8_t P = 75;
+            constexpr uint8_t P = 5;
             constexpr uint8_t I = 0;
             constexpr uint8_t D = 0;
         }

lib/Speedometer/speedometer.cpp

@@ -16,14 +16,14 @@ Speedometer::Speedometer(uint8_t pin, double diameter, uint16_t steps) {
     this->steps = steps;
 
     this->pulseCounter = new Counter(pin);
-    this->pulseCounter->setFilterValue(1000); // ignore pulses less than 1000 x 2.5ns
+    this->pulseCounter->setFilterValue(1023); // ignore pulses less than 1000 x 2.5ns
 
     this->pulseCounter->clear();
     this->pulseCounter->resume();
 
     Component::loopDelay = Speedometer::loopDelay;
 
-    clearAvgBuf();
+    this->clearAvgBuf();
 }
 
 Speedometer::~Speedometer() {
@@ -47,18 +47,27 @@ void Speedometer::run() {
     double n = (double)pulse / this->steps;   // Wheel revolutions in absolute time
     double u = n / ((double)elapsedTime / 1000);           // Wheel revolutions per second
     double ms = u * (diameter * PI);                      // Speed in m/s
+    double rad = u * 2 * PI;
+
+    if (speed < 0.1) {
+        speed = 0;
+        rad = 0;
+    }
 
     switch (this->currentDirection) {
         case Direction::Forward :
             this->speed = ms;
+            this->rad = rad;
             break;
         
         case Direction::Backward :
             this->speed = -ms;
+            this->rad = -rad;
             break;
 
         case Direction::None :
             this->speed = 0;
+            this->rad = 0;
             break;
     }
 

lib/Speedometer/speedometer.h

@@ -59,13 +59,6 @@ class Speedometer : public Component {
          */
         void setDirection(Direction dir);
 
-        /**
-         * @brief Set the number of last values ​​to be taken into account for the average.
-         * 
-         * @param val length of the array
-         */
-        void setNumOfValForAvg(uint8_t val);
-
         /**
          * @brief Set the Enc Filter to prevent bouncing
          * 
@@ -88,6 +81,7 @@ class Speedometer : public Component {
          * @return double speed in m/s
          */
         double getSpeed() const { return this->speed; }
+        double getSpeedRad() const { return this->rad; };
         double getAvgSpeed() const;
 
         /**
@@ -111,7 +105,6 @@ class Speedometer : public Component {
 
     private:
         void run() override;
-        void init(uint8_t pin, double diameter, uint16_t steps);
         void clearAvgBuf();
         void addValToBuf(int16_t val);
         int16_t calcAverage() const;
@@ -126,6 +119,7 @@ class Speedometer : public Component {
         bool calibrationRunning = false;
 
         double speed = 0;
+        double rad = 0;
         double diameter;
 
         uint8_t printCounter = 0;

platformio.ini

@@ -15,7 +15,7 @@ board_build.partitions = no_ota.csv
 framework = arduino
 monitor_speed = 115200
 upload_speed = 921600
-monitor_port = COM6
+monitor_port = COM3
 lib_deps = 
 	https://git.kleiax.de/PlatformIO-Libs/SparkFunGNSS.git
 	knolleary/PubSubClient@^2.8
@@ -24,9 +24,8 @@ lib_deps =
 	bblanchon/ArduinoJson@^6.20.0
 	mprograms/QMC5883LCompass@^1.2.0
 	https://git.kleiax.de/PlatformIO-Libs/Menu.git
-	nrf24/RF24@^1.4.5
 	jrowberg/I2Cdevlib-MPU6050@^1.0.0
-upload_port = COM6
+upload_port = COM3
 test_ignore = test_desktop
 build_type = debug
 monitor_filters = esp32_exception_decoder

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