Added debugMqtt and autoPilot

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);
+}