Added PS3 Controller

New strategy in moveControl

src/main.cpp

@@ -1,15 +1,15 @@
 #include <Arduino.h>
+#include <Ps3Controller.h>
 
 #include "config.h"
 #include "motorControl.h"
 #include "moveControl.h"
 #include "speedometer.h"
 
-#define CUSTOM_SETTINGS
-#define INCLUDE_GAMEPAD_MODULE
-#include <DabbleESP32.h>
-
-void gamepadInput();
+void callbackControllerAction();
+void callbackControllerConnect();
+void callbackControllerDisconnect();
+void controllerPrintBattery();
 
 MotorControl left_motor;
 MotorControl right_motor;
@@ -18,10 +18,20 @@ Speedometer speedometer_right;
 MoveControl moveController;
 
 uint64_t last_millis = 0;
+int controller_battery = -1;
+uint8_t controller_led = 1;
+
+// Temp code
+double speed = 0;
 
 void setup() {
   Serial.begin(115200);      // make sure your Serial Monitor is also set at this baud rate.
-  Dabble.begin("Kleiax-Rover");       //set bluetooth name of your device
+
+  Ps3.attach(callbackControllerAction);
+  Ps3.attachOnConnect(callbackControllerConnect);
+  Ps3.attachOnDisconnect(callbackControllerDisconnect);
+  Serial.println("\nReady to connect");
+  Ps3.begin();
 
   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);
@@ -33,43 +43,53 @@ void setup() {
 }
 
 void loop() {
-  if (millis() - last_millis > 400) {
-    gamepadInput();
-    // left_motor.toString();
-    // Serial.printf("speed: %f, ", speedometer_right.getSpeed());
-    // right_motor.toString();
-    // Serial.printf("Speed L: %f, Speed R: %f \n", speedometer_left.getSpeed(), speedometer_right.getSpeed());
+  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();
 
     last_millis = millis();
   }
 
-  // left_motor.runMotorControl();
+  left_motor.runMotorControl();
   right_motor.runMotorControl();
-  // speedometer_left.runSpeedometer();
+  speedometer_left.runSpeedometer();
   speedometer_right.runSpeedometer();
-  // moveController.runMoveControl();
+  moveController.runMoveControl();
 }
 
-void gamepadInput() {
-  Dabble.processInput();             //this function is used to refresh data obtained from smartphone.Hence calling this function is mandatory in order to get data properly from your mobile.
-  //Serial.print("KeyPressed: ");
-
-  if (GamePad.isUpPressed()) {
-    moveController.setSpeed(1.3);
-  } else if (GamePad.isDownPressed()) {
-    moveController.setSpeed(0);
-  } else {
-  }
-
-  if (GamePad.isTrianglePressed()) {
-    right_motor.setTargetSpeed(20);
-  } else if (GamePad.isCrossPressed()) {
-    right_motor.setTargetSpeed(30);
-  } else if (GamePad.isCirclePressed()){
-    right_motor.setTargetSpeed(0);
-  }
-
-  // int a = GamePad.getAngle();
-  // int b = GamePad.getRadius();
-  // Serial.printf("Winkel: %d, Radius: %d \n", a, b);
+void callbackControllerAction() {
+  if (Ps3.event.button_down.r3) { controllerPrintBattery(); }
+  if (Ps3.event.button_down.l1) { moveController.setSpeed(speed -= 0.1); }
+  if (Ps3.event.button_down.r1) { moveController.setSpeed(speed += 0.1); }
+  if (Ps3.event.button_down.up) { moveController.setDrivingStatus(drivingStatus::straightForward); }
+  if (Ps3.event.button_up.up)   { moveController.setDrivingStatus(drivingStatus::stop); }
+}
+
+void callbackControllerConnect() {
+  Serial.println("Controller connected to ESP32");
+
+  delay(400);
+  Serial.print("Setting LEDs to Status "); Serial.println(controller_led, DEC);
+  Ps3.setPlayer(controller_led);
+}
+
+void callbackControllerDisconnect() {
+  Serial.println("Controller disconnected from ESP32");
+}
+
+void controllerPrintBattery() {
+  if( controller_battery != Ps3.data.status.battery ){
+    controller_battery = Ps3.data.status.battery;
+  }
+
+  Serial.print("The controller battery is ");
+  if( controller_battery == ps3_status_battery_charging )      Serial.println("charging");
+  else if( controller_battery == ps3_status_battery_full )     Serial.println("FULL");
+  else if( controller_battery == ps3_status_battery_high )     Serial.println("HIGH");
+  else if( controller_battery == ps3_status_battery_low)       Serial.println("LOW");
+  else if( controller_battery == ps3_status_battery_dying )    Serial.println("DYING");
+  else if( controller_battery == ps3_status_battery_shutdown ) Serial.println("SHUTDOWN");
+  else Serial.println("UNDEFINED");
 }