From c741546ba1568971e53150671ecd06c8d9b9732b Mon Sep 17 00:00:00 2001 From: Alexander Klein Date: Wed, 24 Nov 2021 16:47:29 +0100 Subject: [PATCH] improve pid and other stuff --- doc/PinBelegungen.txt | 30 ++++++++++ doc/statesAutopilot.uxf | 13 ++++ src/config.h | 18 +++--- src/driveModi/captureRoute.cpp | 14 ++--- src/driveModi/manualControl.cpp | 1 - src/main.cpp | 102 +++++++++++++++++++++----------- src/moveControl.cpp | 38 ++++++++---- 7 files changed, 152 insertions(+), 64 deletions(-) create mode 100644 doc/PinBelegungen.txt create mode 100644 doc/statesAutopilot.uxf diff --git a/doc/PinBelegungen.txt b/doc/PinBelegungen.txt new file mode 100644 index 0000000..62981a2 --- /dev/null +++ b/doc/PinBelegungen.txt @@ -0,0 +1,30 @@ +ESP32 Rover Pinbelegung + + 3V3 GND + x 23 DirB1 + x 22 DirB2 + x x + x x + x 21 +INTA1 32 GND +INTA2 33 19 +INTB1 25 18 +INTB2 26 5 +PWMA 27 x +PWMB 14 x +DirA1 12 4 + GND x +DirA2 13 2 + x 15 + x x + CMD x + 5V USB x + + +Connector Encoder + +- 2 1 + +OB 4 3 - +3V3 6 [5] OA + 8 7 + 10 9 \ No newline at end of file diff --git a/doc/statesAutopilot.uxf b/doc/statesAutopilot.uxf new file mode 100644 index 0000000..32eec97 --- /dev/null +++ b/doc/statesAutopilot.uxf @@ -0,0 +1,13 @@ +10UMLSpecialState40202020type=initialRelation402013030lt=<-110;10;10;10UMLState1501010040search next point +bg=greenUMLSpecialState180904040bg=green +type=decisionRelation190403070lt=<-10;50;10;10UMLState15018010040detect alignment +bg=greenRelation19012012080lt=<- +[distance <= 5m]10;60;10;10UMLObject200840590Autopilot + +valign=topRelation2109014040lt=<- +[distance > 5m]120;20;10;20UMLState3309010040Error +bg=redUMLState15027010040correct alignment +bg=greenUMLState15036010040drive to point +bg=greenRelation1902103080lt=<-10;60;10;10Relation1903003080lt=<-10;60;10;10Relation9028080120lt=<-60;10;10;10;10;100;60;100UMLState44010410300detect alignment +valign=topUMLSpecialState460602020type=finalRelation470609030lt=->10;10;70;10UMLState5405011040drive 1m forwardUMLState54014011040drive 2m backwardUMLState54023011040drive 1m forwardUMLState70023012040calculate straight lineRelation580803080lt=<-10;60;10;10Relation5801703080lt=<-10;60;10;10Relation6205808030lt=<-60;10;10;10UMLSpecialState7501502020type=terminationRelation7501603090lt=<-10;10;10;70UMLState440320410260correct alignment +valign=topUMLState54037019040calculate relativ target positionRelation4703809030lt=->10;10;70;10UMLSpecialState4603802020type=finalUMLState58046012040rotate x degreeRelation6304003080lt=<-10;60;10;10Relation6304903080lt=<-10;60;10;10UMLSpecialState6305502020type=termination \ No newline at end of file diff --git a/src/config.h b/src/config.h index ba53854..46c965a 100644 --- a/src/config.h +++ b/src/config.h @@ -46,14 +46,14 @@ #define BUF_SIZE 10 //MoveControl config -#define RUN_MOVE_CONTROL_DELAY 30 // Normal 10 untested +#define RUN_MOVE_CONTROL_DELAY 10 #define ACCELERATE_STEPS 1 #define PID_LEFT_P 40 -#define PID_LEFT_I 20 -#define PID_LEFT_D 10 +#define PID_LEFT_I 0 +#define PID_LEFT_D 0 #define PID_RIGHT_P 40 -#define PID_RIGHT_I 20 -#define PID_RIGHT_D 10 +#define PID_RIGHT_I 0 +#define PID_RIGHT_D 0 #define PID_OUT_MIN -100 #define PID_OUT_MAX 100 #define PID_SAMPLETIME 30 @@ -63,6 +63,10 @@ #define MANUALCONTROL_MAX_SPEED 1.0 #define MANUALCONTROL_MAX_ROTATION 1.0 +//MQTT global config +#define MQTT_BUFFER_SITE 128 +#define MQTT_TIME_RECONNECT 2500 + // PS3 Controller // ESP32 MAC BL 24:62:AB:F2:4B:3A @@ -77,7 +81,6 @@ #define WLAN_GATEWAY 0x0100a8c0 //192.168.0.1 #define MQTT_SERVER 0x0701A8C0 //192.168.1.7 #define MQTT_PORT 1883 -#define MQTT_BUFFER_SITE 128 #define MQTT_AUTH #define MQTT_USER "kleiax" #define MQTT_PASSWORD "p?{$_~5%hBM7wrcFkr55KWr#" @@ -94,7 +97,6 @@ #define WLAN_GATEWAY 0x0100a8c0 //192.168.0.1 #define MQTT_SERVER 0xD84016AC // 172.22.64.216 #define MQTT_PORT 1883 -#define MQTT_BUFFER_SITE 128 #define NTP_SERVER "2.de.pool.ntp.org" #endif //HW1 @@ -107,7 +109,6 @@ #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 @@ -120,7 +121,6 @@ #define WLAN_GATEWAY 0x0101a8c0 //192.168.1.1 #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 //TPMOBIL diff --git a/src/driveModi/captureRoute.cpp b/src/driveModi/captureRoute.cpp index 3c98427..fb6bfbd 100644 --- a/src/driveModi/captureRoute.cpp +++ b/src/driveModi/captureRoute.cpp @@ -11,14 +11,14 @@ void CaptureRoute::init(Route *route, MoveControl *moveControl, MotorControl *mo } void CaptureRoute::runCaptureRoute() { - this->runManualControl(); + // this->runManualControl(); - static uint64_t last_millis = 0; - if (millis() - last_millis < RUN_CAPTuRE_ROUTE_DELAY) { - return; - } - last_millis = millis(); + // static uint64_t last_millis = 0; + // if (millis() - last_millis < RUN_CAPTuRE_ROUTE_DELAY) { + // return; + // } + // last_millis = millis(); //Add Point to route - this->route->addCurrentLocationToRoute(); + // this->route->addCurrentLocationToRoute(); } \ No newline at end of file diff --git a/src/driveModi/manualControl.cpp b/src/driveModi/manualControl.cpp index 752028e..f5478a6 100644 --- a/src/driveModi/manualControl.cpp +++ b/src/driveModi/manualControl.cpp @@ -49,7 +49,6 @@ void ManualControl::runManualControl() { switch (this->controlMode) { case ControMode::halt: this->moveControl->setDrivingStatus(DrivingStatus::stop); - //this->reset(); break; case ControMode::joystick: diff --git a/src/main.cpp b/src/main.cpp index d1c5015..b20e962 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -16,13 +16,27 @@ #include "route.h" #include "debugMqtt.h" -//TODO: Mal vernünftig Programmieren lernen. - void callbackControllerAction(); void callbackControllerConnect(); void controllerPrintBattery(); -void reconnectMqtt(); +bool reconnectMqtt(); + +// TODO: This is a temporary function for testing +void checkTimes(bool set, uint16_t maxTime,const char *name) { + static uint64_t lastMillis = 0; + if (set) { + lastMillis = millis(); + } else { + uint64_t pastTime = millis() - lastMillis; + if (pastTime > maxTime) { + Serial.print("Function: "); + Serial.print(name); + Serial.print("-Time: "); + Serial.println(pastTime); + } + } +} MotorControl left_motor; MotorControl right_motor; @@ -45,6 +59,7 @@ IPAddress subnet(WLAN_SUBNETMASK); WiFiClient wifi_client; PubSubClient mqtt_client(wifi_client); +uint64_t lastReconnectAttempt = 0; int controller_battery = -1; enum DriveMode {autopilot_e, captureRoute_e, manualControl_e, consolControl_e}; DriveMode driveMode = manualControl_e; @@ -77,7 +92,8 @@ void setup() { mqtt_client.setServer(MQTT_SERVER, MQTT_PORT); reconnectMqtt(); - configTime(3600, 3600, "192.168.1.2"); + //FIXME: make it from config.h + configTime(3600, 3600, "2.de.pool.ntp.org"); DebugMqtt::init(&mqtt_client, Loglevel::debug); DebugMqtt::initRealMillis(); @@ -101,25 +117,54 @@ void setup() { manualControl.init(&moveController, &left_motor, &right_motor); captureRoute.init(&route, &moveController, &left_motor, &right_motor); autopilot.init(&route, &moveController); - Serial.println("main 1"); consolControl.init(&moveController); - Serial.println("main 2"); + + //Test checkTimes function + checkTimes(true, 0, ""); + delay(2); + checkTimes(false, 1, "Test checkTimes"); + + //Mqtt set down Sockettimeout + mqtt_client.setSocketTimeout(1); } void loop() { - if (!mqtt_client.connected()) - reconnectMqtt(); + if (!mqtt_client.connected()) { + long now = millis(); + if (now - lastReconnectAttempt > MQTT_TIME_RECONNECT) { + lastReconnectAttempt = now; + // Attempt to reconnect + if (reconnectMqtt()) { + lastReconnectAttempt = 0; + } + } + } else { + // Client connected + checkTimes(true, 0, ""); mqtt_client.loop(); + checkTimes(false, 100, "mqttloop"); + } + checkTimes(true, 0, ""); left_motor.runMotorControl(); + checkTimes(false, 100, "left_motor"); + checkTimes(true, 0, ""); right_motor.runMotorControl(); + checkTimes(false, 100, "right_motor"); + checkTimes(true, 0, ""); speedometer_left.runSpeedometer(); + checkTimes(false, 100, "left_speed"); + checkTimes(true, 0, ""); speedometer_right.runSpeedometer(); - route.runRoute(); + checkTimes(false, 100, "right_speed"); + // TODO: Auskommentiert weil macht vielleicht komische Sachen + // route.runRoute(); switch (driveMode) { case manualControl_e: + checkTimes(true, 0, ""); manualControl.runManualControl(); + checkTimes(false, 100, "manualControl run"); break; case captureRoute_e: @@ -139,37 +184,26 @@ void loop() { } } -void reconnectMqtt() { - // Loop until reconnection - while (!mqtt_client.connected()) { - Serial.print("Attempting MQTT connection..."); - // Create a random client ID - String clientId = "ESP32Rover-"; - clientId += String(random(0xffff), HEX); - // Attempt to connect - #ifdef MQTT_AUTH - if (mqtt_client.connect(clientId.c_str(), MQTT_USER, MQTT_PASSWORD)) { - #endif //MQTT_AUTH - #ifndef MQTT_AUTH - if (mqtt_client.connect(clientId.c_str())) { - #endif //MQTT_AUTH - Serial.println("connected"); - // Once connected, publish an announcement... - mqtt_client.publish("Rover/Info", "Connected to Mqtt-Broker"); - } else { - Serial.print("failed, rc="); - Serial.print(mqtt_client.state()); - Serial.println(" try again in 5 seconds"); - // Wait 5 seconds before retrying - delay(5000); - } +bool reconnectMqtt() { + // Create a random client ID + String clientId = "ESP32Rover-"; + clientId += String(random(0xffff), HEX); + #ifdef MQTT_AUTH + if (mqtt_client.connect(clientId.c_str(), MQTT_USER, MQTT_PASSWORD)) { + #endif //MQTT_AUTH + #ifndef MQTT_AUTH + if (mqtt_client.connect(clientId.c_str())) { + #endif //MQTT_AUTH + // Once connected, publish an announcement... + mqtt_client.publish("Rover/Info", "Connected to Mqtt-Broker"); } + return mqtt_client.connected(); } void callbackControllerAction() { if (Ps3.event.button_down.ps) { switch (driveMode) { - case manualControl_e: + case manualControl_e: driveMode = DriveMode::captureRoute_e; debugger.sendMsg(Loglevel::info, "New driveMode = CaptureRoute"); break; diff --git a/src/moveControl.cpp b/src/moveControl.cpp index ebd330a..61f755e 100644 --- a/src/moveControl.cpp +++ b/src/moveControl.cpp @@ -35,10 +35,7 @@ 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; + static uint64_t last_millis = 0; if (millis() - last_millis < RUN_MOVE_CONTROL_DELAY) { return; } @@ -46,13 +43,21 @@ void MoveControl::runMoveControl() { this->updateWheelSpeed(); this->calcWheelSpeed(); + + this->right_pid->Compute(); + this->left_pid->Compute(); + this->regulateMotors(); - char str[128]; - sprintf(str, "x_speed: %3.2f, rotation_speed: %3.2f, left: %3.2f, right: %3.2f", - this->x_speed, this->rotation_speed, this->wheelspeed_left, - this->wheelspeed_right); - debug->sendMsg(Loglevel::debug, str); + static uint32_t functioncalls = 0; + functioncalls++; + if (functioncalls % 20 == 0) { + char str[128]; + sprintf(str, "x_speed: %3.2f, rotation_speed: %3.2f, left: %3.2f, right: %3.2f", + this->x_speed, this->rotation_speed, this->wheelspeed_left, + this->wheelspeed_right); + debug->sendMsg(Loglevel::debug, str); + } } void MoveControl::setDrivingStatus(DrivingStatus status) { @@ -60,12 +65,19 @@ void MoveControl::setDrivingStatus(DrivingStatus status) { } void MoveControl::setSpeed(double speed) { - this->x_speed = speed; + if (speed < 0.2 && speed > -0.2) + this->x_speed = 0; + else + this->x_speed = speed; // Serial.printf("New speed: %f in moveControll.cpp \n", speed); } void MoveControl::setRotationspeed(double speed) { - this->rotation_speed = speed; + if (speed < 0.2 && speed > -0.2) + this->rotation_speed = 0; + else + this->rotation_speed = speed; + } void MoveControl::setPidTunings(uint8_t side, double p, double i, double d) { @@ -91,8 +103,8 @@ void MoveControl::calcWheelSpeed() { this->wheelspeed_right_target = (A * this->x_speed + B * this->rotation_speed) * (WHEEL_DIAMETER / 2); this->wheelspeed_left_target = (A * this->x_speed + (-B) * this->rotation_speed) * (WHEEL_DIAMETER / 2); - debug->writeToInflux("moveControl", "wheelspeed_right_target", this->wheelspeed_right_target); - debug->writeToInflux("moveControl", "wheelspeed_left_target", this->wheelspeed_left_target); + //debug->writeToInflux("moveControl", "wheelspeed_right_target", this->wheelspeed_right_target); + //debug->writeToInflux("moveControl", "wheelspeed_left_target", this->wheelspeed_left_target); } void MoveControl::regulateMotors() {