Added debugMqtt and autoPilot

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

src/autopilot.h

@@ -0,0 +1,20 @@
+#ifndef AUTOPILOT_H
+#define AUTOPILOT_H
+
+#include <TinyGPS++.h>
+
+#include "route.h"
+#include "moveControl.h"
+
+class Autopilot {
+    public:
+        Autopilot();
+
+    private:
+        Route *route;
+        MoveControl *moveControl;
+        TinyGPSPlus * gps;
+
+};
+
+#endif // AUTOPILOT_H

src/config.h

@@ -47,4 +47,19 @@
 #define ACCELERATE_STEPS 1
 
 // PS3 Controller
-// ESP32 MAC BL 24:62:AB:F2:4B:3A
+// ESP32 MAC BL 24:62:AB:F2:4B:3A
+
+//Max Values for speed and rotation
+#define MAX_SPEED 1.0
+#define MAX_ROTATION 1.0
+
+//Wlan / Mqtt config
+#define WLAN_SSID "Kleiax2"
+#define WLAN_PASSWORD "Punica-699"
+#define WLAN_IP 0x0101a8c0          //192.168.1.1
+#define WLAN_SUBNETMASK 0x0000FFFF  //255.255.0.0
+#define WLAN_GATEWAY 0x0100a8c0     //192.168.0.1
+#define MQTT_SERVER 0x1300a8c0      //192.168.0.19
+#define MQTT_PORT 1883
+#define MQTT_BUFFER_SITE 50
+

src/debugMqtt.cpp

@@ -0,0 +1,59 @@
+#include "debugMqtt.h"
+
+PubSubClient* DebugMqtt::client;
+Loglevel DebugMqtt::loglevel;
+bool DebugMqtt::isInit = false;
+char DebugMqtt::msg[MQTT_BUFFER_SITE];
+char DebugMqtt::topic[MQTT_BUFFER_SITE];
+
+DebugMqtt::DebugMqtt(const char* name) {
+    this->name = name;
+}
+
+void DebugMqtt::sendMsg(Loglevel loglevel, String topic, String msg) {
+    snprintf (DebugMqtt::msg, MQTT_BUFFER_SITE, "%s: %s",this->name ,msg.c_str());
+    this->sendData(loglevel, topic, DebugMqtt::msg);
+}
+
+void DebugMqtt::sendMsg(Loglevel loglevel, String msg) {
+    this->sendMsg(loglevel, "", msg);
+}
+
+void DebugMqtt::sendData(Loglevel loglevel, String topic, String data) {
+    if (!DebugMqtt::isInit) {return;}
+
+    if (loglevel <= DebugMqtt::loglevel && loglevel > Loglevel::none) {
+        snprintf (DebugMqtt::topic, MQTT_BUFFER_SITE, "%s%s", DebugMqtt::enum_to_string(loglevel).c_str(), topic.c_str());
+        snprintf (DebugMqtt::msg, MQTT_BUFFER_SITE, "%s", data.c_str());
+        client->publish(DebugMqtt::topic, DebugMqtt::msg);
+    }
+}
+
+void DebugMqtt::sendData(Loglevel loglevel, String data){
+    this->sendData(loglevel, "", data);
+}
+
+void DebugMqtt::init(PubSubClient *client, Loglevel loglevel) {
+    DebugMqtt::client = client;
+    DebugMqtt::loglevel = loglevel;
+    DebugMqtt::isInit = true;
+}
+
+void DebugMqtt::changeLoglevel(Loglevel loglevel) {
+    DebugMqtt::loglevel = loglevel;
+}
+
+String DebugMqtt::enum_to_string(Loglevel loglevel) {
+      switch(loglevel){
+        case Loglevel::error :
+            return "/Rover/Error";
+        case Loglevel::warn :
+            return "/Rover/Warn";
+        case Loglevel::info :
+            return "/Rover/Info";
+        case Loglevel::debug :
+            return "/Rover/Debug";
+         default:
+            return "INVALID ENUM";
+      }
+ }

src/debugMqtt.h

@@ -0,0 +1,38 @@
+#ifndef DEBUG_MQTT_H
+#define DEBUG_MQTT_H
+
+#include <PubSubClient.h>
+
+#include "config.h"
+
+enum Loglevel { none,
+                error,
+                warn,
+                info,
+                debug};
+
+class DebugMqtt {
+    public:
+        DebugMqtt(const char* name);
+        void sendMsg(Loglevel loglevel, String topic, String msg);
+        void sendMsg(Loglevel loglevel, String msg);
+        void sendData(Loglevel loglevel, String topic, String data);
+        void sendData(Loglevel loglevel, String data);
+
+        static void init(PubSubClient *client, Loglevel loglevel);
+        static void changeLoglevel(Loglevel loglevel);
+
+    private:
+        const char* name;
+
+        static String enum_to_string(Loglevel loglevel);
+
+        static PubSubClient* client;
+        static Loglevel loglevel;
+        static bool isInit;
+        static char msg[MQTT_BUFFER_SITE];
+        static char topic[MQTT_BUFFER_SITE];
+
+};
+
+#endif // DEBUG_MQTT_H

src/main.cpp

@@ -1,21 +1,36 @@
 #include <Arduino.h>
 #include <Ps3Controller.h>
+#include <WiFi.h>
+#include <PubSubClient.h>
 
 #include "config.h"
 #include "motorControl.h"
 #include "moveControl.h"
 #include "speedometer.h"
+#include "debugMqtt.h"
 
 void callbackControllerAction();
 void callbackControllerConnect();
 void callbackControllerDisconnect();
 void controllerPrintBattery();
 
+void reconnectMqtt();
+
 MotorControl left_motor;
 MotorControl right_motor;
 Speedometer speedometer_left;
 Speedometer speedometer_right;
 MoveControl moveController;
+DebugMqtt debugger("main");
+
+IPAddress local_IP(WLAN_IP);
+IPAddress gateway(WLAN_GATEWAY);
+IPAddress subnet(WLAN_SUBNETMASK);
+IPAddress mqtt_server(MQTT_SERVER);
+
+WiFiClient wifi_client;
+PubSubClient mqtt_client(wifi_client);
+
 
 uint64_t last_millis = 0;
 int controller_battery = -1;
@@ -28,12 +43,37 @@ void driveWithControllerShoulderTrigger(uint8_t left, uint8_t right);
 void changeDriveMode();
 
 void setup() {
-  Serial.begin(115200);      // make sure your Serial Monitor is also set at this baud rate.
+  Serial.begin(115200);
+
+  // Configures static IP address
+  if (!WiFi.config(local_IP, gateway, subnet)) {
+    Serial.println("STA Failed to configure");
+  }
+
+  // Connect to Wi-Fi network with SSID and password
+  Serial.print("Connecting to ");
+  Serial.println(WLAN_SSID);
+  WiFi.begin("Kleiax2", "Punica-699");
+  while (WiFi.status() != WL_CONNECTED) {
+    delay(500);
+    Serial.print(".");
+  }
+
+  // Print local IP address and start web server
+  Serial.println("");
+  Serial.println("WiFi connected.");
+  Serial.println("IP address: ");
+  Serial.println(WiFi.localIP());
+
+  mqtt_client.setServer(mqtt_server, MQTT_PORT);
+  reconnectMqtt();
+
+  DebugMqtt::init(&mqtt_client, Loglevel::debug);
 
   Ps3.attach(callbackControllerAction);
   Ps3.attachOnConnect(callbackControllerConnect);
   Ps3.attachOnDisconnect(callbackControllerDisconnect);
-  Serial.println("\nReady to connect");
+  Serial.println("\nReady to connect a PS3 Controller... \n");
   Ps3.begin();
 
   left_motor.init(M_PWM_1, PWM_CHANNEL_M1, M_DIR_11, M_DIR_12);
@@ -46,11 +86,13 @@ void setup() {
 }
 
 void loop() {
+  if (!mqtt_client.connected()) {
+    reconnectMqtt();
+  }
+  mqtt_client.loop();
+
   if (millis() - last_millis > 1000) {
-    Serial.printf("Speed L: %f, ", speedometer_left.getSpeed());
-    left_motor.toString();
-    Serial.printf("Speed R: %f, ", speedometer_right.getSpeed());
-    right_motor.toString();
+    debugger.sendMsg(Loglevel::info, "loop : eine Sekinde");
 
     last_millis = millis();
   }
@@ -64,6 +106,28 @@ void loop() {
     moveController.runMoveControl();
 }
 
+void reconnectMqtt() {
+  // Loop until reconnection
+  while (!mqtt_client.connected()) {
+    Serial.print("Attempting MQTT connection...");
+    // Create a random client ID
+    String clientId = "ESP32Rover-";
+    clientId += String(random(0xffff), HEX);
+    // Attempt to connect
+    if (mqtt_client.connect(clientId.c_str())) {
+      Serial.println("connected");
+      // Once connected, publish an announcement...
+      mqtt_client.publish("/Rover/Info", "Connected");
+    } else {
+      Serial.print("failed, rc=");
+      Serial.print(mqtt_client.state());
+      Serial.println(" try again in 5 seconds");
+      // Wait 5 seconds before retrying
+      delay(5000);
+    }
+  }
+}
+
 void callbackControllerAction() {
   if (Ps3.event.button_down.r3) { controllerPrintBattery(); }
   if (Ps3.event.button_down.l1) { moveController.setSpeed(speed -= 0.1); }
@@ -108,7 +172,11 @@ void controllerPrintBattery() {
 void driveWithControllerJoystick(int8_t x, int8_t y) {
   if (drive_mode != 1) return;
 
-  
+  double value_per_step = MAX_SPEED * 2 / 256;
+  moveController.setSpeed((y * -1) * value_per_step);
+
+  value_per_step = MAX_ROTATION * 2 / 256;
+  moveController.setRotationspeed(x * value_per_step);
 }
 
 void driveWithControllerShoulderTrigger(uint8_t left, uint8_t right) {
@@ -131,4 +199,11 @@ void changeDriveMode() {
   if (drive_mode == 2) {
     moveController.setSpeed(0);
   }
-}
+}
+
+void getDis(double lat1, double lon1, double lat2, double lon2) {
+  double lat = (lat1 + lat2) / 2 * 0.01745;
+  double dx = 111.3 * cos(lat) * (lon1 - lon2);
+  double dy = 111.3 * (lat1 - lat2);
+  double erg = sqrt(dx * dx + dy * dy);
+}

src/moveControl.cpp

@@ -25,7 +25,7 @@ void MoveControl::runMoveControl() {
     this->regulateMotors();
 }
 
-void MoveControl::setDrivingStatus(drivingStatus status) {
+void MoveControl::setDrivingStatus(DrivingStatus status) {
     this->driving_status = status;
 }
 
@@ -53,13 +53,14 @@ void MoveControl::calcWheelSpeed() {
 }
 
 void MoveControl::regulateMotors() {
+    double ratio = 0;
     switch (this->driving_status) {
-    case drivingStatus::stop :
+    case DrivingStatus::stop :
         this->left_motor->setTargetPower(0);
         this->right_motor->setTargetPower(0);
         break;
 
-    case drivingStatus::straightForward :
+    case DrivingStatus::straightForward :
         // Left motor
         // Too slow
         if (this->left_speedometer->getSpeed() < this->wheelspeed_left_target
@@ -89,7 +90,7 @@ void MoveControl::regulateMotors() {
         }
         break;
 
-    case drivingStatus::straightBackward :
+    case DrivingStatus::straightBackward :
         // Left motor
         // Too slow
         if (this->left_speedometer->getSpeed() > this->wheelspeed_left_target
@@ -119,8 +120,8 @@ void MoveControl::regulateMotors() {
         }
         break;
 
-    case drivingStatus::arcForwardLeft : // Identical with arcForwardRight
-        double ratio = this->wheelspeed_left_target / this->wheelspeed_right_target;
+    case DrivingStatus::arcForwardLeft : // Identical with arcForwardRight
+        ratio = this->wheelspeed_left_target / this->wheelspeed_right_target;
 
         // Left motor
         // Too slow
@@ -151,8 +152,8 @@ void MoveControl::regulateMotors() {
         }
         break;
 
-    case drivingStatus::arcForwardRight : // Identical with arcForwardLeft
-        double ratio = this->wheelspeed_left_target / this->wheelspeed_right_target;
+    case DrivingStatus::arcForwardRight : // Identical with arcForwardLeft
+        ratio = this->wheelspeed_left_target / this->wheelspeed_right_target;
 
         // Left motor
         // Too slow
@@ -183,8 +184,8 @@ void MoveControl::regulateMotors() {
         }
         break;
 
-    case drivingStatus::arcBackwardLeft : // Identical with arcBackwardRight
-        double ratio = this->wheelspeed_left_target / this->wheelspeed_right_target;
+    case DrivingStatus::arcBackwardLeft : // Identical with arcBackwardRight
+        ratio = this->wheelspeed_left_target / this->wheelspeed_right_target;
 
         // Left motor
         // Too slow
@@ -215,8 +216,8 @@ void MoveControl::regulateMotors() {
         }
         break;
 
-    case drivingStatus::arcBackwardRight : // Identical with arcBackwardLeft
-        double ratio = this->wheelspeed_left_target / this->wheelspeed_right_target;
+    case DrivingStatus::arcBackwardRight : // Identical with arcBackwardLeft
+        ratio = this->wheelspeed_left_target / this->wheelspeed_right_target;
 
         // Left motor
         // Too slow
@@ -247,7 +248,7 @@ void MoveControl::regulateMotors() {
         }
         break;
 
-    case drivingStatus::rotateLeft :
+    case DrivingStatus::rotateLeft :
         // Left motor
         // Too slow
         if (this->left_speedometer->getSpeed() > this->wheelspeed_left_target
@@ -277,7 +278,7 @@ void MoveControl::regulateMotors() {
         }
         break;
 
-    case drivingStatus::rotateRight :
+    case DrivingStatus::rotateRight :
         // Left motor
         // Too slow
         if (this->left_speedometer->getSpeed() < this->wheelspeed_left_target

src/moveControl.h

@@ -7,7 +7,7 @@
 #include "speedometer.h"
 #include "config.h"
 
-enum drivingStatus {stop,
+enum DrivingStatus {stop,
                     straightForward,
                     straightBackward,
                     arcForwardLeft,
@@ -17,10 +17,10 @@ enum drivingStatus {stop,
                     rotateLeft,
                     rotateRight};
 
-enum regulateStatus {stop,
-                    drive,
-                    rotateLeft,
-                    rotateRight};
+// enum regulateStatus {stop,
+//                     drive,
+//                     rotateLeft,
+//                     rotateRight};
 
 class MoveControl {
     public:
@@ -28,7 +28,7 @@ class MoveControl {
         void init(MotorControl *left_motor, MotorControl *right_motor,
                     Speedometer *left_encoder, Speedometer *right_encoder);
         void runMoveControl();
-        void setDrivingStatus(drivingStatus status);
+        void setDrivingStatus(DrivingStatus status);
         void setSpeed(double speed);
         void setRotationspeed(double speed);
 
@@ -41,7 +41,7 @@ class MoveControl {
         Speedometer *left_speedometer;
         Speedometer *right_speedometer;
 
-        drivingStatus driving_status = drivingStatus::stop;
+        DrivingStatus driving_status = DrivingStatus::stop;
 
         double x_speed = 0;
         double rotation_speed = 0;

src/route.cpp

@@ -0,0 +1,41 @@
+#include "route.h"
+
+Route::Route(){
+
+}
+
+void Route::addPoint(Point point) {
+    this->points.push_back(point);
+    this->count_points++;
+}
+
+void Route::delRoute() {
+    this->points.clear();
+    this->count_points = 0;
+}
+
+Point Route::startRoute() {
+    this->route_started = true;
+    this->route_finished = false;
+
+    this->it = this->points.begin();
+    return *it;
+}
+
+Point Route::getNextPoint() {
+    if (this->route_started && !route_finished) {
+        it++;
+        return *it;
+    }
+
+    if (it == this->points.end()) {
+        this->route_finished = true;
+    }
+
+    Point fail;
+    return fail;
+}
+
+uint16_t Route::getNumberOfPoints() {
+    return this->count_points;
+}

src/route.h

@@ -0,0 +1,40 @@
+#ifndef ROUTE_H
+#define ROUTE_H
+
+#include <cstdint>
+#include <list>
+
+#include "motorControl.h"
+#include "speedometer.h"
+#include "config.h"
+
+struct Point{
+    float N = 0;
+    float E = 0;
+};
+
+class Route {
+    public:
+        Route();
+
+        void addPoint(Point point);
+        void delRoute();
+        void resetRoute();
+
+        Point startRoute();
+        Point getNextPoint();
+        uint16_t getNumberOfPoints();
+
+
+    private:
+        std::list<Point> points;
+        std::list<Point>::iterator it;
+
+        uint16_t count_points = 0;
+
+        bool route_started = false;
+        bool route_finished = false;
+
+};
+
+#endif // ROUTE_H