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

Added consolControl & PID

Browse Source
This commit is contained in:
kleiax
2021-07-26 20:19:33 +02:00
parent 70f6566365
commit 084c046e8c
9 changed files with 202 additions and 153 deletions
Download Patch File Download Diff File Expand all files Collapse all files
platformio.ini
+3 -2
View File
@@ -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
+49 -6
View File
@@ -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
+1 -3
View File
@@ -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();
}
src/driveModi/consolControl.cpp
+40
View File
@@ -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
+24
View File
@@ -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
+7 -4
View File
@@ -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);
src/main.cpp
+19 -7
View File
@@ -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
+46 -127
View File
@@ -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();
}
src/moveControl.h
+13 -4
View File
@@ -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,
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