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

a lot of bullshit

Browse Source
This commit is contained in:
kleiax
2021-12-14 19:22:06 +01:00
parent 1fc5a75f33
commit cd29191bb4
27 changed files with 703 additions and 551 deletions
Download Patch File Download Diff File Expand all files Collapse all files
doc/Notes and TODOs/TODO allgemein.txt
+3
View File
@@ -4,3 +4,6 @@ Meine Libs von andren Abhängigkeiten lösen
Speedometer:
- Mitrechnen durchschnittliche aufrufzeit
Alle loops Nachgucken ob last_millis aktualisiert wird.
Mqtt kram auslagern
include/config.h
+2 -128
View File
@@ -1,136 +1,10 @@
// Global config file
// The front is where the boards are
//Pin config
//Left
#define M_DIR_11 27
#define M_DIR_12 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
//GPS Serial Pins
#define GPS_RX 16
#define GPS_TX 17
#define GPS_BAUD 9600
//Motors
#define LEFT_MOTOR 1
#define RIGHT_MOTOR 2
#define BOTH_MOTOR 3
//Directions
#define STOP 0
#define FORWARD 1
#define BACKWARD 2
//MotorControl config
#define RUN_MOTOR_CONTROL_DELAY 10 // Normal 10
#define PWM_FREQ 16000
#define PWM_RES 8
#define PWM_CHANNEL_M1 0
#define PWM_CHANNEL_M2 1
#define POWER_STEPS 2 // A total of 20 levels ( 100 / SPEED_STEPS ) * RUN_MOTOR_CONTROL_DELAY = 500ms
#define PWM_MIN 120
#define PWM_MAX 245 // Max 98% of 2^PWM_RES
//Speedometer config
#define RUN_SPEEDOMETER_DELAY 30 // Normal 30 untested
#define ENC_FILTER 1023 // 1023 is max
#define ENC_STEPS 1024
#define WHEEL_DIAMETER 0.1263 //meter
#define BUF_SIZE 10
//MoveControl config
#define RUN_MOVE_CONTROL_DELAY 10
#define ACCELERATE_STEPS 1
#define PID_LEFT_P 85
#define PID_LEFT_I 0
#define PID_LEFT_D 0
#define PID_RIGHT_P 75
#define PID_RIGHT_I 0
#define PID_RIGHT_D 0
#define PID_OUT_MIN -100
#define PID_OUT_MAX 100
#define PID_SAMPLETIME 30
//ManualControl config
#define RUN_MANUALCONTROL_DELAY 10 // normaly 10
#define MANUALCONTROL_MAX_SPEED 1.0 // m/s
#define MANUALCONTROL_MAX_ROTATION 7.0 // rad/s
//MQTT global config
#define MQTT_BUFFER_SITE 128
#define MQTT_TIME_RECONNECT 2500
// PS3 Controller
// ESP32 MAC BL 24:62:AB:F2:4B:3A
#define RHEDE
//Network config RHEDE
#ifdef RHEDE
#define WLAN_SSID "LebennigHuus"
#define WLAN_PASSWORD "Punica-699"
#define WLAN_IP 0x040BA8C0 //192.168.11.4
#define WLAN_SUBNETMASK 0x0080FFFF //255.255.128.0
#define WLAN_GATEWAY 0x0100a8c0 //192.168.0.1
#define MQTT_SERVER 0x0701A8C0 //192.168.1.7
#define MQTT_PORT 1883
#define MQTT_AUTH
#define MQTT_USER "kleiax"
#define MQTT_PASSWORD "p?{$_~5%hBM7wrcFkr55KWr#"
#define NTP_SERVER 0x0100a8c0 //192.168.0.1
#endif //RHEDE
//Network config HW1
#ifdef HW1
#define WLAN_SSID "hw1_gast"
#define WLAN_PASSWORD "KeineAhnung"
#define WLAN_IP 0x3400a8c0 //192.168.0.67
#define WLAN_SUBNETMASK 0x00FFFFFF //255.255.255.0
#define WLAN_GATEWAY 0x0100a8c0 //192.168.0.1
#define MQTT_SERVER 0xD84016AC // 172.22.64.216
#define MQTT_PORT 1883
#define NTP_SERVER "2.de.pool.ntp.org"
#endif //HW1
//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 NTP_SERVER 0x0201a8c0 //192.168.1.2
#endif //FRENZY
//Network config TPMOBIL
#ifdef TPMOBIL
#define WLAN_SSID "TP-Mobil"
#define WLAN_PASSWORD "Punica-699"
#define WLAN_IP 0x0F01a8c0 //192.168.1.15
#define WLAN_SUBNETMASK 0x00FFFFFF //255.255.255.0
#define WLAN_GATEWAY 0x0101a8c0 //192.168.1.1
#define MQTT_SERVER 0x0201a8c0 //192.168.1.2
#define MQTT_PORT 1883
#define NTP_SERVER 0x0201a8c0 //192.168.1.2
#endif //TPMOBIL
// GPS config
#define GPS_BAUD 9600
#define RUN_ROUTE_DELAY 330
// CaptureRoute config
#define RUN_CAPTuRE_ROUTE_DELAY 300
// Route config
#define DISTANCE_BETWEEN_POINTS 0.75 // Meter
include/moveControl.h
+119 -15
View File
@@ -1,47 +1,151 @@
/**
* @file moveControl.h
* @author Alexander Klein (alex@kleiax.de)
* @brief Contrains the MoveControl class
* @version 0.1
* @date 2021-12-14
*
* @copyright Copyright (c) 2021
*
*/
#ifndef MOVE_CONTROL_H
#define MOVE_CONTROL_H
#include <cstdint>
#include <PID_v1.h>
#include <Arduino.h>
#include "motorControl.h"
#include "speedometer.h"
#include "config.h"
#include "debugMqtt.h"
#include "debugTimes.h"
#include "moveControlConfig.h"
/**
* @brief used to set driving status
*
* When set to stop all motors are set to halt
*
*/
enum DrivingStatus {stop,
drive};
/**
* @brief This class manages the motors and the encoders
*
* The controler uses two PIDs to control the motors. The PIDs
* use the Speedometer class to get the current speed and the
* given target speed to calculate a new duty cycle for the motors.
*
*/
class MoveControl {
public:
/**
* @brief Construct a new Move Control object
*
* Initalize the motors, encoders and PIDs with given
* values in moveControlConfig.h
*/
MoveControl();
void init(MotorControl *left_motor, MotorControl *right_motor,
Speedometer *left_encoder, Speedometer *right_encoder);
/**
* @brief Destroy the Move Control object
*
* Stops the motors with emergencyStop()
*/
~MoveControl();
/**
* @brief Calls runMoveControl() to update all Values.
*
* Besides that this function calls the loop() functions for the motors and encodes.
* This function should be called every mainloop. If the delay is not reached, than the
* functions returns immediately.
* @see runMoveControl()
* @see setDelay()
*/
void loop();
/**
* @brief Noramly called repeatedly by loop() to calcluate new values.
*/
void runMoveControl();
/**
* @brief Set the DrivingStatus
*
* @see DrDrivingStatus
* @param status
*/
void setDrivingStatus(DrivingStatus status);
/**
* @brief Set the target speed
*
* If the given speed is close to zero, then the
* target speed is set to zero.
* @param speed in m/s
*/
void setSpeed(double speed);
/**
* @brief Set the rotationspeed
*
* If the given rotationspeed is close to zero, then the
* target rotationspeed is set to zero.
* @param speed rad/s
*/
void setRotationspeed(double speed);
/**
* @brief Set the pid tunings
*
* TODO: check if the function is working or indicates seg fault
*
* @param side 0 -> left, 1 -> right
* @param p
* @param i
* @param d
*/
void setPidTunings(uint8_t side, double p, double i, double d);
private:
void calcWheelSpeed();
void regulateMotors();
void updateWheelSpeed();
/**
* @brief Set the min delay between each loop
*
* @param delay time in Milliseconds
*/
void setDelay(uint8_t delay_) { delay = delay_; }
DebugMqtt *debug;
private:
/**
* @brief Converts values into wheel speeds
*
* Converts target speed and target rotationspeed into
* wheel speeds
*/
void calcTargetWheelSpeed();
/**
* @brief Set the target power to motors
*
* Checks if the driving_status is set to drive.
* If yes, then the motors get the pid_out values as targetpower.
* If no, then the motors target power is set to zero.
*/
void regulateMotors();
/**
* @brief Updates the wheel speeds with speedometer
*/
void updateCurrentWheelSpeed();
MotorControl *left_motor;
MotorControl *right_motor;
Speedometer *left_speedometer;
Speedometer *right_speedometer;
DrivingStatus driving_status = DrivingStatus::stop;
PID *left_pid;
PID *right_pid;
DrivingStatus driving_status = DrivingStatus::stop;
double x_speed = 0;
double rotation_speed = 0;
@@ -52,6 +156,6 @@ class MoveControl {
double left_pid_out;
double right_pid_out;
uint8_t delay = 30;
};
#endif // MOVE_CONTROL_H
include/moveControlConfig.h
+44
View File
@@ -0,0 +1,44 @@
/**
* @file moveControlConfig.h
* @author Alexander Klein (alex@kleiax.de)
* @brief This file inherits some configuartion for moveControl.h
* @version 0.1
* @date 2021-12-14
*
* @copyright Copyright (c) 2021
*
*/
//Pin config
//Left Motor
#define M_DIR_11 27
#define M_DIR_12 12
#define M_PWM_1 13
#define M_ENCODE_1A 33
#define M_ENCODE_1B 32
//Right Motor
#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
// PID config
#define PID_OUT_MIN -100
#define PID_OUT_MAX 100
#define PID_SAMPLETIME 30
// Left PID
#define PID_LEFT_P 85
#define PID_LEFT_I 0
#define PID_LEFT_D 0
// Right PID
#define PID_RIGHT_P 75
#define PID_RIGHT_I 0
#define PID_RIGHT_D 0
// PWM
#define PWM_CHANNEL_M1 0
#define PWM_CHANNEL_M2 1
// SPEEDOMETER
#define WHEEL_DIAMETER 0.1263
#define ENC_STEPS 1024
include/network.h
+36
View File
@@ -0,0 +1,36 @@
/**
* @file network.h
* @author Alexander Klein (alex@kleiax.de)
* @brief Some network and MQTT stuff
* @version 0.1
* @date 2021-12-14
*
* @copyright Copyright (c) 2021
*
*/
#ifndef NETWORK_H
#define NETWORK_H
#include <WiFi.h>
#include <PubSubClient.h>
#include "networkConfig.h"
#define MQTT_TIME_RECONNECT 2500
IPAddress local_IP;
extern IPAddress gateway;
extern IPAddress subnet;
extern IPAddress mqtt_server;
extern WiFiClient wifi_client;
PubSubClient mqtt_client(wifi_client);
void setIps();
void connectWifi();
void setupMQTT();
bool connectMQTT();
void checkMQTT();
#endif // NETWORK_H
include/networkConfig.h
+64
View File
@@ -0,0 +1,64 @@
#define HW1_2
//Network config RHEDE
#ifdef RHEDE
#define WLAN_SSID "LebennigHuus"
#define WLAN_PASSWORD "Punica-699"
#define WLAN_IP 0x040BA8C0 //192.168.11.4
#define WLAN_SUBNETMASK 0x0080FFFF //255.255.128.0
#define WLAN_GATEWAY 0x0100a8c0 //192.168.0.1
#define MQTT_SERVER 0x0701A8C0 //192.168.1.7
#define MQTT_PORT 1883
#define MQTT_AUTH
#define MQTT_USER "kleiax"
#define MQTT_PASSWORD "p?{$_~5%hBM7wrcFkr55KWr#"
#define NTP_SERVER 0x0100a8c0 //192.168.0.1
#endif //RHEDE
//Network config HW1
#ifdef HW1
#define WLAN_SSID "hw1_gast"
#define WLAN_PASSWORD "KeineAhnung"
#define WLAN_IP 0x3400a8c0 //192.168.0.67
#define WLAN_SUBNETMASK 0x00FFFFFF //255.255.255.0
#define WLAN_GATEWAY 0x0100a8c0 //192.168.0.1
#define MQTT_SERVER 0xD84016AC // 172.22.64.216
#define MQTT_PORT 1883
#define NTP_SERVER "2.de.pool.ntp.org"
#endif //HW1
//Network config HW1
#ifdef HW1_2
#define WLAN_SSID "hw1_gast"
#define WLAN_PASSWORD "KeineAhnung"
#define WLAN_IP "192.168.0.67"
#define WLAN_SUBNETMASK "255.255.255.0"
#define WLAN_GATEWAY "192.168.0.1"
#define MQTT_SERVER "172.22.64.216"
#define MQTT_PORT 1883
#define NTP_SERVER "2.de.pool.ntp.org"
#endif //HW1
//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 NTP_SERVER 0x0201a8c0 //192.168.1.2
#endif //FRENZY
//Network config TPMOBIL
#ifdef TPMOBIL
#define WLAN_SSID "TP-Mobil"
#define WLAN_PASSWORD "Punica-699"
#define WLAN_IP 0x0F01a8c0 //192.168.1.15
#define WLAN_SUBNETMASK 0x00FFFFFF //255.255.255.0
#define WLAN_GATEWAY 0x0101a8c0 //192.168.1.1
#define MQTT_SERVER 0x0201a8c0 //192.168.1.2
#define MQTT_PORT 1883
#define NTP_SERVER 0x0201a8c0 //192.168.1.2
#endif //TPMOBIL
lib/MotorControl/motorControl.h
+3
View File
@@ -95,6 +95,9 @@ class MotorControl {
/**
* @brief Set the Target Power
*
* If the given power is greater than 100 or smaller than -100, then
* this function only print an error to consol.
*
* @param power power in percent
*/
void setTargetPower(int8_t power);
src/driveModi/CaptureRoute/captureRoute.h
-22
View File
@@ -1,22 +0,0 @@
#ifndef CAPTURE_ROUTE_H
#define CAPTURE_ROUTE_H
#include "driveModi/manualControl.h"
#include "debugMqtt.h"
#include "route.h"
#include "config.h"
class CaptureRoute : ManualControl {
public:
CaptureRoute();
void init(Route *route, MoveControl *moveControl, MotorControl *motorControlLeft, MotorControl *motorControlRight);
void runCaptureRoute();
private:
Route *route;
DebugMqtt *debug;
};
#endif // CAPTURE_ROUTE_H
src/driveModi/ManualControl/manualControl.cpp
-125
View File
@@ -1,125 +0,0 @@
#include "manualControl.h"
ManualControl::ManualControl() {
this->debug = new DebugMqtt("ManualControl");
}
void ManualControl::init(MoveControl *moveControl, MotorControl *motorControlLeft, MotorControl *motorControlRight) {
debug->sendMsg(Loglevel::info, "Init...");
this->moveControl = moveControl;
this->motorControlLeft = motorControlLeft;
this->motorControlRight = motorControlRight;
debug->sendMsg(Loglevel::info, "Init finished!");
}
void ManualControl::runManualControl() {
//Cancel if delay is not reached
if (millis() - this->last_millis < RUN_MANUALCONTROL_DELAY) {
return;
}
//Reset if this function was called a long time ago
if (millis() - this->last_millis > 1000) {
this->reset();
controlMode = ControMode::halt;
debug->sendMsg(Loglevel::info, "Changed ControlMode to halt by Timereset");
}
this->last_millis = millis();
//Change controlMode
if (this->controlMode == ControMode::halt && Ps3.data.button.l3) {
this->reset();
controlMode = ControMode::joystick;
debug->sendMsg(Loglevel::info, "Changed ControlMode to Joystick by Gamepad");
} else if (this->controlMode == ControMode::halt && Ps3.data.button.r3) {
this->reset();
controlMode = ControMode::trigger;
debug->sendMsg(Loglevel::info, "Changed ControlMode to ShoulderTrigger by Gamepad");
} else if (this->controlMode == ControMode::joystick && Ps3.data.button.r3) {
this->reset();
controlMode = ControMode::trigger;
debug->sendMsg(Loglevel::info, "Changed ControlMode to ShoulderTrigger by Gamepad");
} else if (this->controlMode == ControMode::trigger && Ps3.data.button.l3) {
this->reset();
controlMode = ControMode::joystick;
debug->sendMsg(Loglevel::info, "Changed ControlMode to Joystick by Gamepad");
}
switch (this->controlMode) {
case ControMode::halt:
this->moveControl->setDrivingStatus(DrivingStatus::stop);
break;
case ControMode::joystick: {
// DebugTimes driveWithJoyDebug;
this->driveWithControllerJoystick();
// driveWithJoyDebug.stop("driveWithControllerJoystick", 100);
// DebugTimes moveControllDebug;
this->moveControl->runMoveControl();
// moveControllDebug.stop("moveControl", 100);
}
break;
case ControMode::trigger:
this->driveWithControllerShoulderTrigger();
break;
default:
break;
}
}
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);
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);
}
void ManualControl::changeDriveMode(ControMode drive_mode) {
this->reset();
controlMode = drive_mode;
debug->sendMsg(Loglevel::info, "Changed ControlMode by function");
}
void ManualControl::driveWithControllerShoulderTrigger() {
uint8_t nowL = Ps3.data.analog.button.l2;
uint8_t nowR = Ps3.data.analog.button.r2;;
nowL = map(nowL, 0, 255, 0, 100);
nowR = map(nowR, 0, 255, 0, 100);
// char str[64];
// sprintf(str, "nachher: nowL: %u, nowR: %u", nowL, nowR);
// debug->sendMsg(Loglevel::debug, str);
if (nowL != this->oldL) {
this->motorControlLeft->setTargetPower(nowL);
this->oldL = nowL;
}
if (nowR != this->oldR) {
this->motorControlRight->setTargetPower(nowR);
this->oldR = nowR;
}
}
void ManualControl::reset() {
this->oldL = 0;
this->oldR = 0;
motorControlLeft->setTargetPower(0);
motorControlRight->setTargetPower(0);
moveControl->setRotationspeed(0);
moveControl->setSpeed(0);
}
src/driveModi/ManualControl/manualControl.h
-37
View File
@@ -1,37 +0,0 @@
#ifndef MANUAL_CONTROL_H
#define MANUAL_CONTROL_H
#include <Ps3Controller.h>
#include "moveControl.h"
#include "motorControl.h"
#include "debugMqtt.h"
#include "debugTimes.h"
enum ControMode {halt, joystick, trigger};
class ManualControl {
public:
ManualControl();
void init(MoveControl *moveControl, MotorControl *motorControlLeft, MotorControl *motorControlRight);
void runManualControl();
void changeDriveMode(ControMode drive_mode);
private:
void driveWithControllerJoystick();
void driveWithControllerShoulderTrigger();
void reset();
MoveControl *moveControl;
MotorControl *motorControlLeft;
MotorControl *motorControlRight;
DebugMqtt *debug;
ControMode controlMode = ControMode::halt;
uint32_t last_millis = 0;
// Is needed for driveWithControllerShoulderTrigger()
uint8_t oldL = 0;
uint8_t oldR = 0;
};
#endif // MANUAL_CONTROL_H
src/driveModi/Autopilot/autopilot.cpp → src/driveModi/Modi/Autopilot/autopilot.cpp
+1 -1
View File
@@ -1,4 +1,4 @@
#include "driveModi/autopilot.h"
#include "driveModi/Modi/Autopilot/autopilot.h"
Autopilot::Autopilot() {
this->debug = new DebugMqtt("Autopilot");
src/driveModi/Autopilot/autopilot.h → src/driveModi/Modi/Autopilot/autopilot.h
+3 -1
View File
@@ -6,12 +6,14 @@
#include "route.h"
#include "moveControl.h"
#include "debugMqtt.h"
#include "driveModi/driveModi.h"
class Autopilot {
class Autopilot : DriveModi{
public:
Autopilot();
void init(Route *route, MoveControl *moveControl);
void loop();
void runAutopilot();
src/driveModi/CaptureRoute/captureRoute.cpp → src/driveModi/Modi/CaptureRoute/captureRoute.cpp
+3 -5
View File
@@ -1,12 +1,10 @@
#include "driveModi/captureRoute.h"
#include "driveModi/Modi/CaptureRoute/captureRoute.h"
CaptureRoute::CaptureRoute() {
this->debug = new DebugMqtt("CaptureRoute");
}
void CaptureRoute::init(Route *route, MoveControl *moveControl, MotorControl *motorControlLeft, MotorControl *motorControlRight) {
this->route = route;
ManualControl::init(moveControl, motorControlLeft, motorControlRight);
void CaptureRoute::init() {
}
src/driveModi/Modi/CaptureRoute/captureRoute.h
+21
View File
@@ -0,0 +1,21 @@
#ifndef CAPTURE_ROUTE_H
#define CAPTURE_ROUTE_H
#include "driveModi/Modi/ManualControl/manualControl.h"
// #include "route.h"
#include "config.h"
class CaptureRoute : ManualControl {
public:
CaptureRoute();
void init();
void runCaptureRoute();
private:
// Route *route;
// DebugMqtt *debug;
};
#endif // CAPTURE_ROUTE_H
src/driveModi/ConsolControl/consolControl.cpp → src/driveModi/Modi/ConsolControl/consolControl.cpp
View File
src/driveModi/ConsolControl/consolControl.h → src/driveModi/Modi/ConsolControl/consolControl.h
+2 -1
View File
@@ -4,9 +4,10 @@
#include <string>
#include "moveControl.h"
#include "driveModi/driveModi.h"
#include "debugMqtt.h"
class ConsolControl {
class ConsolControl : DriveModi{
public:
ConsolControl();
void init(MoveControl *moveControl);
src/driveModi/Modi/ManualControl/manualControl.cpp
+42
View File
@@ -0,0 +1,42 @@
/**
* @file manualControl.cpp
* @author Alexander Klein (alex@kleiax.de)
* @brief Implementation of the class manualControl.h
* @version 0.1
* @date 2021-12-13
*
* @copyright Copyright (c) 2021
*
*/
#include "manualControl.h"
ManualControl::ManualControl() { }
ManualControl::~ManualControl() {
this->moveControl->setSpeed(0);
this->moveControl->setRotationspeed(0);
this->moveControl->setDrivingStatus(DrivingStatus::stop);
}
void ManualControl::init(MoveControl *moveControl) {
this->moveControl = moveControl;
this->moveControl->setDrivingStatus(DrivingStatus::drive);
}
void ManualControl::loop() {
static uint32_t last_millis = 0;
if (millis() - last_millis < delay) {
return;
}
}
void ManualControl::runManualControl() {
int8_t x = Ps3.data.analog.stick.lx;
int8_t y = Ps3.data.analog.stick.ly;
double value_per_step = this->max_speed * 2 / 256;
this->moveControl->setSpeed((y * -1) * value_per_step);
value_per_step = this->max_rotation * 2 / 256;
this->moveControl->setRotationspeed(-x * value_per_step);
}
src/driveModi/Modi/ManualControl/manualControl.h
+91
View File
@@ -0,0 +1,91 @@
/**
* @file manualControl.h
* @author Alexander Klein (alex@kleiax.de)
* @brief A small class to drive the Rover by the controller joystick.
* @version 0.1
* @date 2021-12-13
*
* @copyright Copyright (c) 2021
*
*/
#ifndef MANUAL_CONTROL_H
#define MANUAL_CONTROL_H
#include <Ps3Controller.h>
#include "moveControl.h"
#include "driveModi/driveModi.h"
/**
* @brief Drive the Rover with a Joystick
*
* This class gets the x and y value from the PS3 controller
* and map the values to moveControl
*
* @see MoveControl
*/
class ManualControl : public DriveModi{
public:
ManualControl();
/**
* @brief Destroy the Manual Control object
* Set in moveControl speed and rotation to 0 and set DrivingStatus::stop
*/
~ManualControl();
/**
* @brief Initalize ManualControl
*
* set DrivingStatus::drive
*
* @param moveControl
*/
void init(MoveControl *moveControl);
/**
* @brief Calls runManualControl() to update all values.
*
* This function should be called every mainloop. If the delay is not reached, than the
* functions returns immediately.
* @see runSpeedometer()
* @see setDelay()
*/
void loop();
/**
* @brief Noramly called repeatedly by loop() to calcluate new values.
* Set new values for speed and rotation in moveControl
*/
void runManualControl();
/**
* @brief Set the min delay between each loop
*
* @param delay time in Milliseconds
*/
void setDelay(uint8_t delay_) { delay = delay_; }
/**
* @brief Set the max speed
*
* @param maxSpeed in m/s
*/
void setMaxSpeed(double maxSpeed) { max_speed = maxSpeed; }
/**
* @brief Set the max rotation
*
* @param maxRotation in rad/s (maybe)
*/
void setMaxRotation(double maxRotation) { max_rotation = maxRotation; }
private:
MoveControl *moveControl;
uint8_t delay = 10;
double max_speed = 1;
double max_rotation = 7;
};
#endif // MANUAL_CONTROL_H
src/driveModi/Modi/TestMode/testMode.cpp
View File
src/driveModi/Modi/TestMode/testMode.h
View File
src/driveModi/driveManager.cpp
+72
View File
@@ -0,0 +1,72 @@
/**
* @file driveManager.cpp
* @author Alexander Klein (alex@kleiax.de)
* @brief Implemention of the class driveManager.h.
* @version 0.1
* @date 2021-12-14
*
* @copyright Copyright (c) 2021
*
*/
#include "driveModi/driveManager.h"
DriveManager::DriveManager(MoveControl *moveControl) {
this->moveControl = moveControl;
}
void DriveManager::loop() {
this->currentModus->loop();
this->moveControl->loop();
}
void DriveManager::runDriveManager() {
}
void DriveManager::nextModus() {
}
void DriveManager::changeModus(Modi modus) {
//Set moveControl to a safe state
this->moveControl->setSpeed(0);
this->moveControl->setRotationspeed(0);
this->moveControl->setDrivingStatus(DrivingStatus::stop);
delete this->currentModus;
switch (modus) {
case Modi::Off:
this->currentModus = nullptr;
break;
case Modi::ManualControl: {
ManualControl *ptr = new ManualControl;
ptr->init(this->moveControl);
this->currentModus = ptr;
}
break;
case Modi::CaptureRoute:
this->currentModus = nullptr;
break;
case Modi::Autopilot:
this->currentModus = nullptr;
break;
case Modi::ConsolControl:
this->currentModus = nullptr;
break;
case Modi::TestMode:
this->currentModus = nullptr;
break;
default:
this->currentModus = nullptr;
break;
}
}
src/driveModi/driveManager.h
+51
View File
@@ -0,0 +1,51 @@
/**
* @file driveManager.h
* @author Alexander Klein (alex@kleiax.de)
* @brief Contains a class to switch the DriveModi
* @version 0.1
* @date 2021-12-14
*
* @copyright Copyright (c) 2021
*
*/
#ifndef DRIVE_MANAGER_H
#define DRIVE_MANAGER_H
#include "moveControl.h"
#include "driveModi/driveModi.h"
// All Drive Modi
#include "driveModi/Modi/ManualControl/manualControl.h"
#include "driveModi/Modi/CaptureRoute/captureRoute.h"
#include "driveModi/Modi/Autopilot/autopilot.h"
#include "driveModi/Modi/ConsolControl/consolControl.h"
#include "driveModi/Modi/TestMode/testMode.h"
enum class Modi {
Off,
ManualControl,
CaptureRoute,
Autopilot,
ConsolControl,
TestMode
};
class DriveManager {
public:
DriveManager(MoveControl *moveControl);
void loop();
void runDriveManager();
void nextModus();
void changeModus(Modi modus);
private:
MoveControl *moveControl;
DriveModi *currentModus;
};
#endif // DRIVE_MANAGER_H
src/driveModi/driveModi.h
+22
View File
@@ -0,0 +1,22 @@
/**
* @file driveModi.h
* @author Alexander Klein (alex@kleiax.de)
* @brief Contains a virtual class for all Modi
* @version 0.1
* @date 2021-12-14
*
* @copyright Copyright (c) 2021
*
*/
#ifndef DRIVEMODI_H
#define DRIVEMODI_H
class DriveModi {
public:
DriveModi(){}
virtual ~DriveModi(){}
virtual void loop() = 0;
};
#endif // DRIVEMODI_H
src/main.cpp
+11 -170
View File
@@ -1,208 +1,49 @@
#include <Arduino.h>
#include <Ps3Controller.h>
#include <WiFi.h>
#include <PubSubClient.h>
#include <time.h>
#include "config.h"
#include "motorControl.h"
#include "speedometer.h"
#include "driveModi/autopilot.h"
#include "driveModi/captureRoute.h"
#include "driveModi/manualControl.h"
#include "driveModi/consolControl.h"
#include "driveModi/Modi/ManualControl/manualControl.h"
#include "driveModi/driveManager.h"
#include "moveControl.h"
#include "route.h"
#include "debugMqtt.h"
#include "debugTimes.h"
#include "network.h"
void callbackControllerAction();
void callbackControllerConnect();
void controllerPrintBattery();
bool reconnectMqtt();
MotorControl left_motor;
MotorControl right_motor;
Speedometer speedometer_left;
Speedometer speedometer_right;
MoveControl moveController;
Route route;
DebugMqtt debugger("main");
ManualControl manualControl;
CaptureRoute captureRoute;
Autopilot autopilot;
ConsolControl consolControl;
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 lastReconnectAttempt = 0;
int controller_battery = -1;
enum DriveMode {autopilot_e, captureRoute_e, manualControl_e, consolControl_e};
DriveMode driveMode = manualControl_e;
DriveManager driveManager(&moveController);
void setup() {
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(WLAN_SSID, WLAN_PASSWORD);
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);
// mqtt_client.setServer(MQTT_SERVER, MQTT_PORT);
mqtt_client.setSocketTimeout(1);
reconnectMqtt();
//FIXME: make it from config.h
configTime(3600, 3600, "2.de.pool.ntp.org");
DebugMqtt::init(&mqtt_client, Loglevel::debug);
setIps();
connectWifi();
setupMQTT();
Ps3.attach(callbackControllerAction);
Ps3.attachOnConnect(callbackControllerConnect);
// Ps3.attachOnDisconnect(callbackControllerDisconnect);
Serial.println("\nReady to connect a PS3 Controller... \n");
Ps3.begin();
// Init hardware
left_motor.init(M_PWM_1, PWM_CHANNEL_M1, M_DIR_11, M_DIR_12);
right_motor.init(M_PWM_2, PWM_CHANNEL_M2, M_DIR_21, M_DIR_22);
speedometer_left.init(M_ENCODE_1A, M_ENCODE_1B, WHEEL_DIAMETER, ENC_STEPS);
speedometer_right.init(M_ENCODE_2A, M_ENCODE_2B, WHEEL_DIAMETER, ENC_STEPS);
moveController.init(&left_motor, &right_motor, &speedometer_left, &speedometer_right);
// Init driveModi
manualControl.init(&moveController, &left_motor, &right_motor);
captureRoute.init(&route, &moveController, &left_motor, &right_motor);
autopilot.init(&route, &moveController);
consolControl.init(&moveController);
}
void loop() {
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
mqtt_client.loop();
}
left_motor.runMotorControl();
right_motor.runMotorControl();
speedometer_left.runSpeedometer();
speedometer_right.runSpeedometer();
// TODO: Auskommentiert weil macht vielleicht komische Sachen
// route.runRoute();
switch (driveMode) {
case manualControl_e:
manualControl.runManualControl();
break;
case captureRoute_e:
captureRoute.runCaptureRoute();
break;
case autopilot_e:
autopilot.runAutopilot();
break;
case consolControl_e:
consolControl.run();
break;
default:
break;
}
}
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();
checkMQTT();
driveManager.loop();
}
void callbackControllerAction() {
if (Ps3.event.button_down.ps) {
switch (driveMode) {
case manualControl_e:
driveMode = DriveMode::captureRoute_e;
debugger.sendMsg(Loglevel::info, "New driveMode = CaptureRoute");
break;
case captureRoute_e:
driveMode = DriveMode::autopilot_e;
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;
debugger.sendMsg(Loglevel::info, "New driveMode = ManualControl");
break;
default:
break;
}
}
}
void callbackControllerConnect() {
Serial.println("Controller connected to ESP32");
debugger.sendMsg(Loglevel::info, "Controller connected to ESP32");
}
void controllerPrintBattery() {
static uint8_t controller_battery = -1;
if( controller_battery != Ps3.data.status.battery ){
controller_battery = Ps3.data.status.battery;
}
src/moveControl.cpp
+32 -42
View File
@@ -1,9 +1,11 @@
#include "moveControl.h"
#include <Arduino.h>
MoveControl::MoveControl() {
this->debug = new DebugMqtt("MoveControl");
this->left_motor = new MotorControl;
this->right_motor = new MotorControl;
this->left_speedometer = new Speedometer;
this->right_speedometer = new Speedometer;
this->left_pid = new PID( &this->wheelspeed_left,
&this->left_pid_out,
&this->wheelspeed_left_target,
@@ -22,48 +24,41 @@ MoveControl::MoveControl() {
this->right_pid->SetOutputLimits(PID_OUT_MIN, PID_OUT_MAX);
this->right_pid->SetSampleTime(PID_SAMPLETIME);
this->right_pid->SetMode(AUTOMATIC);
this->left_motor->init(M_PWM_1, PWM_CHANNEL_M1, M_DIR_11, M_DIR_12);
this->right_motor->init(M_PWM_2, PWM_CHANNEL_M2, M_DIR_21, M_DIR_22);
this->left_speedometer->init(M_ENCODE_1A, M_ENCODE_1B, WHEEL_DIAMETER, ENC_STEPS);
this->right_speedometer->init(M_ENCODE_2A, M_ENCODE_2B, WHEEL_DIAMETER, ENC_STEPS);
}
void MoveControl::init(MotorControl *left_motor, MotorControl *right_motor,
Speedometer *left_speedometer, Speedometer *right_speedometer) {
debug->sendMsg(Loglevel::info, "Init...");
this->left_motor = left_motor;
this->right_motor = right_motor;
this->left_speedometer = left_speedometer;
this->right_speedometer = right_speedometer;
debug->sendMsg(Loglevel::info, "Init finished!");
MoveControl::~MoveControl() {
this->left_motor->emergencyStop();
this->right_motor->emergencyStop();
}
void MoveControl::loop() {
this->left_motor->loop();
this->right_motor->loop();
this->left_speedometer->loop();
this->right_speedometer->loop();
static uint64_t last_millis = 0;
if (millis() - last_millis < delay)
return;
this->runMoveControl();
last_millis = millis();
}
void MoveControl::runMoveControl() {
static uint64_t last_millis = 0;
if (millis() - last_millis < RUN_MOVE_CONTROL_DELAY) {
return;
}
last_millis = millis();
this->updateCurrentWheelSpeed();
this->calcTargetWheelSpeed();
this->updateWheelSpeed();
this->calcWheelSpeed();
DebugTimes pidTimes;
this->right_pid->Compute();
this->left_pid->Compute();
pidTimes.stopConsol("PID Calulate", 100);
DebugTimes regMotorTime;
this->regulateMotors();
regMotorTime.stopConsol("regulateMotors", 100);
static uint32_t functioncalls = 0;
functioncalls++;
if (functioncalls % 50 == 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);
DebugTimes debugMsgTime;
debug->sendMsg(Loglevel::debug, str);
debugMsgTime.stopConsol("debug Msg", 100);
}
}
void MoveControl::setDrivingStatus(DrivingStatus status) {
@@ -75,7 +70,6 @@ void MoveControl::setSpeed(double speed) {
this->x_speed = 0;
else
this->x_speed = speed;
// Serial.printf("New speed: %f in moveControll.cpp \n", speed);
}
void MoveControl::setRotationspeed(double speed) {
@@ -83,7 +77,6 @@ void MoveControl::setRotationspeed(double speed) {
this->rotation_speed = 0;
else
this->rotation_speed = speed;
}
void MoveControl::setPidTunings(uint8_t side, double p, double i, double d) {
@@ -94,10 +87,9 @@ void MoveControl::setPidTunings(uint8_t side, double p, double i, double d) {
selectedPID = this->right_pid;
selectedPID->SetTunings(p, i, d);
}
void MoveControl::calcWheelSpeed() {
void MoveControl::calcTargetWheelSpeed() {
/* original formula:
(1 / r) / 1 b \ / x \ = / Xl \
\ 1 -b / \ T / \ Xr / */
@@ -109,8 +101,6 @@ 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);
}
void MoveControl::regulateMotors() {
@@ -132,7 +122,7 @@ void MoveControl::regulateMotors() {
}
}
void MoveControl::updateWheelSpeed() {
void MoveControl::updateCurrentWheelSpeed() {
this->wheelspeed_left = this->left_speedometer->getSpeed();
this->wheelspeed_right = this->right_speedometer->getSpeed();
}
src/network.cpp
+76
View File
@@ -0,0 +1,76 @@
/**
* @file network.cpp
* @author Alexander Klein (alex@kleiax.de)
* @brief Contains function implementation of network.h
* @version 0.1
* @date 2021-12-14
*
* @copyright Copyright (c) 2021
*
*/
#include "network.h"
void setIps() {
local_IP.fromString(WLAN_IP);
gateway.fromString(WLAN_GATEWAY);
subnet.fromString(WLAN_SUBNETMASK);
mqtt_server.fromString(MQTT_SERVER);
}
void setupMQTT() {
mqtt_client.setServer(mqtt_server, MQTT_PORT);
// mqtt_client.setServer(MQTT_SERVER, MQTT_PORT);
mqtt_client.setSocketTimeout(1);
connectMQTT();
}
bool connectMQTT() {
// 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 connectWifi() {
// 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(WLAN_SSID, WLAN_PASSWORD);
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());
}
void checkMQTT() {
static uint64_t lastReconnectAttempt = 0;
if (!mqtt_client.connected()) {
long now = millis();
if (now - lastReconnectAttempt > MQTT_TIME_RECONNECT) {
lastReconnectAttempt = now;
// Attempt to reconnect
if (connectMQTT())
lastReconnectAttempt = 0;
}
} else
mqtt_client.loop();
}
src/route.cpp
+3 -2
View File
@@ -1,4 +1,5 @@
#include "route.h"
#include "config.h"
Route::Route() {
Serial1.begin(GPS_BAUD, SERIAL_8N1, GPS_RX, GPS_TX);
@@ -7,7 +8,7 @@ Route::Route() {
void Route::runRoute() {
static uint64_t last_millis = 0;
if (millis() - last_millis < RUN_ROUTE_DELAY) {
if (millis() - last_millis < 123456789) {
return;
}
last_millis = millis();
@@ -73,7 +74,7 @@ double Route::getDis(Point point_1, Point point_2) {
}
bool Route::nearlySameLocation(Point p1, Point p2) {
if (this->getDis(p1, p2) < DISTANCE_BETWEEN_POINTS)
if (this->getDis(p1, p2) < 0.5)
return true;
return false;
}