diff --git a/lib/Navigation/navigation.cpp b/lib/Navigation/navigation.cpp index f83fe87..ba76792 100644 --- a/lib/Navigation/navigation.cpp +++ b/lib/Navigation/navigation.cpp @@ -66,16 +66,14 @@ void Navigation::init(Route* route) { Wire.write(0x01); Wire.endTransmission(); this->compass->setMode(0x01,0x0C,0x10,0X00); + this->compass->setCalibration(-1903, 367, -893, 1468, -1110, 310); } Navigation::~Navigation() { delete this->gps; - - if (this->isNtripInit) - delete this->ntripClient; - - if (this->route) - delete this->route; + delete this->compass; + delete this->ntripClient; + delete this->route; } void Navigation::initNtrip(String host, uint16_t port, String mountPoint, String user, String password) { @@ -130,7 +128,7 @@ CourseCorrection Navigation::getCourseCorrection() { return courseCorrection; // Check if I need a new Point - if (distance < MIN_DISTANCE_BETWEEN_POINTS) { + if (distance < MIN_DISTANCE_TO_REACH_POINT) { bool goOn = this->nextPoint(); if (!goOn) { this->navigationFinished = true; diff --git a/lib/Navigation/navigation.h b/lib/Navigation/navigation.h index b75cd2f..56e9bd7 100644 --- a/lib/Navigation/navigation.h +++ b/lib/Navigation/navigation.h @@ -29,6 +29,7 @@ * */ #define MIN_DISTANCE_BETWEEN_POINTS 0.1 +#define MIN_DISTANCE_TO_REACH_POINT 3 // TODO: Only for testing! #define AZIMUTH_UPDATE_DELAY 20 /** diff --git a/lib/Speedometer/speedometer.cpp b/lib/Speedometer/speedometer.cpp index 0e39653..6963cfb 100644 --- a/lib/Speedometer/speedometer.cpp +++ b/lib/Speedometer/speedometer.cpp @@ -1,7 +1,7 @@ /** * @file speedometer.cpp * @author Alexander Klein (alex@kleiax.de) - * @brief Implemention of the class speedometer.h + * @brief Implementation of the class speedometer.h * @see speedometer.h * @version 0.1 * @date 2021-12-13 @@ -12,6 +12,9 @@ #include "speedometer.h" Speedometer::Speedometer() {} +Speedometer::~Speedometer() { + delete[] this->buf; +} void Speedometer::init(uint8_t pinA, uint8_t pinB, double diameter, uint16_t steps, uint8_t numOfValForAvg) { this->init(pinA, pinB, diameter, steps); @@ -30,13 +33,16 @@ void Speedometer::init(uint8_t pinA, uint8_t pinB, double diameter, uint16_t ste } uint16_t Speedometer::loop() { + if (this->calibrationRunning) + return -1; + uint32_t time = millis(); uint16_t elapsed_time = time - this->last_millis_loop; - //Cancel if delay is not reached - if (elapsed_time < delay) { + //Cancel if delayLoop is not reached + if (elapsed_time < this->delayLoop) return elapsed_time; - } + runSpeedometer(); this->last_millis_loop = time; return elapsed_time; @@ -50,7 +56,7 @@ void Speedometer::runSpeedometer() { int16_t count = encoder.getCount(); this->addValToBuf(count); - encoder.clearCount(); + this->encoder.clearCount(); uint16_t count_abs = abs(this->calcAverage()); // Absolute time in milliseconds double n = (double)count_abs / steps; // Wheel revolutions in absolute time @@ -64,6 +70,8 @@ void Speedometer::runSpeedometer() { } else { this->speed = 0; } + + std::cout << "Speedometer::runSpeedometer speed: " << (int) this->speed << std::endl; } void Speedometer::setNumOfValForAvg(uint8_t val) { @@ -73,18 +81,31 @@ void Speedometer::setNumOfValForAvg(uint8_t val) { } void Speedometer::setEncFilter(uint16_t val) { - if (val > 1023) val = 1023; + if (val > 1023) + val = 1023; this->encoder.setFilter(val); } -void Speedometer::setDelay(uint8_t delay) { - this->delay = delay; +void Speedometer::setDelay(uint8_t delayLoop) { + this->delayLoop = delayLoop; } double Speedometer::getSpeed() { return this->speed; } +void Speedometer::calibrationMeasurementStart() { + this->calibrationRunning = true; + this->encoder.clearCount(); +} + +uint16_t Speedometer::calibrationMeasurementStop() { + this->calibrationRunning = false; + uint16_t res = abs(this->encoder.getCount()); + this->encoder.clearCount(); + return res; +} + void Speedometer::initAvgBuf() { this->buf = new int16_t[bufSize]; for (uint8_t i = 0; i < bufSize; i++) @@ -92,13 +113,11 @@ void Speedometer::initAvgBuf() { } void Speedometer::addValToBuf(int16_t val) { - static uint8_t bufPos = 0; - this->buf[bufPos] = val; - bufPos++; + this->buf[this->bufPos] = val; + this->bufPos++; - if (bufPos == bufSize) { + if (bufPos == bufSize) bufPos = 0; - } } void Speedometer::updateAvgBufSize() { @@ -108,7 +127,7 @@ void Speedometer::updateAvgBufSize() { int16_t Speedometer::calcAverage() { int16_t sum = 0; - for (int i = 0; i < bufSize; i++) + for (int i = 0; i < this->bufSize; i++) sum += this->buf[i]; - return sum / bufSize; -} \ No newline at end of file + return sum / this->bufSize; +} diff --git a/lib/Speedometer/speedometer.h b/lib/Speedometer/speedometer.h index 0cbbd01..4a972f2 100644 --- a/lib/Speedometer/speedometer.h +++ b/lib/Speedometer/speedometer.h @@ -14,6 +14,7 @@ #include #include +#include #include /** @@ -39,6 +40,7 @@ class Speedometer { public: Speedometer(); + ~Speedometer(); /** * @brief Initialize the speedometer @@ -55,7 +57,7 @@ class Speedometer { /** * @brief Calls runSpeedometer() to update all Values. * - * This function should be called every mainloop. If the delay is not reached, than the + * This function should be called every mainloop. If the delayLoop is not reached, than the * functions returns immediately. * @see runSpeedometer() * @see DELAY_SPEEDOMETER @@ -85,11 +87,11 @@ class Speedometer { void setEncFilter(uint16_t val); /** - * @brief Set the min delay between each loop + * @brief Set the min delayLoop between each loop * - * @param delay time in Milliseconds + * @param delayLoop time in Milliseconds */ - void setDelay(uint8_t delay); + void setDelay(uint8_t delayLoop); /** * @brief Get the calculated speed of the Wheel @@ -98,6 +100,9 @@ class Speedometer { */ double getSpeed(); + void calibrationMeasurementStart(); + uint16_t calibrationMeasurementStop(); + private: void initAvgBuf(); @@ -108,15 +113,17 @@ class Speedometer { ESP32Encoder encoder; bool isInit = false; + bool calibrationRunning = false; double speed = 0; double diameter; uint8_t bufSize = BUFSIZE; + uint8_t delayLoop = DELAY_SPEEDOMETER; + uint8_t bufPos = 0; uint16_t steps; - uint8_t delay = DELAY_SPEEDOMETER; - int16_t *buf; + int16_t *buf = nullptr; uint32_t last_millis_loop = 0; uint32_t last_millis_calc = 0; diff --git a/src/SpecialMenus/Test/menuTest.cpp b/src/SpecialMenus/Test/menuTest.cpp new file mode 100644 index 0000000..2902b37 --- /dev/null +++ b/src/SpecialMenus/Test/menuTest.cpp @@ -0,0 +1,78 @@ +/** + * @file menuTest.cpp + * @author Alexander Klein (alex@kleiax.de) + * @brief + * @version 0.1 + * @date 2023-01-24 + * + * @copyright Copyright (c) 2023 + * + */ + +#include "menuTest.h" + +MenuTest::MenuTest(Speedometer* speedometer) { + this->speedometer = speedometer; +} + + +void MenuTest::printMenu() { + switch (this->state) { + case State::Off : + this->print("Press Yes (O)", "to start"); + break; + + case State::Running : + this->print("Running, Yes (O)", "to stop"); + break; + + case State::Finished : { + String res = ""; + res.concat(this->result); + this->print("Result: ", res); + } + break; + + default: + this->print("Error in", "MenuTest.cpp"); + break; + } +} + + +void MenuTest::left() { + this->no(); +} + +void MenuTest::no() { + if (this->state == State::Running) + speedometer->calibrationMeasurementStop(); + + this->state = State::Off; + this->parentMenu->printMenu(); +} + +void MenuTest::yes() { + switch (this->state) { + case State::Off : + this->state = State::Running; + this->speedometer->calibrationMeasurementStart(); + this->printMenu(); + break; + + case State::Running : + this->state = State::Finished; + this->result = this->speedometer->calibrationMeasurementStop(); + this->printMenu(); + break; + + case State::Finished : + this->state = State::Running; + this->speedometer->calibrationMeasurementStart(); + this->printMenu(); + break; + + default: + break; + } +} diff --git a/src/SpecialMenus/Test/menuTest.h b/src/SpecialMenus/Test/menuTest.h new file mode 100644 index 0000000..0d89c0b --- /dev/null +++ b/src/SpecialMenus/Test/menuTest.h @@ -0,0 +1,37 @@ +/** + * @file menuTest.h + * @author Alexander Klein (alex@kleiax.de) + * @brief + * @version 0.1 + * @date 2023-01-24 + * + * @copyright Copyright (c) 2023 + * + */ + +#pragma once + +#include "menuControl.h" +#include "speedometer.h" + +class MenuTest : public MenuControl { + public: + enum State { + Off, + Running, + Finished + }; + MenuTest(Speedometer* speedometer); + + void printMenu() override; + + void left() override; + void no() override; + void yes() override; + + private: + Speedometer* speedometer; + State state = State::Off; + + uint16_t result = 0; +}; diff --git a/src/main.cpp b/src/main.cpp index 4d455a5..f2920b3 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -44,6 +44,7 @@ #include "SpecialMenus/PID/menuPidSettings.h" #include "SpecialMenus/Route/menuRoute.h" #include "SpecialMenus/GPS/menuGPS.h" +#include "SpecialMenus/Test/menuTest.h" MoveControl moveController; DriveManager* driveManager; @@ -270,10 +271,11 @@ void makeMenu() { MenuManualControl* man_m = new MenuManualControl(driveManager); MenuCaptureRoute* cap_m = new MenuCaptureRoute(driveManager); MenuAutopilot* auto_m = new MenuAutopilot(driveManager); - MenuTestMode* test_m = new MenuTestMode(driveManager); + MenuTestMode* testM_m = new MenuTestMode(driveManager); MenuGPS* gps_m = new MenuGPS(driveManager); MenuSysteminformation* sys_m = new MenuSysteminformation(mainBattery, gamepad); MenuRoute* rout_m = new MenuRoute(driveManager->getNavigation()->getRoute()); + MenuTest* test_m = new MenuTest(moveController.getSpeedometerLeft()); // Set Menus on LCD main_m->setLcd(lcdWrapper); @@ -284,10 +286,11 @@ void makeMenu() { man_m->setLcd(lcdWrapper); cap_m->setLcd(lcdWrapper); auto_m->setLcd(lcdWrapper); - test_m->setLcd(lcdWrapper); + testM_m->setLcd(lcdWrapper); gps_m->setLcd(lcdWrapper); sys_m->setLcd(lcdWrapper); rout_m->setLcd(lcdWrapper); + test_m->setLcd(lcdWrapper); // Entry for the main menu @@ -298,11 +301,13 @@ void makeMenu() { MenuAction* contr_e = new MenuAction("Remove PS3", disconnectController); MenuAction* sys_e = new MenuAction("Systeminfo", sys_m); MenuAction* rout_e = new MenuAction("Route", rout_m); + MenuAction* test_e = new MenuAction("Test", test_m); main_m->addEntry(mode_e); main_m->addEntry(gps_e); main_m->addEntry(rout_e); main_m->addEntry(pid_e); main_m->addEntry(sys_e); + main_m->addEntry(test_e); main_m->addEntry(contr_e); main_m->addEntry(restart_e); @@ -311,7 +316,7 @@ void makeMenu() { MenuAction* captureRoute_e = new MenuAction("Capture Route", cap_m); MenuAction* consolControl_e = new MenuAction("Consol Control", dummy); MenuAction* manualControl_e = new MenuAction("Manual Control", man_m); - MenuAction* testMode_e = new MenuAction("Test Mode", test_m); + MenuAction* testMode_e = new MenuAction("Test Mode", testM_m); mode_m->addEntry(autopilot_e); mode_m->addEntry(captureRoute_e); mode_m->addEntry(manualControl_e);