bug fix static in motor und speed

.vscode/settings.json

@@ -74,6 +74,8 @@
         "vector": "cpp",
         "fstream": "cpp",
         "istream": "cpp",
-        "streambuf": "cpp"
+        "streambuf": "cpp",
+        "functional": "cpp",
+        "cmath": "cpp"
     }
 }

include/networkConfig.h

@@ -1,36 +1,26 @@
-#define HW1_2
+#define HW1
 
 //Network config RHEDE
 #ifdef RHEDE
 #define WLAN_SSID "LebennigHuus"
 #define WLAN_PASSWORD "Punica-699"
-#define WLAN_IP 0x040BA8C0          //192.168.11.4
+#define WLAN_IP "192.168.11.4"
-#define WLAN_SUBNETMASK 0x0080FFFF  //255.255.128.0
+#define WLAN_SUBNETMASK "255.255.128.0"
-#define WLAN_GATEWAY 0x0100a8c0     //192.168.0.1
+#define WLAN_GATEWAY "192.168.0.1"
-#define MQTT_SERVER 0x0701A8C0      //192.168.1.7
+#define MQTT_SERVER "192.168.1.7"
 #define MQTT_PORT 1883
 #define MQTT_AUTH
 #define MQTT_USER "kleiax"
 #define MQTT_PASSWORD "p?{$_~5%hBM7wrcFkr55KWr#"
-#define NTP_SERVER 0x0100a8c0       //192.168.0.1
+#define NTP_SERVER "2.de.pool.ntp.org"
 #endif //RHEDE
 
+
+
 //Network config HW1
 #ifdef HW1
 #define WLAN_SSID "hw1_gast"
 #define WLAN_PASSWORD "KeineAhnung"
-#define WLAN_IP 0x3400a8c0          //192.168.0.67
-#define WLAN_SUBNETMASK 0x00FFFFFF  //255.255.255.0
-#define WLAN_GATEWAY 0x0100a8c0     //192.168.0.1
-#define MQTT_SERVER 0xD84016AC      // 172.22.64.216
-#define MQTT_PORT 1883
-#define NTP_SERVER "2.de.pool.ntp.org"
-#endif //HW1 
-
-//Network config HW1
-#ifdef HW1_2
-#define WLAN_SSID "hw1_gast"
-#define WLAN_PASSWORD "KeineAhnung"
 #define WLAN_IP "192.168.0.67"
 #define WLAN_SUBNETMASK "255.255.255.0"
 #define WLAN_GATEWAY "192.168.0.1"

lib/Menu/menu.cpp

@@ -20,18 +20,19 @@ void Menu::addEntry(MenuAction* entry) {
     // TODO: Same shit here. Why I get the first element after an increment.
     static uint8_t inc = 0;
     inc++;
-    if (inc == 2) {
+    if (inc == 1) {
         this->it = this->entrys.begin();
     }
 }
 
 void Menu::printMenu() {
-    Serial.println("\n\n");
+    std::cout << std::endl;
     for (std::list<MenuAction*>::iterator iter = this->entrys.begin(); iter != this->entrys.end(); iter++) {
         MenuAction* selectedEntry = *(iter);
         if (this->it == iter)
-            Serial.print("  ");
+            std::cout << "  " << selectedEntry->getName() << std::endl;
-        Serial.println(selectedEntry->getName());
+        else
+            std::cout << selectedEntry->getName() << std::endl;
     }
     this->inSubmenu = false;
 }
@@ -62,11 +63,9 @@ void Menu::up() {
     std::list<MenuAction*>::iterator iter = this->entrys.end();
     iter--;
     if (this->it != this->entrys.begin()) {
-        Serial.println("--");
         this->it--;
     }
     else {
-        Serial.println("begin");
        this->it = iter;
     }
     this->printMenu();
@@ -86,6 +85,8 @@ void Menu::right() {
         selectedEntry->getMenu()->setParentMenu(this);
     } 
     selectedEntry->runAction();
+    if (selectedEntry->getIsMenu())
+        selectedEntry->getMenu()->down();
 }
 
 void Menu::left() {

lib/Menu/menu.h

@@ -13,10 +13,6 @@
 
 #include <list>
 
-// Only for print in consol
-// have to be deleted after display installation
-#include <Arduino.h>
-
 #include "menuAction.h"
 #include "menuControl.h"
 

lib/Menu/menuAction.cpp

@@ -22,6 +22,14 @@ MenuAction::MenuAction(const char* name, void (*function) ()) {
     this->callback = nullptr;
 }
 
+// MenuAction::MenuAction(const char* name, void (DriveManager:: *classFunction) ()) {
+//     this->name = name;
+//     this->classFunction = classFunction;
+//     this->classFunc = true;
+//     this->function = nullptr;
+//     this->callback = nullptr;
+// }
+
 MenuAction::MenuAction(const char* name, MenuControl* menu) {
     this->name = name;
     this->menu = menu;

lib/Menu/menuControl.h

@@ -12,6 +12,8 @@
 #ifndef MENU_CONTROLL_H
 #define MENU_CONTROLL_H
 
+#include <iostream>
+
 class MenuControl {
     public:
 

lib/MotorControl/motorControl.cpp

@@ -34,23 +34,24 @@ void MotorControl::init(uint8_t pwm_pin, uint8_t pwm_channel, uint8_t dir_1, uin
     ledcWrite(this->pwm_channel, 0);
 }
 
-void MotorControl::loop() {
+uint16_t MotorControl::loop() {
-    static uint32_t last_millis = 0;
     uint32_t time = millis();
+    uint16_t elapsed_time = time - this->last_millis;
 
     //Cancel if delay is not reached
-    if (time - last_millis < delay) {
+    if (elapsed_time < delay)
-        return;
+        return elapsed_time;
-    }
+
     runMotorControl();
-    last_millis = time;
+    this->last_millis = time;
+    return elapsed_time;
 }
 
 void MotorControl::runMotorControl() {
-    // Absolute difference between target_power and speed
+    // Absolute difference between target_power and power
     uint8_t abs_difference = abs(this->target_power - this->power);
 
-    // Difference between target_power and speed
+    // Difference between target_power and power
     int16_t difference = this->target_power - this->power;
 
     // Check that the target speed is close to 0 and that the abs_difference is lower than powersteps

lib/MotorControl/motorControl.h

@@ -48,8 +48,9 @@ class MotorControl {
          * functions returns immediately.
          * @see runMotorControl()
          * @see DELAY
+         * @return time since the last call in Milliseconds
          */
-        void loop();
+        uint16_t loop();
 
         /**
          * @brief Normaly called repeatedly by loop() to update the pwm signal.
@@ -161,6 +162,8 @@ class MotorControl {
         uint8_t dir_2;
         uint8_t delay = DELAY;
         uint8_t powersteps = POWERSTEPS;
+
+        uint32_t last_millis = 0;
         
 };
 

lib/Speedometer/speedometer.cpp

@@ -29,24 +29,24 @@ void Speedometer::init(uint8_t pinA, uint8_t pinB, double diameter, uint16_t ste
     this->isInit = true;
 }
 
-void Speedometer::loop() {
+uint16_t Speedometer::loop() {
-    static uint32_t last_millis = 0;
     uint32_t time = millis();
+    uint16_t elapsed_time = time - this->last_millis_loop;
 
     //Cancel if delay is not reached
-    if (time - last_millis < delay) {
+    if (elapsed_time < delay) {
-        return;
+        return elapsed_time;
     }
     runSpeedometer();
-    last_millis = time;
+    this->last_millis_loop = time;
+    return elapsed_time;
 }
 
 void Speedometer::runSpeedometer() {
-    static uint32_t last_millis = 0;
     uint32_t time = millis();
 
-    uint16_t elapsed_time = time - last_millis;
+    uint16_t elapsed_time = time - last_millis_calc;
-    last_millis = time;
+    last_millis_calc = time;
 
     int16_t count = encoder.getCount();
     this->addValToBuf(count);

lib/Speedometer/speedometer.h

@@ -58,8 +58,9 @@ class Speedometer {
          * functions returns immediately.
          * @see runSpeedometer()
          * @see DELAY_SPEEDOMETER
+         * @return time since the last call in Milliseconds
          */
-        void loop();
+        uint16_t loop();
 
         /**
          * @brief Noramly called repeatedly by loop() to calcluate new values.
@@ -115,6 +116,9 @@ class Speedometer {
         uint8_t delay = DELAY_SPEEDOMETER;
 
         int16_t *buf;
+
+        uint32_t last_millis_loop = 0;
+        uint32_t last_millis_calc = 0;
 };
 
 #endif // SPEEDOMETER_H

src/SpecialMenus/menuManualDrive.cpp

@@ -0,0 +1,20 @@
+#include "menuManualDrive.h"
+
+MenuManualControl::MenuManualControl(DriveManager* driveManager) {
+    this->driveManager = driveManager;
+}
+
+void MenuManualControl::left() {
+    this->driveManager->changeModus(Modi::Off);
+    if (parentMenu)
+        this->parentMenu->printMenu();
+}
+
+void MenuManualControl::no() {
+    this->left();
+}
+
+void MenuManualControl::printMenu() {
+    this->driveManager->changeModus(Modi::ManualControl);
+    std::cout << "Jetzt kannst du\n fahren. :-)" << std::endl;
+}

src/SpecialMenus/menuManualDrive.h

@@ -0,0 +1,42 @@
+/**
+ * @file menuManualDrive.h
+ * @author Alexander Klein (alex@kleiax.de)
+ * @brief 
+ * @version 0.1
+ * @date 2022-01-20
+ * 
+ * @copyright Copyright (c) 2022
+ * 
+ */
+
+#ifndef MENU_MANUAL_DRIVE_H
+#define MENU_MANUAL_DRIVE_H
+
+#include "menuControl.h"
+#include "driveModi/driveManager.h"
+
+// TODO: delete after installtion of display
+#include <Arduino.h>
+
+
+
+class MenuManualControl : public MenuControl {
+    public:
+        MenuManualControl(DriveManager* driveManager);
+
+        void down(){}
+        void up(){}
+        void right(){}
+        void left();
+        void no();
+        void yes(){}
+
+        void printMenu();
+
+    private:
+        DriveManager* driveManager;
+        
+
+};
+
+#endif // MENU_MANUAL_DRIVE_H

src/SpecialMenus/menuPidSettings.cpp

@@ -62,25 +62,23 @@ void MenuPidSettings::yes() {
 }
 
 void MenuPidSettings::printMenu() {
-    Serial.println("  P    I    D  ");
+    std::cout << "  P    I    D  " << std::endl;
-    char pos[16];
     switch (this->curPos) {
         case 0:
-            sprintf(pos, "_%d.3_ %d.3  %d.3 ", this->values[0], this->values[1], this->values[2]);
+            printf("_%3.d_ %3.d  %3.d \n", this->values[0], this->values[1], this->values[2]);
             break;
 
         case 1:
-            sprintf(pos, " %d.3 _%d.3_ %d.3 ", this->values[0], this->values[1], this->values[2]);
+            printf(" %3.d _%3.d_ %3.d \n", this->values[0], this->values[1], this->values[2]);
             break;
 
         case 2:
-            sprintf(pos, " %d.3  %d.3 _%d.3_", this->values[0], this->values[1], this->values[2]);
+            printf(" %3.d  %3.d _%3.d_\n", this->values[0], this->values[1], this->values[2]);
             break;
         
         default:
             break;
     }
-    Serial.println(pos);
 }
 
 

src/driveModi/Modi/ManualControl/manualControl.cpp

@@ -24,8 +24,6 @@ void ManualControl::init(MoveControl *moveControl) {
 }
 
 void ManualControl::loop() {
-    this->moveControl->loop();
-
     static uint32_t last_millis = 0;
     if (millis() - last_millis < delay) {
         return;

src/driveModi/driveManager.cpp

@@ -16,6 +16,7 @@ DriveManager::DriveManager(MoveControl *moveControl) {
 }
 
 void DriveManager::loop() {
+    this->moveControl->loop();
     if (currentModus)
         this->currentModus->loop();
 }
@@ -31,11 +32,12 @@ void DriveManager::nextModus() {
 void DriveManager::changeModus(Modi modus) {
 
     //Set moveControl to a safe state
+    delete this->currentModus;
+
     this->moveControl->setSpeed(0);
     this->moveControl->setRotationspeed(0);
     this->moveControl->setDrivingStatus(DrivingStatus::stop);
-    delete this->currentModus;
+    
-
     switch (modus) {
         case Modi::Off:
             this->currentModus = nullptr;

src/main.cpp

@@ -1,5 +1,6 @@
 #include <Arduino.h>
 #include <Ps3Controller.h>
+#include <iostream>
 
 #include "config.h"
 
@@ -11,26 +12,26 @@
 
 #include "menu.h"
 #include "menuAction.h"
-#include "menuPidSettings.h"
+#include "SpecialMenus/menuPidSettings.h"
-
+#include "SpecialMenus/menuManualDrive.h"
-// Platzhalter, muss irgendwann weg
-void dummy(){Serial.println("Dummy in Action");}
-
-void callbackControllerAction();
-void callbackControllerConnect();
-void controllerPrintBattery();
-void p_f();
-void i_f();
-void d_f();
-int32_t displaySelectValue();
 
 MoveControl moveController;
 DriveManager driveManager(&moveController);
 Menu* main_m;
 
+// TODO: Platzhalter, muss irgendwann weg
+void dummy(){std::cout << "Dummy in Action" <<std::endl;}
+
+
+void callbackControllerAction();
+void callbackControllerConnect();
+void controllerPrintBattery();
+
+void exitDriveMode() {driveManager.changeModus(Modi::Off);}
+
 void setup() {
     Serial.begin(115200);
-    Serial.println("Welcome to Kleiax-Rover");
+    std::cout << "Welcome to Kleiax-Rover" << std::endl;
 
     Network::setIps();
     Network::connectWifi();
@@ -38,7 +39,7 @@ void setup() {
 
     Ps3.attach(callbackControllerAction);
     Ps3.attachOnConnect(callbackControllerConnect);
-    Serial.println("\nReady to connect a PS3 Controller... \n");
+    std::cout << "\nReady to connect a PS3 Controller... \n";
     Ps3.begin();
 
     Serial1.begin(GPS_BAUD, SERIAL_8N1, GPS_RX, GPS_TX);
@@ -49,6 +50,7 @@ void setup() {
     Menu* pid_m = new Menu();
     MenuPidSettings* pidl_m = new MenuPidSettings(moveController.getPID(0));
     MenuPidSettings* pidr_m = new MenuPidSettings(moveController.getPID(1));
+    MenuManualControl* man_m = new MenuManualControl(&driveManager);
 
     // Entry for the main menu
     MenuAction* mode_e = new MenuAction("Mode", mode_m);
@@ -62,7 +64,7 @@ void setup() {
     MenuAction* autopilot_e = new MenuAction("Autopilot", dummy);
     MenuAction* captureRoute_e = new MenuAction("Capture Route", dummy);
     MenuAction* consolControl_e = new MenuAction("Consol Control", dummy);
-    MenuAction* manualControl_e = new MenuAction("Manual Control", dummy);
+    MenuAction* manualControl_e = new MenuAction("Manual Control", man_m);
     MenuAction* testMode_e = new MenuAction("Test Mode", dummy);
     mode_m->addEntry(autopilot_e);
     mode_m->addEntry(captureRoute_e);
@@ -82,7 +84,8 @@ void loop() {
     Network::checkMQTT();
     driveManager.loop();
     while (Serial1.available() > 0)
-        Serial.print(Serial1.read());
+        std::cout << "In while von gps in loop in main.cpp" << std::endl;
+        // Serial.print(Serial1.read());
 }
 
 bool isDelayReached() {
@@ -112,16 +115,15 @@ 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.select)
-            main_m->printMenu();
         else if (Ps3.data.button.ps)
             controllerPrintBattery();
     }
 }
 
 void callbackControllerConnect() {
-    Serial.println("Controller connected to ESP32");
+    std::cout << "Controller connected to ESP32" << std::endl;
+    main_m->printMenu();
 }
 
 void controllerPrintBattery() {
@@ -131,11 +133,11 @@ void controllerPrintBattery() {
     }
 
     Serial.print("The controller battery is ");
-    if( controller_battery == ps3_status_battery_charging )      Serial.println("charging");
+    if( controller_battery == ps3_status_battery_charging )      printf("charging\n");
-    else if( controller_battery == ps3_status_battery_full )     Serial.println("FULL");
+    else if( controller_battery == ps3_status_battery_full )     printf("FULL\n");
-    else if( controller_battery == ps3_status_battery_high )     Serial.println("HIGH");
+    else if( controller_battery == ps3_status_battery_high )     printf("HIGH\n");
-    else if( controller_battery == ps3_status_battery_low)       Serial.println("LOW");
+    else if( controller_battery == ps3_status_battery_low)       printf("LOW\n");
-    else if( controller_battery == ps3_status_battery_dying )    Serial.println("DYING");
+    else if( controller_battery == ps3_status_battery_dying )    printf("DYING\n");
-    else if( controller_battery == ps3_status_battery_shutdown ) Serial.println("SHUTDOWN");
+    else if( controller_battery == ps3_status_battery_shutdown ) printf("SHUTDOWN\n");
-    else Serial.println("UNDEFINED");
+    else printf("UNDEFINED\n");
 }

src/moveControl.cpp

@@ -38,10 +38,17 @@ MoveControl::~MoveControl() {
 }
 
 void MoveControl::loop() {
-    this->left_motor->loop();
+    uint16_t left_motor_time = this->left_motor->loop();
-    this->right_motor->loop();
+    uint16_t right_motor_time = this->right_motor->loop();
-    this->left_speedometer->loop();
+    uint16_t left_speed_time = this->left_speedometer->loop();
-    this->right_speedometer->loop();
+    uint16_t right_speed_time = this->right_speedometer->loop();
+
+    if (left_motor_time > 50
+        ||  right_motor_time > 50
+        ||  left_speed_time > 50
+        ||  right_speed_time > 50) {
+        Serial.printf("left M: %d, right M %d, left S %d, right S %d in moveControl::loop\n", left_motor_time, right_motor_time, left_speed_time, right_speed_time);
+    }
 
     static uint64_t last_millis = 0;
     if (millis() - last_millis < delay) 
@@ -63,6 +70,19 @@ void MoveControl::runMoveControl() {
 
 void MoveControl::setDrivingStatus(DrivingStatus status) {
     this->driving_status = status;
+    // switch (this->driving_status) {
+    //     case DrivingStatus::stop :
+    //         Serial.println("New drivingState = stop in MoveControl::setDrivingStatus");
+    //         break;
+
+    //     case DrivingStatus::drive :
+    //         Serial.println("New drivingState = drive in MoveControl::setDrivingStatus");
+    //         break;
+
+    //     default:
+    //         Serial.println("Wrong drivingState in MoveControl::regulateMotors");
+    //         break;
+    // }
 }
 
 void MoveControl::setSpeed(double speed) {
@@ -114,19 +134,28 @@ void MoveControl::calcTargetWheelSpeed() {
 
 void MoveControl::regulateMotors() {
     switch (this->driving_status) {
-    case DrivingStatus::stop :
+        case DrivingStatus::stop :
-        this->left_motor->setTargetPower(0);
+            this->left_motor->setTargetPower(0);
-        this->right_motor->setTargetPower(0);
+            this->right_motor->setTargetPower(0);
-        break;
+            // Serial.println("in stop MoveControl::regulateMotors");
+            // if (abs(left_speedometer->getSpeed()) > 0.2) {
+            //     Serial.printf("Left Motor Target: %d, Ist: %d\n", this->left_motor->getTargetPower(), this->left_motor->getPower());
+            //     Serial.printf("Left Speedometer speed: %f\n", this->left_speedometer->getSpeed());
+            // }
+            // if (abs(right_speedometer->getSpeed()) > 0.2) {
+            //     Serial.printf("Right Motor Target: %d, Ist: %d\n", this->right_motor->getTargetPower(), this->right_motor->getPower());
+            //     Serial.printf("Right Speedometer speed: %f\n", this->right_speedometer->getSpeed());
+            // }
+            break;
 
-    case DrivingStatus::drive :
+        case DrivingStatus::drive :
-        this->left_motor->setTargetPower( (int8_t) this->left_pid_out);
+            this->left_motor->setTargetPower( (int8_t) this->left_pid_out);
-        this->right_motor->setTargetPower( (int8_t) this->right_pid_out);
+            this->right_motor->setTargetPower( (int8_t) this->right_pid_out);
-        break;
+            break;
 
-    default:
+        default:
-        Serial.println("Wrong drivingState in MoveControl::regulateMotors");
+            Serial.println("Wrong drivingState in MoveControl::regulateMotors");
-        break;
+            break;
     }
 }