Added moveControll in main.cpp

src/main.cpp

@@ -7,30 +7,99 @@
 #define INCLUDE_GAMEPAD_MODULE
 #include <DabbleESP32.h>
 
+void gamepadInput();
+int8_t modSpeed(int8_t dir, int8_t speed);
+
 MotorControl left_motor;
+MotorControl right_motor;
+
+uint64_t last_millis = 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
 
   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);
 
 }
 
 void loop() {
-  delay(500);
-  left_motor.runMotorControl();
-  left_motor.toString();
-  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()) {
-    Serial.print("Up\n");
-    left_motor.setTargetSpeed(100);
-  } else if (GamePad.isDownPressed()) {
-    Serial.print("Down\n");
-    left_motor.setTargetSpeed(-100);
-  } else if (GamePad.isCirclePressed()) {
-    left_motor.setTargetSpeed(0);
+  if (millis() - last_millis > 300) {
+    gamepadInput();
+    Serial.print("Motor Links: ");
+    left_motor.toString();
+    Serial.print("Motor Rechts: ");
+    right_motor.toString();
+
+    last_millis = millis();
   }
+  left_motor.runMotorControl();
+  right_motor.runMotorControl();
+}
+
+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: ");
+
+  static int8_t speed_left = 0;
+  if (GamePad.isUpPressed()) {
+    speed_left = modSpeed(1, speed_left);
+  } else if (GamePad.isDownPressed()) {
+    speed_left = modSpeed(-1, speed_left);
+  } else {
+    speed_left = modSpeed(0, speed_left);
+  }
+
+  static int8_t speed_right = 0;
+  if (GamePad.isTrianglePressed()) {
+    speed_right = modSpeed(1, speed_right);
+  } else if (GamePad.isCrossPressed()) {
+    speed_right = modSpeed(-1, speed_right);
+  } else {
+    speed_right = modSpeed(0, speed_right);
+  }
+
+  left_motor.setTargetSpeed(speed_left);
+  right_motor.setTargetSpeed(speed_right);
+}
+
+int8_t modSpeed(int8_t dir, int8_t speed) {
+  int8_t new_speed = 0;
+  if (dir == 1) {
+    if (speed > 0) {
+      new_speed = speed + 5;
+    } else if (speed == 0) {
+      new_speed = 5;
+    } else {
+      new_speed = speed + 5;
+    }
+  } else if (dir == 0) {
+    if (speed > 0) {
+      new_speed = speed - 5;
+    } else if (speed == 0) {
+      new_speed = 0;
+    } else {
+      new_speed = speed + 5;
+    }
+  } else if (dir == -1) {
+    if (speed > 0) {
+      new_speed = speed - 5;
+    } else if (speed == 0) {
+      new_speed = -5;
+    } else {
+      new_speed = speed - 5;
+    }
+  }
+
+  if (new_speed < -100) {
+    new_speed = -100;
+  }
+
+  if (new_speed > 100) {
+    new_speed = 100;
+  }
+
+  return new_speed;
 }