diff --git a/platformio.ini b/platformio.ini index 74216ef..5376795 100644 --- a/platformio.ini +++ b/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 diff --git a/src/main.cpp b/src/main.cpp index cfdc4fe..fe80d2e 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -1,153 +1,36 @@ #include #include "config.h" +#include "motorControl.h" #define CUSTOM_SETTINGS #define INCLUDE_GAMEPAD_MODULE #include - -// Functions -void setMotorDirection(uint8_t motor ,uint8_t direction = FORWARD); -void setMotorSpeed(uint8_t motor, uint8_t speed); +MotorControl left_motor; 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 - 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); + left_motor.init(M_PWM_1, PWM_CHANNEL_M1, M_DIR_11, M_DIR_12); } 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); - } 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"); + left_motor.setTargetSpeed(-100); } else if (GamePad.isCirclePressed()) { - setMotorDirection(RIGHT_MOTOR, FORWARD); - Serial.print("3\n"); - } else if (GamePad.isCrossPressed()) { - setMotorDirection(RIGHT_MOTOR, BACKWARD); - Serial.print("4\n"); - - - - - } else { - setMotorDirection(BOTH_MOTOR, STOP); + left_motor.setTargetSpeed(0); } } -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"); - } -} \ No newline at end of file diff --git a/src/mototControl.cpp b/src/motorControl.cpp similarity index 80% rename from src/mototControl.cpp rename to src/motorControl.cpp index 2775651..ee4bf49 100644 --- a/src/mototControl.cpp +++ b/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; diff --git a/src/motorControl.h b/src/motorControl.h index 372aa89..d08b442 100644 --- a/src/motorControl.h +++ b/src/motorControl.h @@ -2,14 +2,20 @@ #define MOTOR_CONTROL_H #include +#include 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); diff --git a/src/moveControl.cpp b/src/moveControl.cpp new file mode 100644 index 0000000..2c5d952 --- /dev/null +++ b/src/moveControl.cpp @@ -0,0 +1,57 @@ +#include "moveControl.h" + +#include + +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; +} diff --git a/src/moveControl.h b/src/moveControl.h new file mode 100644 index 0000000..641e893 --- /dev/null +++ b/src/moveControl.h @@ -0,0 +1,25 @@ +#ifndef MOVE_CONTROL_H +#define MOVE_CONTROL_H + +#include + +#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 \ No newline at end of file