kleiax/Bachelorarbeit-Rover
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases 1 Wiki Activity

improve pid and other stuff

Browse Source
This commit is contained in:
kleiax
2021-11-24 16:47:29 +01:00
parent f086bfa6b3
commit c741546ba1
7 changed files with 152 additions and 64 deletions
Download Patch File Download Diff File Expand all files Collapse all files
doc/PinBelegungen.txt
+30
View File
@@ -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
doc/statesAutopilot.uxf
+13
View File
@@ -0,0 +1,13 @@
<diagram program="umletino" version="14.4.0-SNAPSHOT"><zoom_level>10</zoom_level><element><id>UMLSpecialState</id><coordinates><x>40</x><y>20</y><w>20</w><h>20</h></coordinates><panel_attributes>type=initial</panel_attributes><additional_attributes></additional_attributes></element><element><id>Relation</id><coordinates><x>40</x><y>20</y><w>130</w><h>30</h></coordinates><panel_attributes>lt=&lt;-</panel_attributes><additional_attributes>110;10;10;10</additional_attributes></element><element><id>UMLState</id><coordinates><x>150</x><y>10</y><w>100</w><h>40</h></coordinates><panel_attributes>search next point
bg=green</panel_attributes><additional_attributes></additional_attributes></element><element><id>UMLSpecialState</id><coordinates><x>180</x><y>90</y><w>40</w><h>40</h></coordinates><panel_attributes>bg=green
type=decision</panel_attributes><additional_attributes></additional_attributes></element><element><id>Relation</id><coordinates><x>190</x><y>40</y><w>30</w><h>70</h></coordinates><panel_attributes>lt=&lt;-</panel_attributes><additional_attributes>10;50;10;10</additional_attributes></element><element><id>UMLState</id><coordinates><x>150</x><y>180</y><w>100</w><h>40</h></coordinates><panel_attributes>detect alignment
bg=green</panel_attributes><additional_attributes></additional_attributes></element><element><id>Relation</id><coordinates><x>190</x><y>120</y><w>120</w><h>80</h></coordinates><panel_attributes>lt=&lt;-
[distance &lt;= 5m]</panel_attributes><additional_attributes>10;60;10;10</additional_attributes></element><element><id>UMLObject</id><coordinates><x>20</x><y>0</y><w>840</w><h>590</h></coordinates><panel_attributes>Autopilot
valign=top</panel_attributes><additional_attributes></additional_attributes></element><element><id>Relation</id><coordinates><x>210</x><y>90</y><w>140</w><h>40</h></coordinates><panel_attributes>lt=&lt;-
[distance &gt; 5m]</panel_attributes><additional_attributes>120;20;10;20</additional_attributes></element><element><id>UMLState</id><coordinates><x>330</x><y>90</y><w>100</w><h>40</h></coordinates><panel_attributes>Error
bg=red</panel_attributes><additional_attributes></additional_attributes></element><element><id>UMLState</id><coordinates><x>150</x><y>270</y><w>100</w><h>40</h></coordinates><panel_attributes>correct alignment
bg=green</panel_attributes><additional_attributes></additional_attributes></element><element><id>UMLState</id><coordinates><x>150</x><y>360</y><w>100</w><h>40</h></coordinates><panel_attributes>drive to point
bg=green</panel_attributes><additional_attributes></additional_attributes></element><element><id>Relation</id><coordinates><x>190</x><y>210</y><w>30</w><h>80</h></coordinates><panel_attributes>lt=&lt;-</panel_attributes><additional_attributes>10;60;10;10</additional_attributes></element><element><id>Relation</id><coordinates><x>190</x><y>300</y><w>30</w><h>80</h></coordinates><panel_attributes>lt=&lt;-</panel_attributes><additional_attributes>10;60;10;10</additional_attributes></element><element><id>Relation</id><coordinates><x>90</x><y>280</y><w>80</w><h>120</h></coordinates><panel_attributes>lt=&lt;-</panel_attributes><additional_attributes>60;10;10;10;10;100;60;100</additional_attributes></element><element><id>UMLState</id><coordinates><x>440</x><y>10</y><w>410</w><h>300</h></coordinates><panel_attributes>detect alignment
valign=top</panel_attributes><additional_attributes></additional_attributes></element><element><id>UMLSpecialState</id><coordinates><x>460</x><y>60</y><w>20</w><h>20</h></coordinates><panel_attributes>type=final</panel_attributes><additional_attributes></additional_attributes></element><element><id>Relation</id><coordinates><x>470</x><y>60</y><w>90</w><h>30</h></coordinates><panel_attributes>lt=-&gt;</panel_attributes><additional_attributes>10;10;70;10</additional_attributes></element><element><id>UMLState</id><coordinates><x>540</x><y>50</y><w>110</w><h>40</h></coordinates><panel_attributes>drive 1m forward</panel_attributes><additional_attributes></additional_attributes></element><element><id>UMLState</id><coordinates><x>540</x><y>140</y><w>110</w><h>40</h></coordinates><panel_attributes>drive 2m backward</panel_attributes><additional_attributes></additional_attributes></element><element><id>UMLState</id><coordinates><x>540</x><y>230</y><w>110</w><h>40</h></coordinates><panel_attributes>drive 1m forward</panel_attributes><additional_attributes></additional_attributes></element><element><id>UMLState</id><coordinates><x>700</x><y>230</y><w>120</w><h>40</h></coordinates><panel_attributes>calculate straight line</panel_attributes><additional_attributes></additional_attributes></element><element><id>Relation</id><coordinates><x>580</x><y>80</y><w>30</w><h>80</h></coordinates><panel_attributes>lt=&lt;-</panel_attributes><additional_attributes>10;60;10;10</additional_attributes></element><element><id>Relation</id><coordinates><x>580</x><y>170</y><w>30</w><h>80</h></coordinates><panel_attributes>lt=&lt;-</panel_attributes><additional_attributes>10;60;10;10</additional_attributes></element><element><id>Relation</id><coordinates><x>620</x><y>580</y><w>80</w><h>30</h></coordinates><panel_attributes>lt=&lt;-</panel_attributes><additional_attributes>60;10;10;10</additional_attributes></element><element><id>UMLSpecialState</id><coordinates><x>750</x><y>150</y><w>20</w><h>20</h></coordinates><panel_attributes>type=termination</panel_attributes><additional_attributes></additional_attributes></element><element><id>Relation</id><coordinates><x>750</x><y>160</y><w>30</w><h>90</h></coordinates><panel_attributes>lt=&lt;-</panel_attributes><additional_attributes>10;10;10;70</additional_attributes></element><element><id>UMLState</id><coordinates><x>440</x><y>320</y><w>410</w><h>260</h></coordinates><panel_attributes>correct alignment
valign=top</panel_attributes><additional_attributes></additional_attributes></element><element><id>UMLState</id><coordinates><x>540</x><y>370</y><w>190</w><h>40</h></coordinates><panel_attributes>calculate relativ target position</panel_attributes><additional_attributes></additional_attributes></element><element><id>Relation</id><coordinates><x>470</x><y>380</y><w>90</w><h>30</h></coordinates><panel_attributes>lt=-&gt;</panel_attributes><additional_attributes>10;10;70;10</additional_attributes></element><element><id>UMLSpecialState</id><coordinates><x>460</x><y>380</y><w>20</w><h>20</h></coordinates><panel_attributes>type=final</panel_attributes><additional_attributes></additional_attributes></element><element><id>UMLState</id><coordinates><x>580</x><y>460</y><w>120</w><h>40</h></coordinates><panel_attributes>rotate x degree</panel_attributes><additional_attributes></additional_attributes></element><element><id>Relation</id><coordinates><x>630</x><y>400</y><w>30</w><h>80</h></coordinates><panel_attributes>lt=&lt;-</panel_attributes><additional_attributes>10;60;10;10</additional_attributes></element><element><id>Relation</id><coordinates><x>630</x><y>490</y><w>30</w><h>80</h></coordinates><panel_attributes>lt=&lt;-</panel_attributes><additional_attributes>10;60;10;10</additional_attributes></element><element><id>UMLSpecialState</id><coordinates><x>630</x><y>550</y><w>20</w><h>20</h></coordinates><panel_attributes>type=termination</panel_attributes><additional_attributes></additional_attributes></element></diagram>
src/config.h
+9 -9
View File
@@ -46,14 +46,14 @@
#define BUF_SIZE 10 #define BUF_SIZE 10
//MoveControl config //MoveControl config
#define RUN_MOVE_CONTROL_DELAY 30 // Normal 10 untested #define RUN_MOVE_CONTROL_DELAY 10
#define ACCELERATE_STEPS 1 #define ACCELERATE_STEPS 1
#define PID_LEFT_P 40 #define PID_LEFT_P 40
#define PID_LEFT_I 20 #define PID_LEFT_I 0
#define PID_LEFT_D 10 #define PID_LEFT_D 0
#define PID_RIGHT_P 40 #define PID_RIGHT_P 40
#define PID_RIGHT_I 20 #define PID_RIGHT_I 0
#define PID_RIGHT_D 10 #define PID_RIGHT_D 0
#define PID_OUT_MIN -100 #define PID_OUT_MIN -100
#define PID_OUT_MAX 100 #define PID_OUT_MAX 100
#define PID_SAMPLETIME 30 #define PID_SAMPLETIME 30
@@ -63,6 +63,10 @@
#define MANUALCONTROL_MAX_SPEED 1.0 #define MANUALCONTROL_MAX_SPEED 1.0
#define MANUALCONTROL_MAX_ROTATION 1.0 #define MANUALCONTROL_MAX_ROTATION 1.0
//MQTT global config
#define MQTT_BUFFER_SITE 128
#define MQTT_TIME_RECONNECT 2500
// PS3 Controller // PS3 Controller
// ESP32 MAC BL 24:62:AB:F2:4B:3A // ESP32 MAC BL 24:62:AB:F2:4B:3A
@@ -77,7 +81,6 @@
#define WLAN_GATEWAY 0x0100a8c0 //192.168.0.1 #define WLAN_GATEWAY 0x0100a8c0 //192.168.0.1
#define MQTT_SERVER 0x0701A8C0 //192.168.1.7 #define MQTT_SERVER 0x0701A8C0 //192.168.1.7
#define MQTT_PORT 1883 #define MQTT_PORT 1883
#define MQTT_BUFFER_SITE 128
#define MQTT_AUTH #define MQTT_AUTH
#define MQTT_USER "kleiax" #define MQTT_USER "kleiax"
#define MQTT_PASSWORD "p?{$_~5%hBM7wrcFkr55KWr#" #define MQTT_PASSWORD "p?{$_~5%hBM7wrcFkr55KWr#"
@@ -94,7 +97,6 @@
#define WLAN_GATEWAY 0x0100a8c0 //192.168.0.1 #define WLAN_GATEWAY 0x0100a8c0 //192.168.0.1
#define MQTT_SERVER 0xD84016AC // 172.22.64.216 #define MQTT_SERVER 0xD84016AC // 172.22.64.216
#define MQTT_PORT 1883 #define MQTT_PORT 1883
#define MQTT_BUFFER_SITE 128
#define NTP_SERVER "2.de.pool.ntp.org" #define NTP_SERVER "2.de.pool.ntp.org"
#endif //HW1 #endif //HW1
@@ -107,7 +109,6 @@
#define WLAN_GATEWAY 0xfe01a8c0 //192.168.1.254 #define WLAN_GATEWAY 0xfe01a8c0 //192.168.1.254
#define MQTT_SERVER 0x0201a8c0 //192.168.1.2 #define MQTT_SERVER 0x0201a8c0 //192.168.1.2
#define MQTT_PORT 1883 #define MQTT_PORT 1883
#define MQTT_BUFFER_SITE 128
#define NTP_SERVER 0x0201a8c0 //192.168.1.2 #define NTP_SERVER 0x0201a8c0 //192.168.1.2
#endif //FRENZY #endif //FRENZY
@@ -120,7 +121,6 @@
#define WLAN_GATEWAY 0x0101a8c0 //192.168.1.1 #define WLAN_GATEWAY 0x0101a8c0 //192.168.1.1
#define MQTT_SERVER 0x0201a8c0 //192.168.1.2 #define MQTT_SERVER 0x0201a8c0 //192.168.1.2
#define MQTT_PORT 1883 #define MQTT_PORT 1883
#define MQTT_BUFFER_SITE 128
#define NTP_SERVER 0x0201a8c0 //192.168.1.2 #define NTP_SERVER 0x0201a8c0 //192.168.1.2
#endif //TPMOBIL #endif //TPMOBIL
src/driveModi/captureRoute.cpp
+7 -7
View File
@@ -11,14 +11,14 @@ void CaptureRoute::init(Route *route, MoveControl *moveControl, MotorControl *mo
} }
void CaptureRoute::runCaptureRoute() { void CaptureRoute::runCaptureRoute() {
this->runManualControl(); // this->runManualControl();
static uint64_t last_millis = 0; // static uint64_t last_millis = 0;
if (millis() - last_millis < RUN_CAPTuRE_ROUTE_DELAY) { // if (millis() - last_millis < RUN_CAPTuRE_ROUTE_DELAY) {
return; // return;
} // }
last_millis = millis(); // last_millis = millis();
//Add Point to route //Add Point to route
this->route->addCurrentLocationToRoute(); // this->route->addCurrentLocationToRoute();
} }
src/driveModi/manualControl.cpp
-1
View File
@@ -49,7 +49,6 @@ void ManualControl::runManualControl() {
switch (this->controlMode) { switch (this->controlMode) {
case ControMode::halt: case ControMode::halt:
this->moveControl->setDrivingStatus(DrivingStatus::stop); this->moveControl->setDrivingStatus(DrivingStatus::stop);
//this->reset();
break; break;
case ControMode::joystick: case ControMode::joystick:
src/main.cpp
+68 -34
View File
@@ -16,13 +16,27 @@
#include "route.h" #include "route.h"
#include "debugMqtt.h" #include "debugMqtt.h"
//TODO: Mal vernünftig Programmieren lernen.
void callbackControllerAction(); void callbackControllerAction();
void callbackControllerConnect(); void callbackControllerConnect();
void controllerPrintBattery(); 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 left_motor;
MotorControl right_motor; MotorControl right_motor;
@@ -45,6 +59,7 @@ IPAddress subnet(WLAN_SUBNETMASK);
WiFiClient wifi_client; WiFiClient wifi_client;
PubSubClient mqtt_client(wifi_client); PubSubClient mqtt_client(wifi_client);
uint64_t lastReconnectAttempt = 0;
int controller_battery = -1; int controller_battery = -1;
enum DriveMode {autopilot_e, captureRoute_e, manualControl_e, consolControl_e}; enum DriveMode {autopilot_e, captureRoute_e, manualControl_e, consolControl_e};
DriveMode driveMode = manualControl_e; DriveMode driveMode = manualControl_e;
@@ -77,7 +92,8 @@ void setup() {
mqtt_client.setServer(MQTT_SERVER, MQTT_PORT); mqtt_client.setServer(MQTT_SERVER, MQTT_PORT);
reconnectMqtt(); 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::init(&mqtt_client, Loglevel::debug);
DebugMqtt::initRealMillis(); DebugMqtt::initRealMillis();
@@ -101,25 +117,54 @@ void setup() {
manualControl.init(&moveController, &left_motor, &right_motor); manualControl.init(&moveController, &left_motor, &right_motor);
captureRoute.init(&route, &moveController, &left_motor, &right_motor); captureRoute.init(&route, &moveController, &left_motor, &right_motor);
autopilot.init(&route, &moveController); autopilot.init(&route, &moveController);
Serial.println("main 1");
consolControl.init(&moveController); 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() { void loop() {
if (!mqtt_client.connected()) if (!mqtt_client.connected()) {
reconnectMqtt(); 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(); mqtt_client.loop();
checkTimes(false, 100, "mqttloop");
}
checkTimes(true, 0, "");
left_motor.runMotorControl(); left_motor.runMotorControl();
checkTimes(false, 100, "left_motor");
checkTimes(true, 0, "");
right_motor.runMotorControl(); right_motor.runMotorControl();
checkTimes(false, 100, "right_motor");
checkTimes(true, 0, "");
speedometer_left.runSpeedometer(); speedometer_left.runSpeedometer();
checkTimes(false, 100, "left_speed");
checkTimes(true, 0, "");
speedometer_right.runSpeedometer(); speedometer_right.runSpeedometer();
route.runRoute(); checkTimes(false, 100, "right_speed");
// TODO: Auskommentiert weil macht vielleicht komische Sachen
// route.runRoute();
switch (driveMode) { switch (driveMode) {
case manualControl_e: case manualControl_e:
checkTimes(true, 0, "");
manualControl.runManualControl(); manualControl.runManualControl();
checkTimes(false, 100, "manualControl run");
break; break;
case captureRoute_e: case captureRoute_e:
@@ -139,37 +184,26 @@ void loop() {
} }
} }
void reconnectMqtt() { bool reconnectMqtt() {
// Loop until reconnection // Create a random client ID
while (!mqtt_client.connected()) { String clientId = "ESP32Rover-";
Serial.print("Attempting MQTT connection..."); clientId += String(random(0xffff), HEX);
// Create a random client ID #ifdef MQTT_AUTH
String clientId = "ESP32Rover-"; if (mqtt_client.connect(clientId.c_str(), MQTT_USER, MQTT_PASSWORD)) {
clientId += String(random(0xffff), HEX); #endif //MQTT_AUTH
// Attempt to connect #ifndef MQTT_AUTH
#ifdef MQTT_AUTH if (mqtt_client.connect(clientId.c_str())) {
if (mqtt_client.connect(clientId.c_str(), MQTT_USER, MQTT_PASSWORD)) { #endif //MQTT_AUTH
#endif //MQTT_AUTH // Once connected, publish an announcement...
#ifndef MQTT_AUTH mqtt_client.publish("Rover/Info", "Connected to Mqtt-Broker");
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);
}
} }
return mqtt_client.connected();
} }
void callbackControllerAction() { void callbackControllerAction() {
if (Ps3.event.button_down.ps) { if (Ps3.event.button_down.ps) {
switch (driveMode) { switch (driveMode) {
case manualControl_e: case manualControl_e:
driveMode = DriveMode::captureRoute_e; driveMode = DriveMode::captureRoute_e;
debugger.sendMsg(Loglevel::info, "New driveMode = CaptureRoute"); debugger.sendMsg(Loglevel::info, "New driveMode = CaptureRoute");
break; break;
src/moveControl.cpp
+25 -13
View File
@@ -35,10 +35,7 @@ void MoveControl::init(MotorControl *left_motor, MotorControl *right_motor,
} }
void MoveControl::runMoveControl() { void MoveControl::runMoveControl() {
this->right_pid->Compute(); static uint64_t last_millis = 0;
this->left_pid->Compute();
static uint64_t last_millis = 0;
if (millis() - last_millis < RUN_MOVE_CONTROL_DELAY) { if (millis() - last_millis < RUN_MOVE_CONTROL_DELAY) {
return; return;
} }
@@ -46,13 +43,21 @@ void MoveControl::runMoveControl() {
this->updateWheelSpeed(); this->updateWheelSpeed();
this->calcWheelSpeed(); this->calcWheelSpeed();
this->right_pid->Compute();
this->left_pid->Compute();
this->regulateMotors(); this->regulateMotors();
char str[128]; static uint32_t functioncalls = 0;
sprintf(str, "x_speed: %3.2f, rotation_speed: %3.2f, left: %3.2f, right: %3.2f", functioncalls++;
this->x_speed, this->rotation_speed, this->wheelspeed_left, if (functioncalls % 20 == 0) {
this->wheelspeed_right); char str[128];
debug->sendMsg(Loglevel::debug, str); 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) { void MoveControl::setDrivingStatus(DrivingStatus status) {
@@ -60,12 +65,19 @@ void MoveControl::setDrivingStatus(DrivingStatus status) {
} }
void MoveControl::setSpeed(double speed) { 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); // Serial.printf("New speed: %f in moveControll.cpp \n", speed);
} }
void MoveControl::setRotationspeed(double 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) { 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_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); 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_right_target", this->wheelspeed_right_target);
debug->writeToInflux("moveControl", "wheelspeed_left_target", this->wheelspeed_left_target); //debug->writeToInflux("moveControl", "wheelspeed_left_target", this->wheelspeed_left_target);
} }
void MoveControl::regulateMotors() { void MoveControl::regulateMotors() {