Added consolControl & PID

platformio.ini

@@ -14,10 +14,11 @@ board = esp32doit-devkit-v1
 board_build.partitions = no_ota.csv
 framework = arduino
 monitor_speed = 115200
-monitor_port = COM12
+monitor_port = COM3
 lib_deps = 
 	madhephaestus/ESP32Encoder@^0.4.0
 	jvpernis/PS3 Controller Host@^1.1.0
 	mikalhart/TinyGPSPlus@^1.0.2
 	knolleary/PubSubClient@^2.8
-upload_port = COM12
+	br3ttb/PID@^1.2.1
+upload_port = COM3

src/config.h

@@ -7,14 +7,12 @@
     #define M_PWM_1 13
     #define M_ENCODE_1A 33
     #define M_ENCODE_1B 32
-
     //Right
     #define M_DIR_21 23
     #define M_DIR_22 14
     #define M_PWM_2 22
     #define M_ENCODE_2A 26
     #define M_ENCODE_2B 25
-
     //GPS
     #define GPS_RX 16
     #define GPS_TX 17
@@ -47,28 +45,73 @@
 #define BUF_SIZE 10
 
 //MoveControl config
-#define RUN_MOVE_CONTROL_DELAY 150 // Normal 10 untested
+#define RUN_MOVE_CONTROL_DELAY 30 // Normal 10 untested
 #define ACCELERATE_STEPS 1
+#define PID_LEFT_P 2
+#define PID_LEFT_I 5
+#define PID_LEFT_D 1
+#define PID_RIGHT_P 2
+#define PID_RIGHT_I 5
+#define PID_RIGHT_D 1
+#define PID_OUT_MIN -100
+#define PID_OUT_MAX 100
+#define PID_SAMPLETIME 30
 
 //ManualControl config
-#define RUN_MANUALCONTROL_DELAY 10
+#define RUN_MANUALCONTROL_DELAY 10 // normaly 10
 #define MANUALCONTROL_MAX_SPEED 1.0
 #define MANUALCONTROL_MAX_ROTATION 1.0
 
 // PS3 Controller
 // ESP32 MAC BL 24:62:AB:F2:4B:3A
 
-//Network config
+#define VOERDE
+
+//Network config DORTMUND
+#ifdef DORTMUND
 #define WLAN_SSID "Kleiax2"
 #define WLAN_PASSWORD "Punica-699"
 #define WLAN_IP 0x0101a8c0          //192.168.1.1
 #define WLAN_SUBNETMASK 0x0000FFFF  //255.255.0.0
 #define WLAN_GATEWAY 0x0100a8c0     //192.168.0.1
-// #define MQTT_SERVER 0x1300a8c0      //192.168.0.19
 #define MQTT_SERVER 0x0201a8c0      //192.168.1.2
 #define MQTT_PORT 1883
 #define MQTT_BUFFER_SITE 128
 #define NTP_SERVER 0x0201a8c0       //192.168.1.2
+#endif //DORTMUND
+
+//Network config FRENZY
+#ifdef FRENZY
+#define WLAN_SSID "GNX7EDD84-2"
+#define WLAN_PASSWORD "2W2ZLPJ9NVQ3"
+#define WLAN_IP 0x0F01a8c0          //192.168.1.15
+#define WLAN_SUBNETMASK 0x00FFFFFF  //255.255.255.0
+#define WLAN_GATEWAY 0xfe01a8c0     //192.168.1.254
+#define MQTT_SERVER 0x0201a8c0      //192.168.1.2
+#define MQTT_PORT 1883
+#define MQTT_BUFFER_SITE 128
+#define NTP_SERVER 0x0201a8c0       //192.168.1.2
+#endif //FRENZY 
+
+//Network config VOERDE
+#ifdef VOERDE
+// #define KELLER
+#ifndef KELLER
+    #define WLAN_SSID "Der Neue"
+    #define WLAN_PASSWORD "Punica-699"
+#endif // KELLLER
+#ifdef KELLER
+    #define WLAN_SSID "Keller-Wlan"
+    #define WLAN_PASSWORD "Punica-699"
+#endif // KELLER
+#define WLAN_IP 0x4db2a8c0          //192.168.178.77
+#define WLAN_SUBNETMASK 0x00FFFFFF  //255.255.255.0
+#define WLAN_GATEWAY 0x01b2a8c0     //192.168.178.1
+#define MQTT_SERVER 0x4eb2a8c0      //192.168.178.78
+#define MQTT_PORT 1883
+#define MQTT_BUFFER_SITE 128
+#define NTP_SERVER 0x01b2a8c0       //192.168.178.1
+#endif // VOERDE
 
 // GPS config
 #define GPS_BAUD 9600

src/driveModi/captureRoute.cpp

@@ -20,7 +20,5 @@ void CaptureRoute::runCaptureRoute() {
     last_millis = millis();
 
     //Add Point to route
-    Point p;
+    this->route->addCurrentLocationToRoute();
-    p.lat = this.
-    this->route->addPoint()
 }

src/driveModi/consolControl.cpp

@@ -0,0 +1,40 @@
+#include "consolControl.h"
+#include <string>
+
+ConsolControl::ConsolControl() {
+    this->debug = new DebugMqtt("ConsolControl");
+}
+
+void ConsolControl::init(MoveControl *MoveControl) {
+    this->moveControl = moveControl;
+    this->left_pid = this->moveControl->getPID(0);
+    this->right_pid = this->moveControl->getPID(1);
+}
+
+void ConsolControl::run() {
+    PID *selectedPID;
+
+    if (Serial.available()) {
+        char cmd = Serial.read();
+        if (cmd == 'l') {
+            selectedPID = this->left_pid;
+        } else if (cmd == 'r') {
+            selectedPID = this->right_pid;
+        }
+
+        double p = Serial.parseFloat();
+        double i = Serial.parseFloat();
+        double d = Serial.parseFloat();
+
+        if (p && i && d) {
+            Serial.println("ConsolControl changeDate");
+            selectedPID->SetTunings(p, i, d);
+        } else {
+            printManual();
+        }
+    }
+}
+
+void printManual() {
+    Serial.println("r 12.0 13.0 4.0 // site p i d");
+}

src/driveModi/consolControl.h

@@ -0,0 +1,24 @@
+#ifndef CONSOL_CONTROL_H
+#define CONSOL_CONTROL_H
+
+#include "moveControl.h"
+#include "debugMqtt.h"
+
+class ConsolControl {
+    public:
+        ConsolControl();
+        void init(MoveControl *moveControl);
+        void run();
+
+    private:
+        void printManual();
+
+        MoveControl *moveControl;
+        DebugMqtt *debug;
+        PID *left_pid;
+        PID *right_pid;
+
+
+};
+
+#endif // CONSOL_CONTROL_H

src/driveModi/manualControl.cpp

@@ -48,7 +48,8 @@ void ManualControl::runManualControl() {
 
     switch (this->controlMode) {
         case ControMode::halt:
-            this->reset();
+            this->moveControl->setDrivingStatus(DrivingStatus::stop);
+            //this->reset();
             break;
 
         case ControMode::joystick:
@@ -66,14 +67,16 @@ void ManualControl::runManualControl() {
 }
 
 void ManualControl::driveWithControllerJoystick() {
+    this->moveControl->setDrivingStatus(DrivingStatus::drive);
     int8_t x = Ps3.data.analog.stick.lx;
     int8_t y = Ps3.data.analog.stick.ly;
 
     double value_per_step = MANUALCONTROL_MAX_SPEED * 2 / 256;
     this->moveControl->setSpeed((y * -1) * value_per_step);
-    char str[64];
+    this->moveControl->setDrivingStatus(DrivingStatus::drive);
-    sprintf(str, "%f", (y * -1) * value_per_step);
+    // char str[64];
-    debug->sendMsg(Loglevel::debug, str);
+    // sprintf(str, "driveWithJoy: %f", (y * -1) * value_per_step);
+    // debug->sendMsg(Loglevel::debug, str); // Kommt 1 raus bei vollausschlag
 
     value_per_step = MANUALCONTROL_MAX_ROTATION * 2 / 256;
     this->moveControl->setRotationspeed(x * value_per_step);

src/main.cpp

@@ -11,6 +11,7 @@
 #include "driveModi/autopilot.h"
 #include "driveModi/captureRoute.h"
 #include "driveModi/manualControl.h"
+#include "driveModi/consolControl.h"
 #include "moveControl.h"
 #include "route.h"
 #include "debugMqtt.h"
@@ -27,11 +28,12 @@ Speedometer speedometer_left;
 Speedometer speedometer_right;
 MoveControl moveController;
 Route route;
-DebugMqtt debug("main");
+DebugMqtt debugger("main");
 
 ManualControl manualControl;
 CaptureRoute captureRoute;
 Autopilot autopilot;
+ConsolControl consolControl;
 
 IPAddress local_IP(WLAN_IP);
 IPAddress gateway(WLAN_GATEWAY);
@@ -42,7 +44,7 @@ WiFiClient wifi_client;
 PubSubClient mqtt_client(wifi_client);
 
 int controller_battery = -1;
-enum DriveMode {autopilot_e, captureRoute_e, manualControl_e};
+enum DriveMode {autopilot_e, captureRoute_e, manualControl_e, consolControl_e};
 DriveMode driveMode = manualControl_e;
 
 
@@ -57,7 +59,7 @@ void setup() {
     // Connect to Wi-Fi network with SSID and password
     Serial.print("Connecting to ");
     Serial.println(WLAN_SSID);
-    WiFi.begin("Kleiax2", "Punica-699");
+    WiFi.begin(WLAN_SSID, WLAN_PASSWORD);
     while (WiFi.status() != WL_CONNECTED) {
         delay(500);
         Serial.print(".");
@@ -96,6 +98,7 @@ void setup() {
     manualControl.init(&moveController, &left_motor, &right_motor);
     captureRoute.init(&route, &moveController, &left_motor, &right_motor);
     autopilot.init(&route, &moveController);
+    consolControl.init(&moveController);
 }
 
 void loop() {
@@ -122,6 +125,10 @@ void loop() {
             autopilot.runAutopilot();
             break;
 
+        case consolControl_e:
+            consolControl.run();
+            break;
+
         default:
             break;
     }
@@ -154,17 +161,22 @@ void callbackControllerAction() {
         switch (driveMode) {
             case manualControl_e:
             driveMode = DriveMode::captureRoute_e;
-            debug.sendMsg(Loglevel::info, "New driveMode = CaptureRoute");
+            debugger.sendMsg(Loglevel::info, "New driveMode = CaptureRoute");
             break;
 
         case captureRoute_e:
             driveMode = DriveMode::autopilot_e;
-            debug.sendMsg(Loglevel::info, "New driveMode = Autopilot");
+            debugger.sendMsg(Loglevel::info, "New driveMode = Autopilot");
             break;
 
         case autopilot_e:
+            driveMode = DriveMode::consolControl_e;
+            debugger.sendMsg(Loglevel::info, "New driveMode = ConsolControl");
+            break;
+
+        case consolControl_e:
             driveMode = DriveMode::manualControl_e;
-            debug.sendMsg(Loglevel::info, "New driveMode = ManualControl");
+            debugger.sendMsg(Loglevel::info, "New driveMode = ManualControl");
             break;
 
         default:
@@ -175,7 +187,7 @@ void callbackControllerAction() {
 
 void callbackControllerConnect() {
     Serial.println("Controller connected to ESP32");
-    debug.sendMsg(Loglevel::info, "Controller connected to ESP32");
+    debugger.sendMsg(Loglevel::info, "Controller connected to ESP32");
 }
 
 void controllerPrintBattery() {

src/moveControl.cpp

@@ -4,6 +4,24 @@
 
 MoveControl::MoveControl() {
     this->debug = new DebugMqtt("MoveControl");
+    this->left_pid = new PID(   &this->wheelspeed_left,
+                                &this->left_pid_out,
+                                &this->wheelspeed_left_target,
+                                PID_LEFT_P, PID_LEFT_I, PID_LEFT_D,
+                                DIRECT);
+    this->right_pid = new PID(  &this->wheelspeed_right,
+                                &this->left_pid_out,
+                                &this->wheelspeed_right_target,
+                                PID_RIGHT_P, PID_RIGHT_I, PID_RIGHT_D,
+                                DIRECT);
+
+    this->left_pid->SetOutputLimits(PID_OUT_MIN, PID_OUT_MAX);
+    this->left_pid->SetSampleTime(PID_SAMPLETIME);
+    this->left_pid->SetMode(AUTOMATIC);
+
+    this->right_pid->SetOutputLimits(PID_OUT_MIN, PID_OUT_MAX);
+    this->right_pid->SetSampleTime(PID_SAMPLETIME);
+    this->right_pid->SetMode(AUTOMATIC);                                
 }
 
 void MoveControl::init(MotorControl *left_motor, MotorControl *right_motor,
@@ -17,18 +35,24 @@ void MoveControl::init(MotorControl *left_motor, MotorControl *right_motor,
 }
 
 void MoveControl::runMoveControl() {
+    this->right_pid->Compute();
+    this->left_pid->Compute();
+
     static uint64_t last_millis = 0;
     if (millis() - last_millis < RUN_MOVE_CONTROL_DELAY) {
         return;
     }
     last_millis = millis();
 
+    this->updateWheelSpeed();
     this->calcWheelSpeed();
     this->regulateMotors();
 
-    char str[64];
+    // char str[128];
-    sprintf(str, "x_speed: %3.2f, rotation_speed: %3.2f, tar_left: %3.2f, tar_right: %3.2f", this->x_speed, this->rotation_speed, this->wheelspeed_left_target, this->wheelspeed_right_target);
+    // sprintf(str, "x_speed: %3.2f, rotation_speed: %3.2f, tar_left: %3.2f, tar_right: %3.2f",
-    debug->sendMsg(Loglevel::debug, str);
+    //     this->x_speed, this->rotation_speed, this->wheelspeed_left_target,
+    //     this->wheelspeed_right_target);
+    // debug->sendMsg(Loglevel::debug, str);
 }
 
 void MoveControl::setDrivingStatus(DrivingStatus status) {
@@ -37,13 +61,23 @@ void MoveControl::setDrivingStatus(DrivingStatus status) {
 
 void MoveControl::setSpeed(double speed) {
     this->x_speed = speed;
-    Serial.printf("New speed: %f in moveControll.cpp \n", speed);
+    // Serial.printf("New speed: %f in moveControll.cpp \n", speed);
 }
 
 void MoveControl::setRotationspeed(double speed) {
     this->rotation_speed = speed;
 }
 
+PID* MoveControl::getPID(uint8_t i) {
+    if (i == 0) {
+        return this->left_pid;
+    } else if (i == 1) {
+        return this->right_pid;
+    } else {
+        return nullptr;
+    }
+}
+
 void MoveControl::calcWheelSpeed() {
     /* original formula:
        (1 / r) / 1  b \ / x \ = / Xl \
@@ -68,134 +102,19 @@ void MoveControl::regulateMotors() {
         this->right_motor->setTargetPower(0);
         break;
 
-    case DrivingStatus::forward :
+    case DrivingStatus::drive :
-        ratio = this->wheelspeed_left_target / this->wheelspeed_right_target;
+        this->left_motor->setTargetPower( (int8_t) this->left_pid_out);
-
+        this->right_motor->setTargetPower( (int8_t) this->right_pid_out);
-        // Left motor
+        
-        // Too slow
-        if (this->left_speedometer->getSpeed() < this->wheelspeed_left_target
-            && this->left_speedometer->getSpeed() <= (this->right_speedometer->getSpeed() * ratio)) {
-
-            this->left_motor->setTargetPower(this->left_motor->getTargetPower() + ACCELERATE_STEPS);
-
-        // Too fast
-        } else if (this->left_speedometer->getSpeed() > this->wheelspeed_left_target
-                    && this->left_speedometer->getSpeed() >= (this->right_speedometer->getSpeed() * ratio)) {
-
-            this->left_motor->setTargetPower(this->left_motor->getTargetPower() - ACCELERATE_STEPS);
-        }
-
-        // Right motor
-        // Too slow
-        if (this->right_speedometer->getSpeed() < this->wheelspeed_right_target
-            && (this->right_speedometer->getSpeed() * ratio) <= this->left_speedometer->getSpeed()) {
-
-            this->right_motor->setTargetPower(this->right_motor->getTargetPower() + ACCELERATE_STEPS);
-
-        // Too fast
-        } else if (this->right_speedometer->getSpeed() > this->wheelspeed_right_target
-                    && (this->right_speedometer->getSpeed() * ratio) >= this->left_speedometer->getSpeed()) {
-
-            this->right_motor->setTargetPower(this->right_motor->getTargetPower() - ACCELERATE_STEPS);
-        }
-        break;
-
-    case DrivingStatus::backward :
-        ratio = this->wheelspeed_left_target / this->wheelspeed_right_target;
-
-        // Left motor
-        // Too slow
-        if (this->left_speedometer->getSpeed() > this->wheelspeed_left_target
-            && this->left_speedometer->getSpeed() >= (this->right_speedometer->getSpeed() * ratio)) {
-
-            this->left_motor->setTargetPower(this->left_motor->getTargetPower() - ACCELERATE_STEPS);
-
-        // Too fast
-        } else if (this->left_speedometer->getSpeed() < this->wheelspeed_left_target
-                    && this->left_speedometer->getSpeed() <= (this->right_speedometer->getSpeed() * ratio)) {
-
-            this->left_motor->setTargetPower(this->left_motor->getTargetPower() + ACCELERATE_STEPS);
-        }
-
-        // Right motor
-        // Too slow
-        if (this->right_speedometer->getSpeed() > this->wheelspeed_right_target
-            && (this->right_speedometer->getSpeed() * ratio) >= this->left_speedometer->getSpeed()) {
-
-            this->right_motor->setTargetPower(this->right_motor->getTargetPower() - ACCELERATE_STEPS);
-
-        // Too fast
-        } else if (this->right_speedometer->getSpeed() < this->wheelspeed_right_target
-                    && (this->right_speedometer->getSpeed() * ratio) <= this->left_speedometer->getSpeed()) {
-
-            this->right_motor->setTargetPower(this->right_motor->getTargetPower() + ACCELERATE_STEPS);
-        }
-        break;
-
-    case DrivingStatus::rotateLeft :
-        // Left motor
-        // Too slow
-        if (this->left_speedometer->getSpeed() > this->wheelspeed_left_target
-            && this->left_speedometer->getSpeed() >= this->right_speedometer->getSpeed()) {
-
-            this->left_motor->setTargetPower(this->left_motor->getTargetPower() - ACCELERATE_STEPS);
-
-        // Too fast
-        } else if (this->left_speedometer->getSpeed() < this->wheelspeed_left_target
-                    && this->left_speedometer->getSpeed() <= this->right_speedometer->getSpeed()) {
-
-            this->left_motor->setTargetPower(this->left_motor->getTargetPower() + ACCELERATE_STEPS);
-        }
-
-        // Right motor
-        // Too slow
-        if (this->right_speedometer->getSpeed() < this->wheelspeed_right_target
-            && this->right_speedometer->getSpeed() <= this->left_speedometer->getSpeed()) {
-
-            this->right_motor->setTargetPower(this->right_motor->getTargetPower() + ACCELERATE_STEPS);
-
-        // Too fast
-        } else if (this->right_speedometer->getSpeed() > this->wheelspeed_right_target
-                    && this->right_speedometer->getSpeed() >= this->left_speedometer->getSpeed()) {
-
-            this->right_motor->setTargetPower(this->right_motor->getTargetPower() - ACCELERATE_STEPS);
-        }
-        break;
-
-    case DrivingStatus::rotateRight :
-        // Left motor
-        // Too slow
-        if (this->left_speedometer->getSpeed() < this->wheelspeed_left_target
-            && this->left_speedometer->getSpeed() <= this->right_speedometer->getSpeed()) {
-
-            this->left_motor->setTargetPower(this->left_motor->getTargetPower() + ACCELERATE_STEPS);
-
-        // Too fast
-        } else if (this->left_speedometer->getSpeed() > this->wheelspeed_left_target
-                    && this->left_speedometer->getSpeed() >= this->right_speedometer->getSpeed()) {
-
-            this->left_motor->setTargetPower(this->left_motor->getTargetPower() - ACCELERATE_STEPS);
-        }
-
-        // Right motor
-        // Too slow
-        if (this->right_speedometer->getSpeed() > this->wheelspeed_right_target
-            && this->right_speedometer->getSpeed() >= this->left_speedometer->getSpeed()) {
-
-            this->right_motor->setTargetPower(this->right_motor->getTargetPower() - ACCELERATE_STEPS);
-
-        // Too fast
-        } else if (this->right_speedometer->getSpeed() < this->wheelspeed_right_target
-                    && this->right_speedometer->getSpeed() <= this->left_speedometer->getSpeed()) {
-
-            this->right_motor->setTargetPower(this->right_motor->getTargetPower() + ACCELERATE_STEPS);
-        }
         break;
 
     default:
         Serial.println("Wrong drivingState in MoveControl::regulateMotors");
         break;
     }
+}
 
-
+void MoveControl::updateWheelSpeed() {
+    this->wheelspeed_left = this->left_speedometer->getSpeed();
+    this->wheelspeed_right = this->right_speedometer->getSpeed();
 }

src/moveControl.h

@@ -2,6 +2,7 @@
 #define MOVE_CONTROL_H
 
 #include <cstdint>
+#include <PID_v1.h>
 
 #include "hardware/motorControl.h"
 #include "hardware/speedometer.h"
@@ -9,10 +10,7 @@
 #include "debugMqtt.h"
 
 enum DrivingStatus {stop,
-                    forward,
+                    drive};
-                    backward,
-                    rotateLeft,
-                    rotateRight};
 
 
 class MoveControl {
@@ -24,10 +22,13 @@ class MoveControl {
         void setDrivingStatus(DrivingStatus status);
         void setSpeed(double speed);
         void setRotationspeed(double speed);
+        
+        PID* getPID(uint8_t i);
 
     private:
         void calcWheelSpeed();
         void regulateMotors();
+        void updateWheelSpeed();
 
         DebugMqtt *debug;
 
@@ -38,11 +39,19 @@ class MoveControl {
 
         DrivingStatus driving_status = DrivingStatus::stop;
 
+        PID *left_pid;
+        PID *right_pid;
+
         double x_speed = 0;
         double rotation_speed = 0;
 
         double wheelspeed_left_target = 0;
         double wheelspeed_right_target = 0;
+        double wheelspeed_left = 0;
+        double wheelspeed_right = 0;
+        double left_pid_out;
+        double right_pid_out;
+
 };
 
 #endif // MOVE_CONTROL_H