Added move Controll

platformio.ini

@@ -13,4 +13,7 @@ platform = espressif32
 board = esp32doit-devkit-v1
 framework = arduino
 monitor_speed = 115200
+monitor_port = COM12
 lib_deps = stempedia/DabbleESP32@^1.5.1
+
+upload_port = COM12

src/main.cpp

@@ -1,153 +1,36 @@
 #include <Arduino.h>
 
 #include "config.h"
+#include "motorControl.h"
 
 #define CUSTOM_SETTINGS
 #define INCLUDE_GAMEPAD_MODULE
 #include <DabbleESP32.h>
 
-
+MotorControl left_motor;
-// Functions
-void setMotorDirection(uint8_t motor ,uint8_t direction = FORWARD);
-void setMotorSpeed(uint8_t motor, uint8_t speed);
 
 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
 
-  //SETUP PINS
+  left_motor.init(M_PWM_1, PWM_CHANNEL_M1, M_DIR_11, M_DIR_12);
-  pinMode(M_DIR_11, OUTPUT);
-  pinMode(M_DIR_12, OUTPUT);
-  pinMode(M_DIR_21, OUTPUT);
-  pinMode(M_DIR_22, OUTPUT);
-
-  digitalWrite(M_DIR_11, LOW);
-  digitalWrite(M_DIR_12, LOW);
-  digitalWrite(M_DIR_21, LOW);
-  digitalWrite(M_DIR_22, LOW);
-
-  ledcSetup(PWM_CHANNEL_M1, PWM_FREQ, PWM_RES);
-  ledcSetup(PWM_CHANNEL_M2, PWM_FREQ, PWM_RES);
-  ledcAttachPin(M_PWM_1, PWM_CHANNEL_M1);
-  ledcAttachPin(M_PWM_2, PWM_CHANNEL_M2);
-  ledcWrite(M_PWM_1, 0);
-  ledcWrite(M_PWM_2, 0);
-
-
-  //Start speed
-  setMotorSpeed(BOTH_MOTOR, 255);
 
 }
 
 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");
-    setMotorDirection(BOTH_MOTOR, FORWARD);
+    left_motor.setTargetSpeed(100);
   } else if (GamePad.isDownPressed()) {
     Serial.print("Down\n");
-    setMotorDirection(BOTH_MOTOR, BACKWARD);
+    left_motor.setTargetSpeed(-100);
-  } else if (GamePad.isLeftPressed()) {
-    Serial.print("Left\n");
-    setMotorDirection(LEFT_MOTOR, BACKWARD);
-    setMotorDirection(RIGHT_MOTOR, FORWARD);
-  } else if (GamePad.isRightPressed()) {
-    Serial.print("Right\n");
-    setMotorDirection(LEFT_MOTOR, FORWARD);
-    setMotorDirection(RIGHT_MOTOR, BACKWARD);
-
-  } else if (GamePad.isSquarePressed()) {
-    setMotorDirection(LEFT_MOTOR, FORWARD);
-    Serial.print("1\n");
-  } else if (GamePad.isTrianglePressed()) {
-    setMotorDirection(LEFT_MOTOR, BACKWARD);
-    Serial.print("2\n");
   } else if (GamePad.isCirclePressed()) {
-    setMotorDirection(RIGHT_MOTOR, FORWARD);
+    left_motor.setTargetSpeed(0);
-    Serial.print("3\n");
-  } else if (GamePad.isCrossPressed()) {
-    setMotorDirection(RIGHT_MOTOR, BACKWARD);
-    Serial.print("4\n");
-  
-
-
-
-  } else {
-    setMotorDirection(BOTH_MOTOR, STOP);
   }
 }
 
-void setMotorDirection(uint8_t motor ,uint8_t direction) {
-  if (motor == LEFT_MOTOR) {
-    if (direction == FORWARD) {
-      digitalWrite(M_DIR_11, HIGH);
-      digitalWrite(M_DIR_12, LOW);
-    } else if (direction == BACKWARD) {
-      digitalWrite(M_DIR_11, LOW);
-      digitalWrite(M_DIR_12, HIGH);
-    } else if (direction == STOP) {
-      digitalWrite(M_DIR_11, LOW);
-      digitalWrite(M_DIR_12, LOW);
-    } else {
-      digitalWrite(M_DIR_21, LOW);
-      digitalWrite(M_DIR_22, LOW);
-      Serial.print("setDirection: undefined direction\n");
-    }
-  } else if (motor == RIGHT_MOTOR) {
-    if (direction == FORWARD) {
-      digitalWrite(M_DIR_21, HIGH);
-      digitalWrite(M_DIR_22, LOW);
-    } else if (direction == BACKWARD) {
-      digitalWrite(M_DIR_21, LOW);
-      digitalWrite(M_DIR_22, HIGH);
-    } else if (direction == STOP) {
-      digitalWrite(M_DIR_21, LOW);
-      digitalWrite(M_DIR_22, LOW);
-    } else {
-      digitalWrite(M_DIR_21, LOW);
-      digitalWrite(M_DIR_22, LOW);
-      Serial.print("setDirection: undefined direction\n");
-    }
-  } else if (motor == BOTH_MOTOR) {
-    if (direction == FORWARD) {
-      digitalWrite(M_DIR_11, HIGH);
-      digitalWrite(M_DIR_12, LOW);
-      digitalWrite(M_DIR_21, HIGH);
-      digitalWrite(M_DIR_22, LOW);
-    } else if (direction == BACKWARD) {
-      digitalWrite(M_DIR_11, LOW);
-      digitalWrite(M_DIR_12, HIGH);
-      digitalWrite(M_DIR_21, LOW);
-      digitalWrite(M_DIR_22, HIGH);
-    } else if (direction == STOP) {
-      digitalWrite(M_DIR_11, LOW);
-      digitalWrite(M_DIR_12, LOW);
-      digitalWrite(M_DIR_21, LOW);
-      digitalWrite(M_DIR_22, LOW);
-    }else {
-      digitalWrite(M_DIR_11, LOW);
-      digitalWrite(M_DIR_12, LOW);
-      digitalWrite(M_DIR_21, LOW);
-      digitalWrite(M_DIR_22, LOW);
-      Serial.print("setDirection: undefined direction\n");
-    }
-  } else {
-    Serial.print("setDirection: undefined motor\n");
-  }
-}
-
-void setMotorSpeed(uint8_t motor, uint8_t speed) {
-  if (motor == LEFT_MOTOR) {
-    ledcWrite(PWM_CHANNEL_M1, speed);
-  } else if (motor == RIGHT_MOTOR) {
-    ledcWrite(PWM_CHANNEL_M2, speed);
-  } else if (motor == BOTH_MOTOR) {
-    ledcWrite(PWM_CHANNEL_M1, speed);
-    ledcWrite(PWM_CHANNEL_M2, speed);
-  } else {
-    ledcWrite(PWM_CHANNEL_M1, 0);
-    ledcWrite(PWM_CHANNEL_M2, 0);
-    Serial.print("setSpeed: undefined motor \n");
-  }
-}

src/motorControl.cpp

@@ -4,7 +4,11 @@
 
 #include "config.h"
 
-MotorControl::MotorControl(uint8_t pwm_pin, uint8_t pwm_channel, uint8_t direction_pin_1, uint8_t direction_pin_2) {
+MotorControl::MotorControl() {
+
+}
+
+void MotorControl::init(uint8_t pwm_pin, uint8_t pwm_channel, uint8_t direction_pin_1, uint8_t direction_pin_2) {
     this->pwm_pin = pwm_pin;
     this->pwm_channel = pwm_channel;
     this->direction_pin_1 = direction_pin_1;
@@ -74,7 +78,7 @@ void MotorControl::runMotorControl() {
     this->last_millis = millis();
 }
 
-void MotorControl::setTargetSpeed(uint8_t speed) {
+void MotorControl::setTargetSpeed(int16_t speed) {
     //TODO: Exceptionhandling
     if (speed <= 100 || speed <= -100) {
         this->target_speed = speed;
@@ -91,12 +95,28 @@ void MotorControl::emergencyStop() {
     setSpeed(0);
 }
 
+void MotorControl::toString() {
+    Serial.printf("Target speed: %d, speed: %d, direction: %d \n", this->target_speed, this->speed, this->direction);
+}
+
+int16_t MotorControl::getSpeed() {
+    return this->speed;
+}
+
+bool MotorControl::isTargetSpeedReached() {
+    if (this->target_speed == this->speed) {
+        return true;
+    }
+    return false;
+}
+
 void MotorControl::setSpeed(int16_t speed) {
     //TODO: Exceptionhandling
     if (speed <= 100 || speed <= -100) {
-        this->target_speed = speed;
+        this->speed = speed;
     } else {
         Serial.println("Invalid Argument in MotorControl::setSpeed");
+        return;
     }
 
     if (speed == 0) {
@@ -104,28 +124,28 @@ void MotorControl::setSpeed(int16_t speed) {
         digitalWrite(this->direction_pin_1, LOW);
         digitalWrite(this->direction_pin_2, LOW);
         ledcWrite(this->pwm_channel, 0);
-        this->speed = speed;
         return;
     }
 
     uint8_t pwm_val = map(abs(speed), 0, 100, PWM_MIN, PWM_MAX);
 
-    if (this->direction == 1 && speed < 0){  // new direction backward
+    if ((this->direction == 1 || this->direction == 0) && speed < 0){  // new direction backward
         this->direction = 2;
         digitalWrite(this->direction_pin_1, LOW);
         digitalWrite(this->direction_pin_2, HIGH);
-    } else if (this->direction == 2 && speed > 0){  // new direction forward
+    } else if ((this->direction == 2 || this->direction == 0) && speed > 0){  // new direction forward
         this->direction = 1;
         digitalWrite(this->direction_pin_1, HIGH);
         digitalWrite(this->direction_pin_2, LOW);
     }
 
     ledcWrite(this->pwm_channel, pwm_val);
-    this->speed = speed;
+    Serial.printf("pwm_val: %d, ", pwm_val);
 }
 
 void MotorControl::accelerate(int8_t acc) {
     //TODO: Exceptionhandling
+    //TODO: make a stop befor a direction change
     if (abs(acc) > 2 * SPEED_STEPS) {
         Serial.println("Invalid Argument in MotorControl::accelerate");
         return;

src/motorControl.h

@@ -2,14 +2,20 @@
 #define MOTOR_CONTROL_H
 
 #include <cstdint>
+#include <string>
 
 class MotorControl {
     public:
-        MotorControl(uint8_t pwm_pin, uint8_t pwm_channel, uint8_t direction_1, uint8_t direction_2);
+        MotorControl();
+        void init(uint8_t pwm_pin, uint8_t pwm_channel, uint8_t direction_1, uint8_t direction_2);
         void runMotorControl();
         void setTargetSpeed(int16_t speed);
         void stop();
         void emergencyStop();
+        void toString();
+
+        int16_t getSpeed();
+        bool isTargetSpeedReached();
 
     private:
         void setSpeed(int16_t speed);

src/moveControl.cpp

@@ -0,0 +1,57 @@
+#include "moveControl.h"
+
+#include <Arduino.h>
+
+MoveControl::MoveControl() {
+
+}
+
+void MoveControl::init(MotorControl *left_motor, MotorControl *right_motor) {
+    this->left_motor = left_motor;
+    this->right_motor = right_motor;
+}
+
+void MoveControl::rotate(int16_t degree) {
+    this->left_motor->setTargetSpeed(0);
+    this->right_motor->setTargetSpeed(0);
+
+    while ( !(left_motor->isTargetSpeedReached() && right_motor->isTargetSpeedReached()) ) {
+        delay(5);
+    }
+
+    uint8_t left = 0;
+    uint8_t right = 0;
+    if (degree < 0) {
+        left = -this->speed;
+        right = this->speed;
+    } else if (degree > 0) {
+        left = this->speed;
+        right = -this->speed;
+    }
+
+    this->left_motor->setTargetSpeed(this->speed);
+    this->right_motor->setTargetSpeed(this->speed);
+
+    delay(abs(degree));
+
+    this->left_motor->setTargetSpeed(0);
+    this->right_motor->setTargetSpeed(0);
+}
+
+void MoveControl::rotate(int16_t degree, uint8_t radius) {
+
+}
+
+void MoveControl::forward() {
+    this->left_motor->setTargetSpeed(this->speed);
+    this->right_motor->setTargetSpeed(this->speed);
+}
+
+void MoveControl::backward() {
+    this->left_motor->setTargetSpeed(-this->speed);
+    this->right_motor->setTargetSpeed(-this->speed);
+}
+
+void MoveControl::setSpeed(uint8_t speed) {
+    this->speed = speed;
+}

src/moveControl.h

@@ -0,0 +1,25 @@
+#ifndef MOVE_CONTROL_H
+#define MOVE_CONTROL_H
+
+#include <cstdint>
+
+#include "motorControl.h"
+
+class MoveControl {
+    public:
+        MoveControl();
+        void init(MotorControl *left_motor, MotorControl *right_motor);
+        void rotate(int16_t degree);
+        void rotate(int16_t degree, uint8_t radius);
+        void forward();
+        void backward();
+        void setSpeed(uint8_t speed);
+
+    private:
+        MotorControl *left_motor;
+        MotorControl *right_motor;
+
+        uint8_t speed = 0;
+};
+
+#endif // MOVE_CONTROL_H