fix reconnect gamepad

src/main.cpp

@@ -14,14 +14,16 @@
  */
 
 #include <Arduino.h>
+
+#include "config.h"
+
 #include <Ps3Controller.h>
 #include <iostream>
 #include <Wire.h>
 #include <LiquidCrystal_I2C.h>
-#include <BluetoothSerial.h>
 #include <ESP32Ping.h>
-
-#include "config.h"
+#include <BluetoothSerial.h>
+#include <esp_bt_main.h>
 
 #include "driveModi/driveManager.h"
 #include "OutputBuf/outputBuf.h"
@@ -47,11 +49,15 @@ Menu* main_m;
 LiquidCrystal_I2C* lcd;
 OutputBuf* outputBuf;
 DebugMqtt* debugMqtt = nullptr;
-BluetoothSerial* serialBT = nullptr;
 Battery* mainBattery;
+Ps3Controller* gamepad;
+BluetoothSerial SerialBT;
 
 bool wifiIsActive;
+bool disconnectPs3 = false;
 
+
+void configureController(void);
 void callbackControllerAction(void);
 void callbackControllerConnect(void);
 void callbackControllerDisconnect(void);
@@ -61,7 +67,7 @@ void makeMenu(void);
 
 void setup() {
     Serial.begin(115200);
-    // DebugTimes::setConsolOutput(true);
+    DebugTimes::setConsolOutput(true);
     DebugTimes setupTime;
     char wifiIndicator = 'X';
 
@@ -104,18 +110,9 @@ void setup() {
     std::cout << "Welcome to Kleiax-Rover" << std::endl;
     std::cout << "All actions from the main program run on Core -> " << xPortGetCoreID() << std::endl;
 
-    Ps3.attach(callbackControllerAction);
-    Ps3.attachOnConnect(callbackControllerConnect);
-    Ps3.attachOnDisconnect(callbackControllerDisconnect);
-    std::cout << "\nReady to connect a PS3 Controller... \n";
+    gamepad = new Ps3Controller;
+    configureController();
 
-    // Need to be connected befor an other device connect
-    Ps3.begin(CONTROLLER_MAC);
-
-    std::cout << "Activate additional output via Bluetooth..." << std::endl;
-    serialBT = new BluetoothSerial;
-    serialBT->begin("Kleiax-Rover");
-    outputBuf->setSerialBT(serialBT);
     outputBuf->activateMqtt(false);
 
     lcd->backlight();
@@ -141,11 +138,26 @@ void loop() {
     mainBattery->loop();
     mainBatteryTime.stopConsol("MainBatteryTime", 5);
 
-    // static uint32_t lastMillis = 0;
-    // if (millis() - lastMillis > 2000) {
-    //     std::cout << "Is BT-Consol connected: " << serialBT->connected() << std::endl;
-    //     lastMillis = millis();
-    // }
+    if (disconnectPs3) {
+        DebugTimes disconnectPS3Time;
+        std::cout << "Disconnect PS3 Controller." << std::endl;
+        gamepad->end();
+        esp_bluedroid_disable();
+        esp_bluedroid_deinit();
+        configureController();
+        disconnectPs3 = false;
+        disconnectPS3Time.stopConsol("Ps3 Disconnect");
+    }
+}
+
+void configureController() {
+    
+    gamepad->attach(callbackControllerAction);
+    gamepad->attachOnConnect(callbackControllerConnect);
+    gamepad->attachOnDisconnect(callbackControllerDisconnect);
+    std::cout << "\nReady to connect a PS3 Controller... \n";
+
+    gamepad->begin(CONTROLLER_MAC);
 }
 
 void callbackControllerAction() {
@@ -153,7 +165,7 @@ void callbackControllerAction() {
         static uint32_t lastMillis = 0;
         if (reset)
             lastMillis = millis();
-        static const uint16_t delay = 300;
+        static const uint16_t delay = 100;
         bool res = false;
 
         if (millis() - lastMillis > delay)
@@ -163,19 +175,19 @@ void callbackControllerAction() {
 
     // Menu controlling
     if (isDelayReached()) {
-        if (Ps3.data.button.up)
+        if (gamepad->data.button.up)
             main_m->up();
-        else if (Ps3.data.button.down)
+        else if (gamepad->data.button.down)
             main_m->down();
-        else if (Ps3.data.button.left)
+        else if (gamepad->data.button.left)
             main_m->left();
-        else if (Ps3.data.button.right)
+        else if (gamepad->data.button.right)
             main_m->right();
-        else if (Ps3.data.button.circle)
+        else if (gamepad->data.button.circle)
             main_m->yes();
-        else if (Ps3.data.button.cross)
+        else if (gamepad->data.button.cross)
             main_m->no();
-        else if (Ps3.data.button.select)
+        else if (gamepad->data.button.select)
             debugMqtt->sendMsg(Loglevel::info, "Here we are: callbackController..");
 
         isDelayReached(true);        
@@ -185,18 +197,23 @@ void callbackControllerAction() {
 }
 
 void callbackControllerConnect() {
+    static bool firstConnect = true;
+
     std::cout << "Controller connected to ESP32" << std::endl;
     std::cout << "All actions from the Controller run on Core -> " << xPortGetCoreID() << std::endl;
 
     // Display is not functional without this seconde init, first init is in setup(),
     // because after the setup the other core control the display.
-    lcd->init();
-    main_m->printMenu();
+    if (firstConnect) {
+        lcd->init();
+        main_m->printMenu();
+        firstConnect = false;
+    }
 }
 
 void callbackControllerDisconnect() {
     std::cout << "Controller disconnected..." << std::endl;
-    // Ps3.end();
+    // gamepad->end();
 }
 
 void i2cScanner() {
@@ -243,7 +260,7 @@ void makeMenu() {
     };
 
     auto disconnectController = [&]() {
-        Ps3.end();
+        disconnectPs3 = true;
     };
 
     // Create Menu
@@ -257,7 +274,7 @@ void makeMenu() {
     MenuAutopilot* auto_m = new MenuAutopilot(driveManager);
     MenuTestMode* test_m = new MenuTestMode(driveManager);
     MenuGPS* gps_m = new MenuGPS(driveManager);
-    MenuAkku* akku_m = new MenuAkku(mainBattery);
+    MenuAkku* akku_m = new MenuAkku(mainBattery, gamepad);
 
     // Set Menus on LCD
     main_m->setLcd(lcd);