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

Added debugMqtt and autoPilot

Browse Source
This commit is contained in:
kleiax
2021-05-06 14:17:46 +02:00
parent 15bce31197
commit b5f2288db2
11 changed files with 322 additions and 30 deletions
Download Patch File Download Diff File Expand all files Collapse all files
platformio.ini
+3
View File
@@ -11,10 +11,13 @@
[env:esp32doit-devkit-v1]
platform = espressif32
board = esp32doit-devkit-v1
board_build.partitions = no_ota.csv
framework = arduino
monitor_speed = 115200
monitor_port = COM12
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
src/autopilot.cpp
View File
src/autopilot.h
+20
View File
@@ -0,0 +1,20 @@
#ifndef AUTOPILOT_H
#define AUTOPILOT_H
#include <TinyGPS++.h>
#include "route.h"
#include "moveControl.h"
class Autopilot {
public:
Autopilot();
private:
Route *route;
MoveControl *moveControl;
TinyGPSPlus * gps;
};
#endif // AUTOPILOT_H
src/config.h
+16 -1
View File
@@ -47,4 +47,19 @@
#define ACCELERATE_STEPS 1
// PS3 Controller
// ESP32 MAC BL 24:62:AB:F2:4B:3A
// ESP32 MAC BL 24:62:AB:F2:4B:3A
//Max Values for speed and rotation
#define MAX_SPEED 1.0
#define MAX_ROTATION 1.0
//Wlan / Mqtt config
#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_PORT 1883
#define MQTT_BUFFER_SITE 50
src/debugMqtt.cpp
+59
View File
@@ -0,0 +1,59 @@
#include "debugMqtt.h"
PubSubClient* DebugMqtt::client;
Loglevel DebugMqtt::loglevel;
bool DebugMqtt::isInit = false;
char DebugMqtt::msg[MQTT_BUFFER_SITE];
char DebugMqtt::topic[MQTT_BUFFER_SITE];
DebugMqtt::DebugMqtt(const char* name) {
this->name = name;
}
void DebugMqtt::sendMsg(Loglevel loglevel, String topic, String msg) {
snprintf (DebugMqtt::msg, MQTT_BUFFER_SITE, "%s: %s",this->name ,msg.c_str());
this->sendData(loglevel, topic, DebugMqtt::msg);
}
void DebugMqtt::sendMsg(Loglevel loglevel, String msg) {
this->sendMsg(loglevel, "", msg);
}
void DebugMqtt::sendData(Loglevel loglevel, String topic, String data) {
if (!DebugMqtt::isInit) {return;}
if (loglevel <= DebugMqtt::loglevel && loglevel > Loglevel::none) {
snprintf (DebugMqtt::topic, MQTT_BUFFER_SITE, "%s%s", DebugMqtt::enum_to_string(loglevel).c_str(), topic.c_str());
snprintf (DebugMqtt::msg, MQTT_BUFFER_SITE, "%s", data.c_str());
client->publish(DebugMqtt::topic, DebugMqtt::msg);
}
}
void DebugMqtt::sendData(Loglevel loglevel, String data){
this->sendData(loglevel, "", data);
}
void DebugMqtt::init(PubSubClient *client, Loglevel loglevel) {
DebugMqtt::client = client;
DebugMqtt::loglevel = loglevel;
DebugMqtt::isInit = true;
}
void DebugMqtt::changeLoglevel(Loglevel loglevel) {
DebugMqtt::loglevel = loglevel;
}
String DebugMqtt::enum_to_string(Loglevel loglevel) {
switch(loglevel){
case Loglevel::error :
return "/Rover/Error";
case Loglevel::warn :
return "/Rover/Warn";
case Loglevel::info :
return "/Rover/Info";
case Loglevel::debug :
return "/Rover/Debug";
default:
return "INVALID ENUM";
}
}
src/debugMqtt.h
+38
View File
@@ -0,0 +1,38 @@
#ifndef DEBUG_MQTT_H
#define DEBUG_MQTT_H
#include <PubSubClient.h>
#include "config.h"
enum Loglevel { none,
error,
warn,
info,
debug};
class DebugMqtt {
public:
DebugMqtt(const char* name);
void sendMsg(Loglevel loglevel, String topic, String msg);
void sendMsg(Loglevel loglevel, String msg);
void sendData(Loglevel loglevel, String topic, String data);
void sendData(Loglevel loglevel, String data);
static void init(PubSubClient *client, Loglevel loglevel);
static void changeLoglevel(Loglevel loglevel);
private:
const char* name;
static String enum_to_string(Loglevel loglevel);
static PubSubClient* client;
static Loglevel loglevel;
static bool isInit;
static char msg[MQTT_BUFFER_SITE];
static char topic[MQTT_BUFFER_SITE];
};
#endif // DEBUG_MQTT_H
src/main.cpp
+83 -8
View File
@@ -1,21 +1,36 @@
#include <Arduino.h>
#include <Ps3Controller.h>
#include <WiFi.h>
#include <PubSubClient.h>
#include "config.h"
#include "motorControl.h"
#include "moveControl.h"
#include "speedometer.h"
#include "debugMqtt.h"
void callbackControllerAction();
void callbackControllerConnect();
void callbackControllerDisconnect();
void controllerPrintBattery();
void reconnectMqtt();
MotorControl left_motor;
MotorControl right_motor;
Speedometer speedometer_left;
Speedometer speedometer_right;
MoveControl moveController;
DebugMqtt debugger("main");
IPAddress local_IP(WLAN_IP);
IPAddress gateway(WLAN_GATEWAY);
IPAddress subnet(WLAN_SUBNETMASK);
IPAddress mqtt_server(MQTT_SERVER);
WiFiClient wifi_client;
PubSubClient mqtt_client(wifi_client);
uint64_t last_millis = 0;
int controller_battery = -1;
@@ -28,12 +43,37 @@ void driveWithControllerShoulderTrigger(uint8_t left, uint8_t right);
void changeDriveMode();
void setup() {
Serial.begin(115200); // make sure your Serial Monitor is also set at this baud rate.
Serial.begin(115200);
// Configures static IP address
if (!WiFi.config(local_IP, gateway, subnet)) {
Serial.println("STA Failed to configure");
}
// Connect to Wi-Fi network with SSID and password
Serial.print("Connecting to ");
Serial.println(WLAN_SSID);
WiFi.begin("Kleiax2", "Punica-699");
while (WiFi.status() != WL_CONNECTED) {
delay(500);
Serial.print(".");
}
// Print local IP address and start web server
Serial.println("");
Serial.println("WiFi connected.");
Serial.println("IP address: ");
Serial.println(WiFi.localIP());
mqtt_client.setServer(mqtt_server, MQTT_PORT);
reconnectMqtt();
DebugMqtt::init(&mqtt_client, Loglevel::debug);
Ps3.attach(callbackControllerAction);
Ps3.attachOnConnect(callbackControllerConnect);
Ps3.attachOnDisconnect(callbackControllerDisconnect);
Serial.println("\nReady to connect");
Serial.println("\nReady to connect a PS3 Controller... \n");
Ps3.begin();
left_motor.init(M_PWM_1, PWM_CHANNEL_M1, M_DIR_11, M_DIR_12);
@@ -46,11 +86,13 @@ void setup() {
}
void loop() {
if (!mqtt_client.connected()) {
reconnectMqtt();
}
mqtt_client.loop();
if (millis() - last_millis > 1000) {
Serial.printf("Speed L: %f, ", speedometer_left.getSpeed());
left_motor.toString();
Serial.printf("Speed R: %f, ", speedometer_right.getSpeed());
right_motor.toString();
debugger.sendMsg(Loglevel::info, "loop : eine Sekinde");
last_millis = millis();
}
@@ -64,6 +106,28 @@ void loop() {
moveController.runMoveControl();
}
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
if (mqtt_client.connect(clientId.c_str())) {
Serial.println("connected");
// Once connected, publish an announcement...
mqtt_client.publish("/Rover/Info", "Connected");
} else {
Serial.print("failed, rc=");
Serial.print(mqtt_client.state());
Serial.println(" try again in 5 seconds");
// Wait 5 seconds before retrying
delay(5000);
}
}
}
void callbackControllerAction() {
if (Ps3.event.button_down.r3) { controllerPrintBattery(); }
if (Ps3.event.button_down.l1) { moveController.setSpeed(speed -= 0.1); }
@@ -108,7 +172,11 @@ void controllerPrintBattery() {
void driveWithControllerJoystick(int8_t x, int8_t y) {
if (drive_mode != 1) return;
double value_per_step = MAX_SPEED * 2 / 256;
moveController.setSpeed((y * -1) * value_per_step);
value_per_step = MAX_ROTATION * 2 / 256;
moveController.setRotationspeed(x * value_per_step);
}
void driveWithControllerShoulderTrigger(uint8_t left, uint8_t right) {
@@ -131,4 +199,11 @@ void changeDriveMode() {
if (drive_mode == 2) {
moveController.setSpeed(0);
}
}
}
void getDis(double lat1, double lon1, double lat2, double lon2) {
double lat = (lat1 + lat2) / 2 * 0.01745;
double dx = 111.3 * cos(lat) * (lon1 - lon2);
double dy = 111.3 * (lat1 - lat2);
double erg = sqrt(dx * dx + dy * dy);
}
src/moveControl.cpp
+15 -14
View File
@@ -25,7 +25,7 @@ void MoveControl::runMoveControl() {
this->regulateMotors();
}
void MoveControl::setDrivingStatus(drivingStatus status) {
void MoveControl::setDrivingStatus(DrivingStatus status) {
this->driving_status = status;
}
@@ -53,13 +53,14 @@ void MoveControl::calcWheelSpeed() {
}
void MoveControl::regulateMotors() {
double ratio = 0;
switch (this->driving_status) {
case drivingStatus::stop :
case DrivingStatus::stop :
this->left_motor->setTargetPower(0);
this->right_motor->setTargetPower(0);
break;
case drivingStatus::straightForward :
case DrivingStatus::straightForward :
// Left motor
// Too slow
if (this->left_speedometer->getSpeed() < this->wheelspeed_left_target
@@ -89,7 +90,7 @@ void MoveControl::regulateMotors() {
}
break;
case drivingStatus::straightBackward :
case DrivingStatus::straightBackward :
// Left motor
// Too slow
if (this->left_speedometer->getSpeed() > this->wheelspeed_left_target
@@ -119,8 +120,8 @@ void MoveControl::regulateMotors() {
}
break;
case drivingStatus::arcForwardLeft : // Identical with arcForwardRight
double ratio = this->wheelspeed_left_target / this->wheelspeed_right_target;
case DrivingStatus::arcForwardLeft : // Identical with arcForwardRight
ratio = this->wheelspeed_left_target / this->wheelspeed_right_target;
// Left motor
// Too slow
@@ -151,8 +152,8 @@ void MoveControl::regulateMotors() {
}
break;
case drivingStatus::arcForwardRight : // Identical with arcForwardLeft
double ratio = this->wheelspeed_left_target / this->wheelspeed_right_target;
case DrivingStatus::arcForwardRight : // Identical with arcForwardLeft
ratio = this->wheelspeed_left_target / this->wheelspeed_right_target;
// Left motor
// Too slow
@@ -183,8 +184,8 @@ void MoveControl::regulateMotors() {
}
break;
case drivingStatus::arcBackwardLeft : // Identical with arcBackwardRight
double ratio = this->wheelspeed_left_target / this->wheelspeed_right_target;
case DrivingStatus::arcBackwardLeft : // Identical with arcBackwardRight
ratio = this->wheelspeed_left_target / this->wheelspeed_right_target;
// Left motor
// Too slow
@@ -215,8 +216,8 @@ void MoveControl::regulateMotors() {
}
break;
case drivingStatus::arcBackwardRight : // Identical with arcBackwardLeft
double ratio = this->wheelspeed_left_target / this->wheelspeed_right_target;
case DrivingStatus::arcBackwardRight : // Identical with arcBackwardLeft
ratio = this->wheelspeed_left_target / this->wheelspeed_right_target;
// Left motor
// Too slow
@@ -247,7 +248,7 @@ void MoveControl::regulateMotors() {
}
break;
case drivingStatus::rotateLeft :
case DrivingStatus::rotateLeft :
// Left motor
// Too slow
if (this->left_speedometer->getSpeed() > this->wheelspeed_left_target
@@ -277,7 +278,7 @@ void MoveControl::regulateMotors() {
}
break;
case drivingStatus::rotateRight :
case DrivingStatus::rotateRight :
// Left motor
// Too slow
if (this->left_speedometer->getSpeed() < this->wheelspeed_left_target
src/moveControl.h
+7 -7
View File
@@ -7,7 +7,7 @@
#include "speedometer.h"
#include "config.h"
enum drivingStatus {stop,
enum DrivingStatus {stop,
straightForward,
straightBackward,
arcForwardLeft,
@@ -17,10 +17,10 @@ enum drivingStatus {stop,
rotateLeft,
rotateRight};
enum regulateStatus {stop,
drive,
rotateLeft,
rotateRight};
// enum regulateStatus {stop,
// drive,
// rotateLeft,
// rotateRight};
class MoveControl {
public:
@@ -28,7 +28,7 @@ class MoveControl {
void init(MotorControl *left_motor, MotorControl *right_motor,
Speedometer *left_encoder, Speedometer *right_encoder);
void runMoveControl();
void setDrivingStatus(drivingStatus status);
void setDrivingStatus(DrivingStatus status);
void setSpeed(double speed);
void setRotationspeed(double speed);
@@ -41,7 +41,7 @@ class MoveControl {
Speedometer *left_speedometer;
Speedometer *right_speedometer;
drivingStatus driving_status = drivingStatus::stop;
DrivingStatus driving_status = DrivingStatus::stop;
double x_speed = 0;
double rotation_speed = 0;
src/route.cpp
+41
View File
@@ -0,0 +1,41 @@
#include "route.h"
Route::Route(){
}
void Route::addPoint(Point point) {
this->points.push_back(point);
this->count_points++;
}
void Route::delRoute() {
this->points.clear();
this->count_points = 0;
}
Point Route::startRoute() {
this->route_started = true;
this->route_finished = false;
this->it = this->points.begin();
return *it;
}
Point Route::getNextPoint() {
if (this->route_started && !route_finished) {
it++;
return *it;
}
if (it == this->points.end()) {
this->route_finished = true;
}
Point fail;
return fail;
}
uint16_t Route::getNumberOfPoints() {
return this->count_points;
}
src/route.h
+40
View File
@@ -0,0 +1,40 @@
#ifndef ROUTE_H
#define ROUTE_H
#include <cstdint>
#include <list>
#include "motorControl.h"
#include "speedometer.h"
#include "config.h"
struct Point{
float N = 0;
float E = 0;
};
class Route {
public:
Route();
void addPoint(Point point);
void delRoute();
void resetRoute();
Point startRoute();
Point getNextPoint();
uint16_t getNumberOfPoints();
private:
std::list<Point> points;
std::list<Point>::iterator it;
uint16_t count_points = 0;
bool route_started = false;
bool route_finished = false;
};
#endif // ROUTE_H