diff --git a/platformio.ini b/platformio.ini index af3ee9d..3cd232c 100644 --- a/platformio.ini +++ b/platformio.ini @@ -11,10 +11,13 @@ [env:esp32doit-devkit-v1] platform = espressif32 board = esp32doit-devkit-v1 +board_build.partitions = no_ota.csv framework = arduino monitor_speed = 115200 monitor_port = COM12 lib_deps = madhephaestus/ESP32Encoder@^0.4.0 jvpernis/PS3 Controller Host@^1.1.0 + mikalhart/TinyGPSPlus@^1.0.2 + knolleary/PubSubClient@^2.8 upload_port = COM12 diff --git a/src/autopilot.cpp b/src/autopilot.cpp new file mode 100644 index 0000000..e69de29 diff --git a/src/autopilot.h b/src/autopilot.h new file mode 100644 index 0000000..a05c0c5 --- /dev/null +++ b/src/autopilot.h @@ -0,0 +1,20 @@ +#ifndef AUTOPILOT_H +#define AUTOPILOT_H + +#include + +#include "route.h" +#include "moveControl.h" + +class Autopilot { + public: + Autopilot(); + + private: + Route *route; + MoveControl *moveControl; + TinyGPSPlus * gps; + +}; + +#endif // AUTOPILOT_H \ No newline at end of file diff --git a/src/config.h b/src/config.h index c0ba3a5..1daa5eb 100644 --- a/src/config.h +++ b/src/config.h @@ -47,4 +47,19 @@ #define ACCELERATE_STEPS 1 // PS3 Controller -// ESP32 MAC BL 24:62:AB:F2:4B:3A \ No newline at end of file +// ESP32 MAC BL 24:62:AB:F2:4B:3A + +//Max Values for speed and rotation +#define MAX_SPEED 1.0 +#define MAX_ROTATION 1.0 + +//Wlan / Mqtt config +#define WLAN_SSID "Kleiax2" +#define WLAN_PASSWORD "Punica-699" +#define WLAN_IP 0x0101a8c0 //192.168.1.1 +#define WLAN_SUBNETMASK 0x0000FFFF //255.255.0.0 +#define WLAN_GATEWAY 0x0100a8c0 //192.168.0.1 +#define MQTT_SERVER 0x1300a8c0 //192.168.0.19 +#define MQTT_PORT 1883 +#define MQTT_BUFFER_SITE 50 + diff --git a/src/debugMqtt.cpp b/src/debugMqtt.cpp new file mode 100644 index 0000000..c960098 --- /dev/null +++ b/src/debugMqtt.cpp @@ -0,0 +1,59 @@ +#include "debugMqtt.h" + +PubSubClient* DebugMqtt::client; +Loglevel DebugMqtt::loglevel; +bool DebugMqtt::isInit = false; +char DebugMqtt::msg[MQTT_BUFFER_SITE]; +char DebugMqtt::topic[MQTT_BUFFER_SITE]; + +DebugMqtt::DebugMqtt(const char* name) { + this->name = name; +} + +void DebugMqtt::sendMsg(Loglevel loglevel, String topic, String msg) { + snprintf (DebugMqtt::msg, MQTT_BUFFER_SITE, "%s: %s",this->name ,msg.c_str()); + this->sendData(loglevel, topic, DebugMqtt::msg); +} + +void DebugMqtt::sendMsg(Loglevel loglevel, String msg) { + this->sendMsg(loglevel, "", msg); +} + +void DebugMqtt::sendData(Loglevel loglevel, String topic, String data) { + if (!DebugMqtt::isInit) {return;} + + if (loglevel <= DebugMqtt::loglevel && loglevel > Loglevel::none) { + snprintf (DebugMqtt::topic, MQTT_BUFFER_SITE, "%s%s", DebugMqtt::enum_to_string(loglevel).c_str(), topic.c_str()); + snprintf (DebugMqtt::msg, MQTT_BUFFER_SITE, "%s", data.c_str()); + client->publish(DebugMqtt::topic, DebugMqtt::msg); + } +} + +void DebugMqtt::sendData(Loglevel loglevel, String data){ + this->sendData(loglevel, "", data); +} + +void DebugMqtt::init(PubSubClient *client, Loglevel loglevel) { + DebugMqtt::client = client; + DebugMqtt::loglevel = loglevel; + DebugMqtt::isInit = true; +} + +void DebugMqtt::changeLoglevel(Loglevel loglevel) { + DebugMqtt::loglevel = loglevel; +} + +String DebugMqtt::enum_to_string(Loglevel loglevel) { + switch(loglevel){ + case Loglevel::error : + return "/Rover/Error"; + case Loglevel::warn : + return "/Rover/Warn"; + case Loglevel::info : + return "/Rover/Info"; + case Loglevel::debug : + return "/Rover/Debug"; + default: + return "INVALID ENUM"; + } + } diff --git a/src/debugMqtt.h b/src/debugMqtt.h new file mode 100644 index 0000000..fe26cde --- /dev/null +++ b/src/debugMqtt.h @@ -0,0 +1,38 @@ +#ifndef DEBUG_MQTT_H +#define DEBUG_MQTT_H + +#include + +#include "config.h" + +enum Loglevel { none, + error, + warn, + info, + debug}; + +class DebugMqtt { + public: + DebugMqtt(const char* name); + void sendMsg(Loglevel loglevel, String topic, String msg); + void sendMsg(Loglevel loglevel, String msg); + void sendData(Loglevel loglevel, String topic, String data); + void sendData(Loglevel loglevel, String data); + + static void init(PubSubClient *client, Loglevel loglevel); + static void changeLoglevel(Loglevel loglevel); + + private: + const char* name; + + static String enum_to_string(Loglevel loglevel); + + static PubSubClient* client; + static Loglevel loglevel; + static bool isInit; + static char msg[MQTT_BUFFER_SITE]; + static char topic[MQTT_BUFFER_SITE]; + +}; + +#endif // DEBUG_MQTT_H \ No newline at end of file diff --git a/src/main.cpp b/src/main.cpp index db7efc8..7839cfb 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -1,21 +1,36 @@ #include #include +#include +#include #include "config.h" #include "motorControl.h" #include "moveControl.h" #include "speedometer.h" +#include "debugMqtt.h" void callbackControllerAction(); void callbackControllerConnect(); void callbackControllerDisconnect(); void controllerPrintBattery(); +void reconnectMqtt(); + MotorControl left_motor; MotorControl right_motor; Speedometer speedometer_left; Speedometer speedometer_right; MoveControl moveController; +DebugMqtt debugger("main"); + +IPAddress local_IP(WLAN_IP); +IPAddress gateway(WLAN_GATEWAY); +IPAddress subnet(WLAN_SUBNETMASK); +IPAddress mqtt_server(MQTT_SERVER); + +WiFiClient wifi_client; +PubSubClient mqtt_client(wifi_client); + uint64_t last_millis = 0; int controller_battery = -1; @@ -28,12 +43,37 @@ void driveWithControllerShoulderTrigger(uint8_t left, uint8_t right); void changeDriveMode(); void setup() { - Serial.begin(115200); // make sure your Serial Monitor is also set at this baud rate. + Serial.begin(115200); + + // Configures static IP address + if (!WiFi.config(local_IP, gateway, subnet)) { + Serial.println("STA Failed to configure"); + } + + // Connect to Wi-Fi network with SSID and password + Serial.print("Connecting to "); + Serial.println(WLAN_SSID); + WiFi.begin("Kleiax2", "Punica-699"); + while (WiFi.status() != WL_CONNECTED) { + delay(500); + Serial.print("."); + } + + // Print local IP address and start web server + Serial.println(""); + Serial.println("WiFi connected."); + Serial.println("IP address: "); + Serial.println(WiFi.localIP()); + + mqtt_client.setServer(mqtt_server, MQTT_PORT); + reconnectMqtt(); + + DebugMqtt::init(&mqtt_client, Loglevel::debug); Ps3.attach(callbackControllerAction); Ps3.attachOnConnect(callbackControllerConnect); Ps3.attachOnDisconnect(callbackControllerDisconnect); - Serial.println("\nReady to connect"); + Serial.println("\nReady to connect a PS3 Controller... \n"); Ps3.begin(); left_motor.init(M_PWM_1, PWM_CHANNEL_M1, M_DIR_11, M_DIR_12); @@ -46,11 +86,13 @@ void setup() { } void loop() { + if (!mqtt_client.connected()) { + reconnectMqtt(); + } + mqtt_client.loop(); + if (millis() - last_millis > 1000) { - Serial.printf("Speed L: %f, ", speedometer_left.getSpeed()); - left_motor.toString(); - Serial.printf("Speed R: %f, ", speedometer_right.getSpeed()); - right_motor.toString(); + debugger.sendMsg(Loglevel::info, "loop : eine Sekinde"); last_millis = millis(); } @@ -64,6 +106,28 @@ void loop() { moveController.runMoveControl(); } +void reconnectMqtt() { + // Loop until reconnection + while (!mqtt_client.connected()) { + Serial.print("Attempting MQTT connection..."); + // Create a random client ID + String clientId = "ESP32Rover-"; + clientId += String(random(0xffff), HEX); + // Attempt to connect + if (mqtt_client.connect(clientId.c_str())) { + Serial.println("connected"); + // Once connected, publish an announcement... + mqtt_client.publish("/Rover/Info", "Connected"); + } else { + Serial.print("failed, rc="); + Serial.print(mqtt_client.state()); + Serial.println(" try again in 5 seconds"); + // Wait 5 seconds before retrying + delay(5000); + } + } +} + void callbackControllerAction() { if (Ps3.event.button_down.r3) { controllerPrintBattery(); } if (Ps3.event.button_down.l1) { moveController.setSpeed(speed -= 0.1); } @@ -108,7 +172,11 @@ void controllerPrintBattery() { void driveWithControllerJoystick(int8_t x, int8_t y) { if (drive_mode != 1) return; - + double value_per_step = MAX_SPEED * 2 / 256; + moveController.setSpeed((y * -1) * value_per_step); + + value_per_step = MAX_ROTATION * 2 / 256; + moveController.setRotationspeed(x * value_per_step); } void driveWithControllerShoulderTrigger(uint8_t left, uint8_t right) { @@ -131,4 +199,11 @@ void changeDriveMode() { if (drive_mode == 2) { moveController.setSpeed(0); } -} \ No newline at end of file +} + +void getDis(double lat1, double lon1, double lat2, double lon2) { + double lat = (lat1 + lat2) / 2 * 0.01745; + double dx = 111.3 * cos(lat) * (lon1 - lon2); + double dy = 111.3 * (lat1 - lat2); + double erg = sqrt(dx * dx + dy * dy); +} diff --git a/src/moveControl.cpp b/src/moveControl.cpp index acdaae8..a45ba9d 100644 --- a/src/moveControl.cpp +++ b/src/moveControl.cpp @@ -25,7 +25,7 @@ void MoveControl::runMoveControl() { this->regulateMotors(); } -void MoveControl::setDrivingStatus(drivingStatus status) { +void MoveControl::setDrivingStatus(DrivingStatus status) { this->driving_status = status; } @@ -53,13 +53,14 @@ void MoveControl::calcWheelSpeed() { } void MoveControl::regulateMotors() { + double ratio = 0; switch (this->driving_status) { - case drivingStatus::stop : + case DrivingStatus::stop : this->left_motor->setTargetPower(0); this->right_motor->setTargetPower(0); break; - case drivingStatus::straightForward : + case DrivingStatus::straightForward : // Left motor // Too slow if (this->left_speedometer->getSpeed() < this->wheelspeed_left_target @@ -89,7 +90,7 @@ void MoveControl::regulateMotors() { } break; - case drivingStatus::straightBackward : + case DrivingStatus::straightBackward : // Left motor // Too slow if (this->left_speedometer->getSpeed() > this->wheelspeed_left_target @@ -119,8 +120,8 @@ void MoveControl::regulateMotors() { } break; - case drivingStatus::arcForwardLeft : // Identical with arcForwardRight - double ratio = this->wheelspeed_left_target / this->wheelspeed_right_target; + case DrivingStatus::arcForwardLeft : // Identical with arcForwardRight + ratio = this->wheelspeed_left_target / this->wheelspeed_right_target; // Left motor // Too slow @@ -151,8 +152,8 @@ void MoveControl::regulateMotors() { } break; - case drivingStatus::arcForwardRight : // Identical with arcForwardLeft - double ratio = this->wheelspeed_left_target / this->wheelspeed_right_target; + case DrivingStatus::arcForwardRight : // Identical with arcForwardLeft + ratio = this->wheelspeed_left_target / this->wheelspeed_right_target; // Left motor // Too slow @@ -183,8 +184,8 @@ void MoveControl::regulateMotors() { } break; - case drivingStatus::arcBackwardLeft : // Identical with arcBackwardRight - double ratio = this->wheelspeed_left_target / this->wheelspeed_right_target; + case DrivingStatus::arcBackwardLeft : // Identical with arcBackwardRight + ratio = this->wheelspeed_left_target / this->wheelspeed_right_target; // Left motor // Too slow @@ -215,8 +216,8 @@ void MoveControl::regulateMotors() { } break; - case drivingStatus::arcBackwardRight : // Identical with arcBackwardLeft - double ratio = this->wheelspeed_left_target / this->wheelspeed_right_target; + case DrivingStatus::arcBackwardRight : // Identical with arcBackwardLeft + ratio = this->wheelspeed_left_target / this->wheelspeed_right_target; // Left motor // Too slow @@ -247,7 +248,7 @@ void MoveControl::regulateMotors() { } break; - case drivingStatus::rotateLeft : + case DrivingStatus::rotateLeft : // Left motor // Too slow if (this->left_speedometer->getSpeed() > this->wheelspeed_left_target @@ -277,7 +278,7 @@ void MoveControl::regulateMotors() { } break; - case drivingStatus::rotateRight : + case DrivingStatus::rotateRight : // Left motor // Too slow if (this->left_speedometer->getSpeed() < this->wheelspeed_left_target diff --git a/src/moveControl.h b/src/moveControl.h index c9dea6e..a817e80 100644 --- a/src/moveControl.h +++ b/src/moveControl.h @@ -7,7 +7,7 @@ #include "speedometer.h" #include "config.h" -enum drivingStatus {stop, +enum DrivingStatus {stop, straightForward, straightBackward, arcForwardLeft, @@ -17,10 +17,10 @@ enum drivingStatus {stop, rotateLeft, rotateRight}; -enum regulateStatus {stop, - drive, - rotateLeft, - rotateRight}; +// enum regulateStatus {stop, +// drive, +// rotateLeft, +// rotateRight}; class MoveControl { public: @@ -28,7 +28,7 @@ class MoveControl { void init(MotorControl *left_motor, MotorControl *right_motor, Speedometer *left_encoder, Speedometer *right_encoder); void runMoveControl(); - void setDrivingStatus(drivingStatus status); + void setDrivingStatus(DrivingStatus status); void setSpeed(double speed); void setRotationspeed(double speed); @@ -41,7 +41,7 @@ class MoveControl { Speedometer *left_speedometer; Speedometer *right_speedometer; - drivingStatus driving_status = drivingStatus::stop; + DrivingStatus driving_status = DrivingStatus::stop; double x_speed = 0; double rotation_speed = 0; diff --git a/src/route.cpp b/src/route.cpp new file mode 100644 index 0000000..5c61b67 --- /dev/null +++ b/src/route.cpp @@ -0,0 +1,41 @@ +#include "route.h" + +Route::Route(){ + +} + +void Route::addPoint(Point point) { + this->points.push_back(point); + this->count_points++; +} + +void Route::delRoute() { + this->points.clear(); + this->count_points = 0; +} + +Point Route::startRoute() { + this->route_started = true; + this->route_finished = false; + + this->it = this->points.begin(); + return *it; +} + +Point Route::getNextPoint() { + if (this->route_started && !route_finished) { + it++; + return *it; + } + + if (it == this->points.end()) { + this->route_finished = true; + } + + Point fail; + return fail; +} + +uint16_t Route::getNumberOfPoints() { + return this->count_points; +} \ No newline at end of file diff --git a/src/route.h b/src/route.h new file mode 100644 index 0000000..c1a5314 --- /dev/null +++ b/src/route.h @@ -0,0 +1,40 @@ +#ifndef ROUTE_H +#define ROUTE_H + +#include +#include + +#include "motorControl.h" +#include "speedometer.h" +#include "config.h" + +struct Point{ + float N = 0; + float E = 0; +}; + +class Route { + public: + Route(); + + void addPoint(Point point); + void delRoute(); + void resetRoute(); + + Point startRoute(); + Point getNextPoint(); + uint16_t getNumberOfPoints(); + + + private: + std::list points; + std::list::iterator it; + + uint16_t count_points = 0; + + bool route_started = false; + bool route_finished = false; + +}; + +#endif // ROUTE_H \ No newline at end of file