fix bug in route add akku menu begin with akkuread

.vscode/settings.json

@@ -1,4 +1,5 @@
 {
+    "files.insertFinalNewline": true,
     "cSpell.ignoreWords": [
         "ledc",
         "write"

lib/Akku/akku.cpp

@@ -0,0 +1,66 @@
+/**
+ * @file akku.cpp
+ * @author Alexander Klein (alex@kleiax.de)
+ * @brief 
+ * @version 0.1
+ * @date 2022-02-05
+ * 
+ * @copyright Copyright (c) 2022
+ * 
+ */
+
+#include "akku.h"
+
+Akku::Akku(uint8_t pin, double maxVolt, double emptyVolt) {
+    this->pin = pin;
+    this->maxVolt = maxVolt;
+    this->emptyVolt = emptyVolt;
+    this->updateOnePercent();
+}
+
+void Akku::setEmptyVoltage(double volt) {
+    this->emptyVolt = volt;
+    this->updateOnePercent();
+}
+
+void Akku::setMaxVoltage(double volt) {
+    this->maxVolt = volt;
+    this->updateOnePercent();
+}
+
+double Akku::getVoltage() {
+    double rawVoltage = this->readVoltage();
+    if (this->maxVolt) {
+        
+    }
+    return rawVoltage;
+}
+
+uint8_t Akku::getPercent() {
+    uint8_t res = 0;
+    if (this->emptyVolt)
+        res = (uint8_t) ((this->getVoltage() - this->emptyVolt) / this->onePercent);
+    return res;
+}
+
+bool Akku::isEmpty() {
+    if (this->getVoltage() < this->emptyVolt) {
+        return true;
+    }
+    return false;
+}
+
+double Akku::readVoltage() {
+    // Reference voltage is 3v3 so maximum reading is 3v3 = 4095 in range 0 to 4095
+    double reading = analogRead(this->pin);
+    if(reading < 1 || reading > 4095) return 0;
+    return  - 0.000000000000016 * pow(reading,4)
+            + 0.000000000118171 * pow(reading,3)
+            - 0.000000301211691 * pow(reading,2)
+            + 0.001109019271794 * reading
+            + 0.034143524634089;
+}
+
+double Akku::updateOnePercent() {
+    this->onePercent = (this->maxVolt - this->emptyVolt) / 100.0;
+}

lib/Akku/akku.h

@@ -0,0 +1,46 @@
+/**
+ * @file akku.h
+ * @author Alexander Klein (alex@kleiax.de)
+ * @brief 
+ * @version 0.1
+ * @date 2022-02-05
+ * 
+ * @copyright Copyright (c) 2022
+ * 
+ */
+
+#ifndef AKKU_H
+#define AKKU_H
+
+#include <stdint.h>
+#include <math.h>
+#include <Arduino.h>
+
+class Akku {
+    public:
+        Akku(uint8_t pin, double maxVolt = 0, double emptyVolt = 0);
+
+        void setEmptyVoltage(double volt);
+        void setMaxVoltage(double volt);
+
+        double getVoltage();
+        uint8_t getPercent();
+        bool isEmpty();
+
+    private:
+        double readVoltage();
+        double updateOnePercent();
+
+        uint8_t pin;
+        double maxVolt;
+        double emptyVolt;
+        double onePercent;
+        const float capacityVoltages[21] = {9.82, 10.83, 11.06, 11.12,
+                                            11.18, 11.24, 11.3, 11.36,
+                                            11.39, 11.45, 11.51, 11.56,
+                                            11.62, 11.74, 11.86, 11.95,
+                                            12.07, 12.25, 12.33, 12.45,
+                                            12.6 };
+};
+
+#endif // AKKU_H

lib/Navigation/navigation.cpp

@@ -131,4 +131,4 @@ double Navigation::courseTo(Point p1, Point p2) {
     if (p1.isValid() && p2.isValid())
         return TinyGPSPlus::courseTo(p1.lat, p1.lon, p2.lat, p2.lon);
     return -1;
-}
+}

lib/Navigation/route.cpp

@@ -11,6 +11,9 @@
 
 #include "route.h"
 
+// TODO: Delete iostream
+#include <iostream>
+
 Route::Route() {
 
 }
@@ -22,16 +25,16 @@ void Route::addPointToRoute(Point point) {
 Point Route::startRoute() {
     if (this->points.size() < 1)
         return Point(0, 0);
-    *this->it = this->points.begin();
+    this->it = this->points.begin();
     this->currentPoint = 1;
-    return **this->it;
+    return *this->it;
 }
 
 Point Route::getNextPoint() {
-    if (*this->it != this->points.end()) {
+    if (this->it != this->points.end()) {
         this->it++;
         this->currentPoint++;
-        return **this->it;
+        return *this->it;
     }
     else
         return Point(0, 0);
@@ -42,4 +45,4 @@ RouteInfo Route::getRouteInfo() {
     info.totalPoints = this->points.size();
     info.currentPoint = this->currentPoint;
     return info;
-}
+}

lib/Navigation/route.h

@@ -49,8 +49,8 @@ class Route {
         uint16_t currentPoint = 0;
 
         std::list<Point> points;
-        std::list<Point>::iterator* it;
+        std::list<Point>::iterator it;
 
 };
 
-#endif // ROUTE_H
+#endif // ROUTE_H

src/SpecialMenus/Akku/menuAkku.cpp

@@ -50,13 +50,13 @@ void MenuAkku::printMenu() {
     else sprintf(buf, "UNDEF");
 
     if (this->lcd) {
-
-    } else {
         lcd->clear();
         lcd->setCursor(0, 0);
         lcd->printf("Controller: %s", buf);
         lcd->setCursor(0, 1);
         lcd->printf("Main: %u%%", mainBattery);
+    } else {
+        std::cout << "Controller: " << buf << "Main: " << mainBattery << "%" << std::endl;
     }
 }
 
@@ -66,4 +66,4 @@ void MenuAkku::update() {
     }
     this->printMenu();
     this->last_millis = millis();
-}
+}

src/SpecialMenus/Akku/menuAkku.h

@@ -36,4 +36,4 @@ class MenuAkku : public MenuControl {
 
 };
 
-#endif // MENU_AKKU_H
+#endif // MENU_AKKU_H

src/SpecialMenus/GPS/menuGPS.cpp

@@ -129,4 +129,4 @@ void MenuGPS::printPage(uint8_t page) const {
             }
             break;
     }
-}
+}

src/SpecialMenus/driveModi/Autopilot/menuAutopilot.cpp

@@ -71,7 +71,6 @@ void MenuAutopilot::printMenu() {
 
         } else
             std::cout << buf1 << " " << buf2 << std::endl;
-    
 }
 
 void MenuAutopilot::update() {
@@ -91,4 +90,4 @@ void MenuAutopilot::init() {
     this->driveManager->changeModus(Modi::Autopilot);
     this->autopilot = (Autopilot*) this->driveManager->getDriveModiPtr();
     this->routeInfo = this->autopilot->getRouteInfo();
-}
+}

src/SpecialMenus/driveModi/Autopilot/menuAutopilot.h

@@ -27,7 +27,7 @@ class MenuAutopilot : public MenuDriveMode {
         RouteInfo routeInfo;
 
         uint32_t last_millis = 0;
-        uint16_t minDelay = 500;
+        uint16_t minDelay = 1500;
 };
 
-#endif // MENU_AUTOPILOT_DRIVE_H
+#endif // MENU_AUTOPILOT_DRIVE_H

src/main.cpp

@@ -6,19 +6,18 @@
 
 #include "config.h"
 
-
-#include "driveModi/Modi/ManualControl/manualControl.h"
 #include "driveModi/driveManager.h"
 #include "moveControl.h"
 #include "network.h"
 
 #include "menu.h"
 #include "menuAction.h"
-#include "SpecialMenus/PID/menuPidSettings.h"
 #include "SpecialMenus/driveModi/ManualDrive/menuManualDrive.h"
 #include "SpecialMenus/driveModi/CaptureRoute/menuCaptureRoute.h"
 #include "SpecialMenus/driveModi/Autopilot/menuAutopilot.h"
+#include "SpecialMenus/PID/menuPidSettings.h"
 #include "SpecialMenus/GPS/menuGPS.h"
+#include "SpecialMenus/Akku/menuAkku.h"
 
 MoveControl moveController;
 DriveManager driveManager(&moveController);
@@ -69,6 +68,7 @@ void setup() {
     MenuCaptureRoute* cap_m = new MenuCaptureRoute(&driveManager);
     MenuAutopilot* auto_m = new MenuAutopilot(&driveManager);
     MenuGPS* gps_m = new MenuGPS(driveManager.getNavigation()->getGPS());
+    MenuAkku* akku_m = new MenuAkku();
 
     // Set Menus on LCD
     main_m->setLcd(lcd);
@@ -80,6 +80,7 @@ void setup() {
     cap_m->setLcd(lcd);
     auto_m->setLcd(lcd);
     gps_m->setLcd(lcd);
+    akku_m->setLcd(lcd);
 
 
     // Entry for the main menu
@@ -87,9 +88,11 @@ void setup() {
     MenuAction* gps_e = new MenuAction("GPS", gps_m);
     MenuAction* pid_e = new MenuAction("PID", pid_m);
     MenuAction* restart_e = new MenuAction("Restart", restart);
+    MenuAction* akku_e = new MenuAction("Akku", akku_m);
     main_m->addEntry(mode_e);
     main_m->addEntry(gps_e);
     main_m->addEntry(pid_e);
+    main_m->addEntry(akku_e);
     main_m->addEntry(restart_e);
 
     // Entry for the mode Menu
@@ -100,8 +103,8 @@ void setup() {
     MenuAction* testMode_e = new MenuAction("Test Mode", dummy);
     mode_m->addEntry(autopilot_e);
     mode_m->addEntry(captureRoute_e);
-    mode_m->addEntry(consolControl_e);
     mode_m->addEntry(manualControl_e);
+    mode_m->addEntry(consolControl_e);
     mode_m->addEntry(testMode_e);
 
     // Entry for the PID Menu
@@ -143,9 +146,7 @@ void callbackControllerAction() {
         else if (Ps3.data.button.circle)
             main_m->yes();
         else if (Ps3.data.button.cross)
-            main_m->no();   
+            main_m->no();
-        else if (Ps3.data.button.ps)
-            controllerPrintBattery();
 
         isDelayReached(true);        
     }
@@ -162,26 +163,9 @@ void callbackControllerConnect() {
     main_m->printMenu();
 }
 
-void controllerPrintBattery() {
-    static uint8_t controller_battery = -1;
-    if( controller_battery != Ps3.data.status.battery ){
-        controller_battery = Ps3.data.status.battery;
-    }
-
-    Serial.print("The controller battery is ");
-    if( controller_battery == ps3_status_battery_charging )      printf("charging\n");
-    else if( controller_battery == ps3_status_battery_full )     printf("FULL\n");
-    else if( controller_battery == ps3_status_battery_high )     printf("HIGH\n");
-    else if( controller_battery == ps3_status_battery_low)       printf("LOW\n");
-    else if( controller_battery == ps3_status_battery_dying )    printf("DYING\n");
-    else if( controller_battery == ps3_status_battery_shutdown ) printf("SHUTDOWN\n");
-    else printf("UNDEFINED\n");
-}
-
 void i2cScanner() {
     std::cout << "\nI2C Scanner" << std::endl;
 
-
     byte error, address;
     int nDevices;
     Serial.println("Scanning...");
@@ -211,4 +195,4 @@ void i2cScanner() {
 
 void restart() {
     ESP.restart();
-}
+}