From cd29191bb4d0438c54fe7b9f0e62c7c706b7589a Mon Sep 17 00:00:00 2001 From: Alexander Klein Date: Tue, 14 Dec 2021 19:22:06 +0100 Subject: [PATCH] a lot of bullshit --- doc/Notes and TODOs/TODO allgemein.txt | 5 +- include/config.h | 130 +------------ include/moveControl.h | 134 +++++++++++-- include/moveControlConfig.h | 44 +++++ include/network.h | 36 ++++ include/networkConfig.h | 64 +++++++ lib/MotorControl/motorControl.h | 3 + src/driveModi/CaptureRoute/captureRoute.h | 22 --- src/driveModi/ManualControl/manualControl.cpp | 125 ------------ src/driveModi/ManualControl/manualControl.h | 37 ---- .../{ => Modi}/Autopilot/autopilot.cpp | 2 +- .../{ => Modi}/Autopilot/autopilot.h | 4 +- .../{ => Modi}/CaptureRoute/captureRoute.cpp | 8 +- .../Modi/CaptureRoute/captureRoute.h | 21 ++ .../ConsolControl/consolControl.cpp | 0 .../{ => Modi}/ConsolControl/consolControl.h | 3 +- .../Modi/ManualControl/manualControl.cpp | 42 ++++ .../Modi/ManualControl/manualControl.h | 91 +++++++++ src/driveModi/Modi/TestMode/testMode.cpp | 0 src/driveModi/Modi/TestMode/testMode.h | 0 src/driveModi/driveManager.cpp | 72 +++++++ src/driveModi/driveManager.h | 51 +++++ src/driveModi/driveModi.h | 22 +++ src/main.cpp | 181 ++---------------- src/moveControl.cpp | 76 ++++---- src/network.cpp | 76 ++++++++ src/route.cpp | 5 +- 27 files changed, 703 insertions(+), 551 deletions(-) create mode 100644 include/moveControlConfig.h create mode 100644 include/network.h create mode 100644 include/networkConfig.h delete mode 100644 src/driveModi/CaptureRoute/captureRoute.h delete mode 100644 src/driveModi/ManualControl/manualControl.cpp delete mode 100644 src/driveModi/ManualControl/manualControl.h rename src/driveModi/{ => Modi}/Autopilot/autopilot.cpp (83%) rename src/driveModi/{ => Modi}/Autopilot/autopilot.h (81%) rename src/driveModi/{ => Modi}/CaptureRoute/captureRoute.cpp (52%) create mode 100644 src/driveModi/Modi/CaptureRoute/captureRoute.h rename src/driveModi/{ => Modi}/ConsolControl/consolControl.cpp (100%) rename src/driveModi/{ => Modi}/ConsolControl/consolControl.h (84%) create mode 100644 src/driveModi/Modi/ManualControl/manualControl.cpp create mode 100644 src/driveModi/Modi/ManualControl/manualControl.h create mode 100644 src/driveModi/Modi/TestMode/testMode.cpp create mode 100644 src/driveModi/Modi/TestMode/testMode.h create mode 100644 src/driveModi/driveManager.cpp create mode 100644 src/driveModi/driveManager.h create mode 100644 src/driveModi/driveModi.h create mode 100644 src/network.cpp diff --git a/doc/Notes and TODOs/TODO allgemein.txt b/doc/Notes and TODOs/TODO allgemein.txt index 71ec4bb..49256b0 100644 --- a/doc/Notes and TODOs/TODO allgemein.txt +++ b/doc/Notes and TODOs/TODO allgemein.txt @@ -3,4 +3,7 @@ Meine Libs von andren Abhängigkeiten lösen - Alle config.h und nicht allgemeinen Header entfernen Speedometer: - - Mitrechnen durchschnittliche aufrufzeit \ No newline at end of file + - Mitrechnen durchschnittliche aufrufzeit + +Alle loops Nachgucken ob last_millis aktualisiert wird. +Mqtt kram auslagern \ No newline at end of file diff --git a/include/config.h b/include/config.h index 1bfd71e..425b177 100644 --- a/include/config.h +++ b/include/config.h @@ -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 \ No newline at end of file diff --git a/include/moveControl.h b/include/moveControl.h index 14333cc..729497f 100644 --- a/include/moveControl.h +++ b/include/moveControl.h @@ -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 #include +#include #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 \ No newline at end of file diff --git a/include/moveControlConfig.h b/include/moveControlConfig.h new file mode 100644 index 0000000..322525c --- /dev/null +++ b/include/moveControlConfig.h @@ -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 \ No newline at end of file diff --git a/include/network.h b/include/network.h new file mode 100644 index 0000000..5d79d66 --- /dev/null +++ b/include/network.h @@ -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 +#include + +#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 \ No newline at end of file diff --git a/include/networkConfig.h b/include/networkConfig.h new file mode 100644 index 0000000..ebefa8e --- /dev/null +++ b/include/networkConfig.h @@ -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 \ No newline at end of file diff --git a/lib/MotorControl/motorControl.h b/lib/MotorControl/motorControl.h index e64766e..d65f09e 100644 --- a/lib/MotorControl/motorControl.h +++ b/lib/MotorControl/motorControl.h @@ -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); diff --git a/src/driveModi/CaptureRoute/captureRoute.h b/src/driveModi/CaptureRoute/captureRoute.h deleted file mode 100644 index c22aabb..0000000 --- a/src/driveModi/CaptureRoute/captureRoute.h +++ /dev/null @@ -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 \ No newline at end of file diff --git a/src/driveModi/ManualControl/manualControl.cpp b/src/driveModi/ManualControl/manualControl.cpp deleted file mode 100644 index ebc2f1e..0000000 --- a/src/driveModi/ManualControl/manualControl.cpp +++ /dev/null @@ -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); -} \ No newline at end of file diff --git a/src/driveModi/ManualControl/manualControl.h b/src/driveModi/ManualControl/manualControl.h deleted file mode 100644 index 9cdc4ea..0000000 --- a/src/driveModi/ManualControl/manualControl.h +++ /dev/null @@ -1,37 +0,0 @@ -#ifndef MANUAL_CONTROL_H -#define MANUAL_CONTROL_H - -#include - -#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 \ No newline at end of file diff --git a/src/driveModi/Autopilot/autopilot.cpp b/src/driveModi/Modi/Autopilot/autopilot.cpp similarity index 83% rename from src/driveModi/Autopilot/autopilot.cpp rename to src/driveModi/Modi/Autopilot/autopilot.cpp index ae867e0..4f3da91 100644 --- a/src/driveModi/Autopilot/autopilot.cpp +++ b/src/driveModi/Modi/Autopilot/autopilot.cpp @@ -1,4 +1,4 @@ -#include "driveModi/autopilot.h" +#include "driveModi/Modi/Autopilot/autopilot.h" Autopilot::Autopilot() { this->debug = new DebugMqtt("Autopilot"); diff --git a/src/driveModi/Autopilot/autopilot.h b/src/driveModi/Modi/Autopilot/autopilot.h similarity index 81% rename from src/driveModi/Autopilot/autopilot.h rename to src/driveModi/Modi/Autopilot/autopilot.h index 9d16a2c..14bde29 100644 --- a/src/driveModi/Autopilot/autopilot.h +++ b/src/driveModi/Modi/Autopilot/autopilot.h @@ -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(); diff --git a/src/driveModi/CaptureRoute/captureRoute.cpp b/src/driveModi/Modi/CaptureRoute/captureRoute.cpp similarity index 52% rename from src/driveModi/CaptureRoute/captureRoute.cpp rename to src/driveModi/Modi/CaptureRoute/captureRoute.cpp index fb6bfbd..d161a93 100644 --- a/src/driveModi/CaptureRoute/captureRoute.cpp +++ b/src/driveModi/Modi/CaptureRoute/captureRoute.cpp @@ -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() { + } diff --git a/src/driveModi/Modi/CaptureRoute/captureRoute.h b/src/driveModi/Modi/CaptureRoute/captureRoute.h new file mode 100644 index 0000000..c94d670 --- /dev/null +++ b/src/driveModi/Modi/CaptureRoute/captureRoute.h @@ -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 \ No newline at end of file diff --git a/src/driveModi/ConsolControl/consolControl.cpp b/src/driveModi/Modi/ConsolControl/consolControl.cpp similarity index 100% rename from src/driveModi/ConsolControl/consolControl.cpp rename to src/driveModi/Modi/ConsolControl/consolControl.cpp diff --git a/src/driveModi/ConsolControl/consolControl.h b/src/driveModi/Modi/ConsolControl/consolControl.h similarity index 84% rename from src/driveModi/ConsolControl/consolControl.h rename to src/driveModi/Modi/ConsolControl/consolControl.h index 2f673a9..4bd0e12 100644 --- a/src/driveModi/ConsolControl/consolControl.h +++ b/src/driveModi/Modi/ConsolControl/consolControl.h @@ -4,9 +4,10 @@ #include #include "moveControl.h" +#include "driveModi/driveModi.h" #include "debugMqtt.h" -class ConsolControl { +class ConsolControl : DriveModi{ public: ConsolControl(); void init(MoveControl *moveControl); diff --git a/src/driveModi/Modi/ManualControl/manualControl.cpp b/src/driveModi/Modi/ManualControl/manualControl.cpp new file mode 100644 index 0000000..86bfa81 --- /dev/null +++ b/src/driveModi/Modi/ManualControl/manualControl.cpp @@ -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); +} \ No newline at end of file diff --git a/src/driveModi/Modi/ManualControl/manualControl.h b/src/driveModi/Modi/ManualControl/manualControl.h new file mode 100644 index 0000000..e23df23 --- /dev/null +++ b/src/driveModi/Modi/ManualControl/manualControl.h @@ -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 + +#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 \ No newline at end of file diff --git a/src/driveModi/Modi/TestMode/testMode.cpp b/src/driveModi/Modi/TestMode/testMode.cpp new file mode 100644 index 0000000..e69de29 diff --git a/src/driveModi/Modi/TestMode/testMode.h b/src/driveModi/Modi/TestMode/testMode.h new file mode 100644 index 0000000..e69de29 diff --git a/src/driveModi/driveManager.cpp b/src/driveModi/driveManager.cpp new file mode 100644 index 0000000..2c20726 --- /dev/null +++ b/src/driveModi/driveManager.cpp @@ -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; + } +} + diff --git a/src/driveModi/driveManager.h b/src/driveModi/driveManager.h new file mode 100644 index 0000000..a3c8386 --- /dev/null +++ b/src/driveModi/driveManager.h @@ -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 \ No newline at end of file diff --git a/src/driveModi/driveModi.h b/src/driveModi/driveModi.h new file mode 100644 index 0000000..74ac982 --- /dev/null +++ b/src/driveModi/driveModi.h @@ -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 \ No newline at end of file diff --git a/src/main.cpp b/src/main.cpp index df220ff..23dd089 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -1,208 +1,49 @@ #include #include -#include -#include -#include #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; } diff --git a/src/moveControl.cpp b/src/moveControl.cpp index 8ec4bc7..ed3211e 100644 --- a/src/moveControl.cpp +++ b/src/moveControl.cpp @@ -1,9 +1,11 @@ #include "moveControl.h" -#include - 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, @@ -21,49 +23,42 @@ MoveControl::MoveControl() { this->right_pid->SetOutputLimits(PID_OUT_MIN, PID_OUT_MAX); this->right_pid->SetSampleTime(PID_SAMPLETIME); - this->right_pid->SetMode(AUTOMATIC); + 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(); } \ No newline at end of file diff --git a/src/network.cpp b/src/network.cpp new file mode 100644 index 0000000..bdd4691 --- /dev/null +++ b/src/network.cpp @@ -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(); +} diff --git a/src/route.cpp b/src/route.cpp index 9cc3123..4e57246 100644 --- a/src/route.cpp +++ b/src/route.cpp @@ -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; } \ No newline at end of file