From e798f0904cc37f509e9f4ecba290777848139379 Mon Sep 17 00:00:00 2001 From: Alexander Klein Date: Mon, 15 Mar 2021 22:58:28 +0100 Subject: [PATCH] Added moveControll in main.cpp --- src/config.h | 2 +- src/main.cpp | 95 ++++++++++++++++++++++++++++++++++++++------ src/motorControl.cpp | 12 +++--- 3 files changed, 88 insertions(+), 21 deletions(-) diff --git a/src/config.h b/src/config.h index 8dc2c97..9968266 100644 --- a/src/config.h +++ b/src/config.h @@ -23,5 +23,5 @@ #define PWM_CHANNEL_M2 1 #define UPDATE_TIME 25 #define SPEED_STEPS 5 // A total of 20 levels ( 100 / SPEED_STEPS ) * UPDATE_TIME = 500ms -#define PWM_MIN 40 +#define PWM_MIN 150 #define PWM_MAX 245 // Max 98% of 2^PWM_RES \ No newline at end of file diff --git a/src/main.cpp b/src/main.cpp index fe80d2e..ac5b507 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -7,30 +7,99 @@ #define INCLUDE_GAMEPAD_MODULE #include +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; } diff --git a/src/motorControl.cpp b/src/motorControl.cpp index ee4bf49..747b4b8 100644 --- a/src/motorControl.cpp +++ b/src/motorControl.cpp @@ -26,8 +26,7 @@ void MotorControl::init(uint8_t pwm_pin, uint8_t pwm_channel, uint8_t direction_ } void MotorControl::runMotorControl() { - uint64_t now_millis = millis(); - if (now_millis - this->last_millis > UPDATE_TIME) { + if (millis() - this->last_millis > 300) { // Absolute difference between target_speed and speed uint8_t abs_difference = abs(this->target_speed - this->speed); @@ -38,17 +37,18 @@ void MotorControl::runMotorControl() { // Check that the target speed is close to 0 and that the abs_difference is lower than SPEED_STEPS if (abs(this->target_speed) < SPEED_STEPS && abs_difference < SPEED_STEPS) { this->setSpeed(0); + this->last_millis = millis(); return; } // Correct speed if (abs_difference < SPEED_STEPS) { + this->last_millis = millis(); return; } // Positive or negative tagret speed if (this->target_speed >= 0) { - // Positive or negative speed if (this->speed >= 0) { if (difference > 0) { @@ -61,7 +61,6 @@ void MotorControl::runMotorControl() { } } else { - // Positive or negative speed if (this->speed >= 0) { this->accelerate(-SPEED_STEPS); @@ -73,9 +72,8 @@ void MotorControl::runMotorControl() { } } } + this->last_millis = millis(); } - - this->last_millis = millis(); } void MotorControl::setTargetSpeed(int16_t speed) { @@ -140,7 +138,7 @@ void MotorControl::setSpeed(int16_t speed) { } ledcWrite(this->pwm_channel, pwm_val); - Serial.printf("pwm_val: %d, ", pwm_val); + // Serial.printf("pwm_val: %d, ", pwm_val); } void MotorControl::accelerate(int8_t acc) {