add menuTest

lib/Navigation/navigation.cpp

@@ -66,15 +66,13 @@ 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;
-
+    delete this->compass;
-    if (this->isNtripInit)
     delete this->ntripClient;
-
-    if (this->route)
     delete this->route;
 }
 
@@ -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;

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
 
 /**

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
+    //Cancel if delayLoop is not reached
-    if (elapsed_time < delay) {
+    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) {
+void Speedometer::setDelay(uint8_t delayLoop) {
-    this->delay = delay;
+    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[this->bufPos] = val;
-    this->buf[bufPos] = val;
+    this->bufPos++;
-    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;
+    return sum / this->bufSize;
 }

lib/Speedometer/speedometer.h

@@ -14,6 +14,7 @@
 
 #include <Arduino.h>
 #include <cstdint>
+#include <iostream>
 #include <ESP32Encoder.h>
 
 /**
@@ -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;

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;
+    }
+}

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;
+};

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