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

restructerd projekt

Browse Source
This commit is contained in:
kleiax
2021-12-12 16:53:19 +01:00
parent 54e84e4b5d
commit a1d59c18ad
26 changed files with 492 additions and 487 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/config.h
-136
View File
@@ -1,136 +0,0 @@
// 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
#define GPS_RX 16
#define GPS_TX 17
//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
src/debugMqtt.cpp
-102
View File
@@ -1,102 +0,0 @@
#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];
unsigned long long int DebugMqtt::real_millis = 0;
unsigned long int DebugMqtt::last_millis = 0;
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::writeToInflux(String measurement_name, String field_set, float measurement) {
// Example String: "weather temperature=82 1465839830100400200";
unsigned long long int nanos = DebugMqtt::getUpdatedRealMillis() * 1000000;
snprintf(DebugMqtt::msg, MQTT_BUFFER_SITE, "%s %s=%f %llu", measurement_name.c_str(), field_set.c_str(), measurement, nanos);
this->sendData(Loglevel::influx, DebugMqtt::msg);
}
void DebugMqtt::sendTime() {
struct tm timeinfo;
if(!getLocalTime(&timeinfo)){
snprintf(DebugMqtt::msg, MQTT_BUFFER_SITE, "Failed to obtain time");
}
snprintf(DebugMqtt::msg, MQTT_BUFFER_SITE, "%d %d %d %d %d %d %llu", timeinfo.tm_year, timeinfo.tm_mon, timeinfo.tm_mday, timeinfo.tm_hour, timeinfo.tm_min, timeinfo.tm_sec, DebugMqtt::getUpdatedRealMillis());
this->sendMsg(Loglevel::info, DebugMqtt::msg);
}
void DebugMqtt::init(PubSubClient *client, Loglevel loglevel) {
DebugMqtt::client = client;
DebugMqtt::loglevel = loglevel;
DebugMqtt::isInit = true;
}
void DebugMqtt::changeLoglevel(Loglevel loglevel) {
DebugMqtt::loglevel = loglevel;
}
void DebugMqtt::initRealMillis() {
time_t now;
struct tm timeinfo;
if(!getLocalTime(&timeinfo)){
snprintf(DebugMqtt::msg, MQTT_BUFFER_SITE, "Failed to obtain time in initRealMillis()");
DebugMqtt::client->publish(DebugMqtt::enum_to_string(Loglevel::error).c_str(), DebugMqtt::msg);
return;
}
time(&now);
DebugMqtt::last_millis = millis();
DebugMqtt::real_millis = now;
DebugMqtt::real_millis = DebugMqtt::real_millis * 1000 + DebugMqtt::last_millis;
}
unsigned long long int DebugMqtt::getUpdatedRealMillis() {
DebugMqtt::real_millis = DebugMqtt::real_millis + (millis() - DebugMqtt::last_millis);
DebugMqtt::last_millis = millis();
return DebugMqtt::real_millis;
}
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";
case Loglevel::influx :
return "Rover/Influx";
default:
return "INVALID ENUM";
}
}
src/debugMqtt.h
-47
View File
@@ -1,47 +0,0 @@
#ifndef DEBUG_MQTT_H
#define DEBUG_MQTT_H
#include <PubSubClient.h>
#include "config.h"
enum Loglevel { none,
error,
warn,
info,
debug,
influx};
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);
void writeToInflux(String measurement_name, String field_set, float measurement);
void sendTime();
static void init(PubSubClient *client, Loglevel loglevel);
static void changeLoglevel(Loglevel loglevel);
static void initRealMillis();
static unsigned long long int getUpdatedRealMillis();
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];
static unsigned long long int real_millis;
static unsigned long int last_millis;
};
#endif // DEBUG_MQTT_H
src/debugTimes.cpp
-11
View File
@@ -1,11 +0,0 @@
#include "debugTimes.h"
DebugTimes::DebugTimes() {
this->startTime = millis();
}
void DebugTimes::stop(const char* name, uint16_t minTime) {
uint64_t time = millis() - this->startTime;
if (time > minTime)
Serial.printf("%s needs %llu ms\n", name, time);
}
src/debugTimes.h
-18
View File
@@ -1,18 +0,0 @@
#ifndef DEBUG_TIMES_H
#define DEBUG_TIMES_H
#include <list>
#include <cstdint>
#include <Arduino.h>
class DebugTimes {
public:
DebugTimes();
void stop(const char* name, uint16_t minTime = 0);
private:
uint64_t startTime;
};
#endif //DEBUG_TIMES_H
src/hardware/motorControl.cpp
-183
View File
@@ -1,183 +0,0 @@
#include "motorControl.h"
#include <Arduino.h>
#include "config.h"
MotorControl::MotorControl() {
this->debug = new DebugMqtt("MotorControl");
}
void MotorControl::init(uint8_t pwm_pin, uint8_t pwm_channel, uint8_t direction_pin_1, uint8_t direction_pin_2, String name) {
debug->sendMsg(Loglevel::info, "Init...");
this->name = name;
this->pwm_pin = pwm_pin;
this->pwm_channel = pwm_channel;
this->direction_pin_1 = direction_pin_1;
this->direction_pin_2 = direction_pin_2;
pinMode(this->direction_pin_1, OUTPUT);
pinMode(this->direction_pin_2, OUTPUT);
digitalWrite(this->direction_pin_1, LOW);
digitalWrite(this->direction_pin_2, LOW);
ledcSetup(this->pwm_channel, PWM_FREQ, PWM_RES);
ledcAttachPin(this->pwm_pin, this->pwm_channel);
ledcWrite(this->pwm_channel, 0);
debug->sendMsg(Loglevel::info, "Init finished!");
}
void MotorControl::runMotorControl() {
if (millis() - this->last_millis > RUN_MOTOR_CONTROL_DELAY) {
// Absolute difference between target_power and speed
uint8_t abs_difference = abs(this->target_power - this->power);
// Difference between target_power and speed
int16_t difference = this->target_power - this->power;
// Check that the target speed is close to 0 and that the abs_difference is lower than POWER_STEPS
if (abs(this->target_power) < POWER_STEPS && abs_difference < POWER_STEPS) {
this->setRealPower(0);
this->last_millis = millis();
return;
}
// Correct speed
if (abs_difference < POWER_STEPS) {
this->last_millis = millis();
return;
}
// Positive or negative tagret speed
if (this->target_power >= 0) {
// Positive or negative speed
if (this->power >= 0) {
if (difference > 0) {
this->increasePower(POWER_STEPS);
} else {
this->increasePower(-POWER_STEPS);
}
} else {
this->increasePower(POWER_STEPS);
}
} else {
// Positive or negative speed
if (this->power >= 0) {
this->increasePower(-POWER_STEPS);
} else {
if (difference > 0) {
this->increasePower(POWER_STEPS);
} else {
this->increasePower(-POWER_STEPS);
}
}
}
this->last_millis = millis();
}
}
void MotorControl::setTargetPower(int8_t power) {
if (power <= 100 && power >= -100) {
this->target_power = power;
char str[32];
sprintf(str, "target_power_%s", this->name.c_str());
debug->writeToInflux("motorControl", str, power);
} else {
char str[64];
sprintf(str, "Invalid Argument (power: %d) in setTargetPower!", power);
debug->sendMsg(Loglevel::error, str);
}
}
void MotorControl::stop() {
this->target_power = 0;
char str[32];
sprintf(str, "target_power_%s", this->name.c_str());
debug->writeToInflux("motorControl", str, this->target_power);
}
void MotorControl::emergencyStop() {
setRealPower(0);
}
void MotorControl::toString() {
Serial.printf("Target power: %d, power: %d, direction: %d \n", this->target_power, this->power, this->direction);
}
int8_t MotorControl::getPower() {
return this->power;
}
int8_t MotorControl::getTargetPower() {
return this->target_power;
}
bool MotorControl::isTargetPowerReached() {
if (this->target_power == this->power)
return true;
return false;
}
bool MotorControl::isAccelerationPositive() {
if (power < target_power)
return true;
return false;
}
bool MotorControl::isAccelerationNegative() {
if (power > target_power)
return true;
return false;
}
void MotorControl::setRealPower(int8_t power) {
//TODO: Exceptionhandling
if (power <= 100 || power <= -100) {
this->power = power;
} else {
debug->sendMsg(Loglevel::error, "Invalid Argument in setRealPower!");
return;
}
if (power == 0) {
this->direction = 0;
digitalWrite(this->direction_pin_1, LOW);
digitalWrite(this->direction_pin_2, LOW);
ledcWrite(this->pwm_channel, 0);
return;
}
uint8_t pwm_val = map(abs(power), 0, 100, PWM_MIN, PWM_MAX);
if ((this->direction == 1 || this->direction == 0) && power < 0){ // new direction backward
this->direction = 2;
digitalWrite(this->direction_pin_1, LOW);
digitalWrite(this->direction_pin_2, HIGH);
} else if ((this->direction == 2 || this->direction == 0) && power > 0){ // new direction forward
this->direction = 1;
digitalWrite(this->direction_pin_1, HIGH);
digitalWrite(this->direction_pin_2, LOW);
}
ledcWrite(this->pwm_channel, pwm_val);
char str[32];
sprintf(str, "real_power_%s", this->name.c_str());
debug->writeToInflux("motorControl", str, power);
// Serial.printf("pwm_val: %d, ", pwm_val);
}
void MotorControl::increasePower(int8_t power) {
//TODO: Exceptionhandling
//TODO: make a stop befor a direction change
if (abs(power) > 2 * POWER_STEPS) {
Serial.println("Invalid Argument in MotorControl::increasePower");
return;
}
this->setRealPower(this->power + power);
}
src/hardware/motorControl.h
-45
View File
@@ -1,45 +0,0 @@
#ifndef MOTOR_CONTROL_H
#define MOTOR_CONTROL_H
#include <cstdint>
#include <string>
#include "debugMqtt.h"
class MotorControl {
public:
MotorControl();
void init(uint8_t pwm_pin, uint8_t pwm_channel, uint8_t direction_1, uint8_t direction_2, String name);
void runMotorControl();
void setTargetPower(int8_t power);
void stop();
void emergencyStop();
void toString();
int8_t getPower();
int8_t getTargetPower();
bool isTargetPowerReached();
bool isAccelerationPositive();
bool isAccelerationNegative();
private:
void setRealPower(int8_t power);
void increasePower(int8_t power);
String name;
DebugMqtt *debug;
int8_t target_power = 0;
int8_t power = 0;
uint8_t direction = 0; // 0 = stop, 1 = forward, 2 = backward
uint64_t last_millis = 0;
uint8_t pwm_pin;
uint8_t pwm_channel;
uint8_t direction_pin_1;
uint8_t direction_pin_2;
};
#endif // MOTOR_CONTROL_H
src/hardware/speedometer.cpp
-92
View File
@@ -1,92 +0,0 @@
#include "speedometer.h"
Speedometer::Speedometer() {
for (int i = 0; i < BUF_SIZE; i++) {
this->buf[i] = 0;
}
this->debug = new DebugMqtt("Speedometer");
}
void Speedometer::init(uint8_t pinA, uint8_t pinB, String name) {
debug->sendMsg(Loglevel::info, "Init...");
this->name = name;
ESP32Encoder::useInternalWeakPullResistors=DOWN;
this->encoder.attachFullQuad(pinA, pinB);
this->encoder.setFilter(ENC_FILTER);
debug->sendMsg(Loglevel::info, "Init finished!");
}
void Speedometer::runSpeedometer() {
uint32_t time = millis();
//Cancel if delay is not reached
if (time - this->last_millis < RUN_SPEEDOMETER_DELAY) {
return;
}
uint16_t elapsed_time = time - this->last_millis;
this->last_millis = time;
int16_t count = encoder.getCount();
this->addValToBuf(count);
encoder.clearCount();
uint8_t direction = STOP;
if (count > 0) {
direction = FORWARD;
} else if (count < 0) {
direction = BACKWARD;
}
count = abs(this->getAverage()); // Absolute time in milliseconds
double n = (double)count / ENC_STEPS; // Wheel revolutions in absolute time
double u = (double)n / ((double)elapsed_time / 1000); // Wheel revolutions per second
double ms = u * (WHEEL_DIAMETER * PI); // Speed in m/s
if (direction == FORWARD) {
this->speed = ms;
} else if (direction == BACKWARD) {
this->speed = ms * (-1);
} else {
this->speed = 0;
}
char str[32];
sprintf(str, "speed_%s", this->name.c_str());
debug->writeToInflux("speedometer", str, this->speed);
// Serial.printf("time: %d, ", time);
// Serial.printf("count: %d, ",count);
// Serial.printf("n: %f, u: %f, ms: %f \n", n, u, ms);
}
double Speedometer::getSpeed() {
return this->speed;
}
uint8_t Speedometer::getDirection() {
if (this->speed > 0) {
return FORWARD;
} else if (this->speed < 0) {
return BACKWARD;
} else {
return STOP;
}
}
void Speedometer::addValToBuf(int16_t val) {
this->buf[this->bufPos] = val;
this->bufPos++;
if (this->bufPos == BUF_SIZE) {
this->bufPos = 0;
}
}
int16_t Speedometer::getAverage() {
int16_t sum = 0;
for (int i = 0; i < BUF_SIZE; i++) {
sum += this->buf[i];
}
return sum / BUF_SIZE;
}
src/hardware/speedometer.h
-35
View File
@@ -1,35 +0,0 @@
#ifndef SPEEDOMETER_H
#define SPEEDOMETER_H
#include <cstdint>
#include <ESP32Encoder.h>
#include <math.h>
#include "config.h"
#include "debugMqtt.h"
class Speedometer {
public:
Speedometer();
void init(uint8_t pinA, uint8_t pinB, String name);
void runSpeedometer();
double getSpeed();
uint8_t getDirection();
private:
void addValToBuf(int16_t val);
int16_t getAverage();
ESP32Encoder encoder;
DebugMqtt *debug;
String name;
double speed = 0;
uint32_t last_millis = 0;
uint8_t bufPos = 0;
int16_t buf[BUF_SIZE];
};
#endif // SPEEDOMETER_H
src/hardware/underfloorLighting.cpp
View File
src/hardware/underfloorLighting.h
View File
src/moveControl.h
-57
View File
@@ -1,57 +0,0 @@
#ifndef MOVE_CONTROL_H
#define MOVE_CONTROL_H
#include <cstdint>
#include <PID_v1.h>
#include "hardware/motorControl.h"
#include "hardware/speedometer.h"
#include "config.h"
#include "debugMqtt.h"
#include "debugTimes.h"
enum DrivingStatus {stop,
drive};
class MoveControl {
public:
MoveControl();
void init(MotorControl *left_motor, MotorControl *right_motor,
Speedometer *left_encoder, Speedometer *right_encoder);
void runMoveControl();
void setDrivingStatus(DrivingStatus status);
void setSpeed(double speed);
void setRotationspeed(double speed);
void setPidTunings(uint8_t side, double p, double i, double d);
private:
void calcWheelSpeed();
void regulateMotors();
void updateWheelSpeed();
DebugMqtt *debug;
MotorControl *left_motor;
MotorControl *right_motor;
Speedometer *left_speedometer;
Speedometer *right_speedometer;
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
src/route.h
-58
View File
@@ -1,58 +0,0 @@
#ifndef ROUTE_H
#define ROUTE_H
#include <cstdint>
#include <list>
#include <TinyGPS++.h>
#include <Arduino.h>
#include "hardware/motorControl.h"
#include "hardware/speedometer.h"
#include "debugMqtt.h"
#include "config.h"
struct Point{
double lat = 0;
double lon = 0;
bool operator==(const Point& rhs) const {
return this->lat == rhs.lat && this->lon == rhs.lon;
}
};
class Route {
public:
Route();
void runRoute();
void addPointToRoute(Point point);
void addCurrentLocationToRoute();
void delRoute();
void resetRoute();
Point startRoute();
Point getNextPoint();
uint16_t getNumberOfPoints();
static double getDis(Point point_1, Point point_2);
private:
bool nearlySameLocation(Point p1, Point p2);
std::list<Point> points;
std::list<Point>::iterator it;
Point currentLocation;
uint16_t count_points = 0;
bool route_started = false;
bool route_finished = false;
TinyGPSPlus gps;
DebugMqtt *debug;
};
#endif // ROUTE_H