diff --git a/doc/TODO allgemein.txt b/doc/TODO allgemein.txt index b1d53b8..5be3695 100644 --- a/doc/TODO allgemein.txt +++ b/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: diff --git a/include/config.h b/include/config.h index d8e60c1..cf8e055 100644 --- a/include/config.h +++ b/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; } diff --git a/lib/Speedometer/speedometer.cpp b/lib/Speedometer/speedometer.cpp index 8313efa..7132b56 100644 --- a/lib/Speedometer/speedometer.cpp +++ b/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; } diff --git a/lib/Speedometer/speedometer.h b/lib/Speedometer/speedometer.h index ae78366..a1635fb 100644 --- a/lib/Speedometer/speedometer.h +++ b/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; diff --git a/platformio.ini b/platformio.ini index c5bc22d..36de8e2 100644 --- a/platformio.ini +++ b/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 diff --git a/src/driveModi/Modi/TestMode/testMode.cpp b/src/driveModi/Modi/TestMode/testMode.cpp index cb067d1..b937a34 100644 --- a/src/driveModi/Modi/TestMode/testMode.cpp +++ b/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; diff --git a/src/driveModi/Modi/TestMode/testMode.h b/src/driveModi/Modi/TestMode/testMode.h index bae19de..1480e36 100644 --- a/src/driveModi/Modi/TestMode/testMode.h +++ b/src/driveModi/Modi/TestMode/testMode.h @@ -14,7 +14,6 @@ #include #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; diff --git a/src/moveControl.cpp b/src/moveControl.cpp index 20c9248..d7614a4 100644 --- a/src/moveControl.cpp +++ b/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(); }