diff --git a/platformio.ini b/platformio.ini index 3cd232c..e6fdc08 100644 --- a/platformio.ini +++ b/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 diff --git a/src/config.h b/src/config.h index d063631..c2fb8bb 100644 --- a/src/config.h +++ b/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 diff --git a/src/driveModi/captureRoute.cpp b/src/driveModi/captureRoute.cpp index 19e4407..3c98427 100644 --- a/src/driveModi/captureRoute.cpp +++ b/src/driveModi/captureRoute.cpp @@ -20,7 +20,5 @@ void CaptureRoute::runCaptureRoute() { last_millis = millis(); //Add Point to route - Point p; - p.lat = this. - this->route->addPoint() + this->route->addCurrentLocationToRoute(); } \ No newline at end of file diff --git a/src/driveModi/consolControl.cpp b/src/driveModi/consolControl.cpp new file mode 100644 index 0000000..52aa9fc --- /dev/null +++ b/src/driveModi/consolControl.cpp @@ -0,0 +1,40 @@ +#include "consolControl.h" +#include + +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"); +} \ No newline at end of file diff --git a/src/driveModi/consolControl.h b/src/driveModi/consolControl.h new file mode 100644 index 0000000..9fe3ec0 --- /dev/null +++ b/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 \ No newline at end of file diff --git a/src/driveModi/manualControl.cpp b/src/driveModi/manualControl.cpp index fcf78ac..752028e 100644 --- a/src/driveModi/manualControl.cpp +++ b/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]; - sprintf(str, "%f", (y * -1) * value_per_step); - debug->sendMsg(Loglevel::debug, str); + this->moveControl->setDrivingStatus(DrivingStatus::drive); + // char str[64]; + // 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); diff --git a/src/main.cpp b/src/main.cpp index a728a9a..b9a11fd 100644 --- a/src/main.cpp +++ b/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() { diff --git a/src/moveControl.cpp b/src/moveControl.cpp index 41751d6..1558ff9 100644 --- a/src/moveControl.cpp +++ b/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]; - 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); - debug->sendMsg(Loglevel::debug, str); + // 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); + // 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 : - 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::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); - } + case DrivingStatus::drive : + this->left_motor->setTargetPower( (int8_t) this->left_pid_out); + this->right_motor->setTargetPower( (int8_t) this->right_pid_out); + 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(); } \ No newline at end of file diff --git a/src/moveControl.h b/src/moveControl.h index 8ebb663..71f9264 100644 --- a/src/moveControl.h +++ b/src/moveControl.h @@ -2,6 +2,7 @@ #define MOVE_CONTROL_H #include +#include #include "hardware/motorControl.h" #include "hardware/speedometer.h" @@ -9,10 +10,7 @@ #include "debugMqtt.h" enum DrivingStatus {stop, - forward, - backward, - rotateLeft, - rotateRight}; + drive}; 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 \ No newline at end of file