From 89050ae2d39d02d9dc75d0019670d49383d25a3a Mon Sep 17 00:00:00 2001 From: Alexander Klein Date: Mon, 19 Apr 2021 13:08:15 +0200 Subject: [PATCH] Added PS3 Controller New strategy in moveControl --- platformio.ini | 2 +- src/config.h | 38 +++++++++------- src/main.cpp | 94 +++++++++++++++++++++++---------------- src/motorControl.cpp | 88 ++++++++++++++++++------------------- src/motorControl.h | 16 +++---- src/moveControl.cpp | 102 ++++++++++++++++++++++++++++++------------- src/moveControl.h | 15 ++++++- src/speedometer.cpp | 6 +-- 8 files changed, 221 insertions(+), 140 deletions(-) diff --git a/platformio.ini b/platformio.ini index 1e1e236..af3ee9d 100644 --- a/platformio.ini +++ b/platformio.ini @@ -15,6 +15,6 @@ framework = arduino monitor_speed = 115200 monitor_port = COM12 lib_deps = - stempedia/DabbleESP32@^1.5.1 madhephaestus/ESP32Encoder@^0.4.0 + jvpernis/PS3 Controller Host@^1.1.0 upload_port = COM12 diff --git a/src/config.h b/src/config.h index 19dc00c..c0ba3a5 100644 --- a/src/config.h +++ b/src/config.h @@ -1,15 +1,19 @@ -//Pin config -#define M_DIR_11 27 -#define M_DIR_12 12 -#define M_PWM_1 13 -#define M_ENCODE_1A 33 -#define M_ENCODE_1B 32 +// The front is where the boards are -#define M_DIR_21 23 -#define M_DIR_22 14 -#define M_PWM_2 22 -#define M_ENCODE_2A 25 -#define M_ENCODE_2B 26 +//Pin config + //Left + #define M_DIR_11 27 + #define M_DIR_12 12 + #define M_PWM_1 13 + #define M_ENCODE_1A 33 + #define M_ENCODE_1B 32 + + //Right + #define M_DIR_21 23 + #define M_DIR_22 14 + #define M_PWM_2 22 + #define M_ENCODE_2A 26 + #define M_ENCODE_2B 25 //Motors #define LEFT_MOTOR 1 @@ -22,12 +26,12 @@ #define BACKWARD 2 //MotorControl config -#define RUN_MOTOR_CONTROL_DELAY 20 // Normal 10 +#define RUN_MOTOR_CONTROL_DELAY 10 // Normal 10 #define PWM_FREQ 16000 #define PWM_RES 8 #define PWM_CHANNEL_M1 0 #define PWM_CHANNEL_M2 1 -#define SPEED_STEPS 2 // A total of 20 levels ( 100 / SPEED_STEPS ) * RUN_MOTOR_CONTROL_DELAY = 500ms +#define POWER_STEPS 2 // A total of 20 levels ( 100 / SPEED_STEPS ) * RUN_MOTOR_CONTROL_DELAY = 500ms #define PWM_MIN 120 #define PWM_MAX 245 // Max 98% of 2^PWM_RES @@ -36,7 +40,11 @@ #define ENC_FILTER 1023 // 1023 is max #define ENC_STEPS 1024 #define WHEEL_DIAMETER 0.1263 //meter -#define BUF_SIZE 40 +#define BUF_SIZE 10 //MoveControl config -#define RUN_MOVE_CONTROL_DELAY 600 // Normal 10 untested \ No newline at end of file +#define RUN_MOVE_CONTROL_DELAY 150 // Normal 10 untested +#define ACCELERATE_STEPS 1 + +// PS3 Controller +// ESP32 MAC BL 24:62:AB:F2:4B:3A \ No newline at end of file diff --git a/src/main.cpp b/src/main.cpp index 0543432..63bd3df 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -1,15 +1,15 @@ #include +#include #include "config.h" #include "motorControl.h" #include "moveControl.h" #include "speedometer.h" -#define CUSTOM_SETTINGS -#define INCLUDE_GAMEPAD_MODULE -#include - -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"); } diff --git a/src/motorControl.cpp b/src/motorControl.cpp index 9dbf870..8d72137 100644 --- a/src/motorControl.cpp +++ b/src/motorControl.cpp @@ -28,47 +28,47 @@ void MotorControl::init(uint8_t pwm_pin, uint8_t pwm_channel, uint8_t direction_ void MotorControl::runMotorControl() { if (millis() - this->last_millis > RUN_MOTOR_CONTROL_DELAY) { - // Absolute difference between target_speed and speed - uint8_t abs_difference = abs(this->target_speed - this->speed); + // Absolute difference between target_power and speed + uint8_t abs_difference = abs(this->target_power - this->power); - // Difference between target_speed and speed - int16_t difference = this->target_speed - this->speed; + // Difference between target_power and speed + int16_t difference = this->target_power - this->power; - // 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); + // Check that the target speed is close to 0 and that the abs_difference is lower than POWER_STEPS + if (abs(this->target_power) < POWER_STEPS && abs_difference < POWER_STEPS) { + this->setRealPower(0); this->last_millis = millis(); return; } // Correct speed - if (abs_difference < SPEED_STEPS) { + if (abs_difference < POWER_STEPS) { this->last_millis = millis(); return; } // Positive or negative tagret speed - if (this->target_speed >= 0) { + if (this->target_power >= 0) { // Positive or negative speed - if (this->speed >= 0) { + if (this->power >= 0) { if (difference > 0) { - this->accelerate(SPEED_STEPS); + this->increasePower(POWER_STEPS); } else { - this->accelerate(-SPEED_STEPS); + this->increasePower(-POWER_STEPS); } } else { - this->accelerate(SPEED_STEPS); + this->increasePower(POWER_STEPS); } } else { // Positive or negative speed - if (this->speed >= 0) { - this->accelerate(-SPEED_STEPS); + if (this->power >= 0) { + this->increasePower(-POWER_STEPS); } else { if (difference > 0) { - this->accelerate(SPEED_STEPS); + this->increasePower(POWER_STEPS); } else { - this->accelerate(-SPEED_STEPS); + this->increasePower(-POWER_STEPS); } } } @@ -76,63 +76,63 @@ void MotorControl::runMotorControl() { } } -void MotorControl::setTargetSpeed(int16_t speed) { +void MotorControl::setTargetPower(int8_t power) { //TODO: Exceptionhandling - if (speed <= 100 && speed >= -100) { - this->target_speed = speed; + if (power <= 100 && power >= -100) { + this->target_power = power; } else { - Serial.println("Invalid Argument in MotorControl::setTargetSpeed"); + Serial.println("Invalid Argument in MotorControl::setTargetPower"); } } void MotorControl::stop() { - this->target_speed = 0; + this->target_power = 0; } void MotorControl::emergencyStop() { - setSpeed(0); + setRealPower(0); } void MotorControl::toString() { - Serial.printf("Target speed: %d, speed: %d, direction: %d \n", this->target_speed, this->speed, this->direction); + Serial.printf("Target power: %d, power: %d, direction: %d \n", this->target_power, this->power, this->direction); } -int16_t MotorControl::getSpeed() { - return this->speed; +int8_t MotorControl::getPower() { + return this->power; } -int16_t MotorControl::getTargetSpeed() { - return this->target_speed; +int8_t MotorControl::getTargetPower() { + return this->target_power; } -bool MotorControl::isTargetSpeedReached() { - if (this->target_speed == this->speed) +bool MotorControl::isTargetPowerReached() { + if (this->target_power == this->power) return true; return false; } bool MotorControl::isAccelerationPositive() { - if (speed < target_speed) + if (power < target_power) return true; return false; } bool MotorControl::isAccelerationNegative() { - if (speed > target_speed) + if (power > target_power) return true; return false; } -void MotorControl::setSpeed(int16_t speed) { +void MotorControl::setRealPower(int8_t power) { //TODO: Exceptionhandling - if (speed <= 100 || speed <= -100) { - this->speed = speed; + if (power <= 100 || power <= -100) { + this->power = power; } else { - Serial.println("Invalid Argument in MotorControl::setSpeed"); + Serial.println("Invalid Argument in MotorControl::setRealPower"); return; } - if (speed == 0) { + if (power == 0) { this->direction = 0; digitalWrite(this->direction_pin_1, LOW); digitalWrite(this->direction_pin_2, LOW); @@ -140,13 +140,13 @@ void MotorControl::setSpeed(int16_t speed) { return; } - uint8_t pwm_val = map(abs(speed), 0, 100, PWM_MIN, PWM_MAX); + uint8_t pwm_val = map(abs(power), 0, 100, PWM_MIN, PWM_MAX); - if ((this->direction == 1 || this->direction == 0) && speed < 0){ // new direction backward + if ((this->direction == 1 || this->direction == 0) && power < 0){ // new direction backward this->direction = 2; digitalWrite(this->direction_pin_1, LOW); digitalWrite(this->direction_pin_2, HIGH); - } else if ((this->direction == 2 || this->direction == 0) && speed > 0){ // new direction forward + } else if ((this->direction == 2 || this->direction == 0) && power > 0){ // new direction forward this->direction = 1; digitalWrite(this->direction_pin_1, HIGH); digitalWrite(this->direction_pin_2, LOW); @@ -156,13 +156,13 @@ void MotorControl::setSpeed(int16_t speed) { // Serial.printf("pwm_val: %d, ", pwm_val); } -void MotorControl::accelerate(int8_t acc) { +void MotorControl::increasePower(int8_t power) { //TODO: Exceptionhandling //TODO: make a stop befor a direction change - if (abs(acc) > 2 * SPEED_STEPS) { - Serial.println("Invalid Argument in MotorControl::accelerate"); + if (abs(power) > 2 * POWER_STEPS) { + Serial.println("Invalid Argument in MotorControl::increasePower"); return; } - this->setSpeed(speed + acc); + this->setRealPower(this->power + power); } \ No newline at end of file diff --git a/src/motorControl.h b/src/motorControl.h index ef5f97f..f44ea75 100644 --- a/src/motorControl.h +++ b/src/motorControl.h @@ -9,23 +9,23 @@ class MotorControl { 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 setTargetPower(int8_t power); void stop(); void emergencyStop(); void toString(); - int16_t getSpeed(); - int16_t getTargetSpeed(); - bool isTargetSpeedReached(); + int8_t getPower(); + int8_t getTargetPower(); + bool isTargetPowerReached(); bool isAccelerationPositive(); bool isAccelerationNegative(); private: - void setSpeed(int16_t speed); - void accelerate(int8_t acc); + void setRealPower(int8_t power); + void increasePower(int8_t power); - int16_t target_speed = 0; - int16_t speed = 0; + int8_t target_power = 0; + int8_t power = 0; uint8_t direction = 0; // 0 = stop, 1 = forward, 2 = backward uint64_t last_millis = 0; diff --git a/src/moveControl.cpp b/src/moveControl.cpp index 759ec93..b3d9a5d 100644 --- a/src/moveControl.cpp +++ b/src/moveControl.cpp @@ -22,13 +22,16 @@ void MoveControl::runMoveControl() { last_millis = millis(); this->calcWheelSpeed(); - //this->regulateMotorPower(this->left_motor, this->left_speedometer, this->wheelspeed_left_target); - this->regulateMotorPower(this->right_motor, this->right_speedometer, this->wheelspeed_right_target); + this->regulateMotors(); +} +void MoveControl::setDrivingStatus(drivingStatus status) { + this->driving_status = status; } void MoveControl::setSpeed(double speed) { this->x_speed = speed; + Serial.printf("New speed: %f in moveControll.cpp \n", speed); } void MoveControl::setRotationspeed(double speed) { @@ -49,38 +52,75 @@ void MoveControl::calcWheelSpeed() { this->wheelspeed_left_target = (A * this->x_speed + (-B) * this->rotation_speed) * (WHEEL_DIAMETER / 2); } -void MoveControl::regulateMotorPower(MotorControl *motor, Speedometer *cur_speed, double tar_speed) { - if (tar_speed > 0) { - // Direction FORWARD - if (cur_speed->getSpeed() - tar_speed < 0) { - // Too fast - if (motor->isAccelerationPositive() || motor->isTargetSpeedReached()) { - motor->setTargetSpeed(motor->getTargetSpeed() - SPEED_STEPS); - } - } else { - // Too slow - if (motor->isAccelerationNegative() || motor->isTargetSpeedReached()) { - motor->setTargetSpeed(motor->getTargetSpeed() + SPEED_STEPS); - } +void MoveControl::regulateMotors() { + switch (this->driving_status) { + case drivingStatus::stop : + this->left_motor->setTargetPower(0); + this->right_motor->setTargetPower(0); + break; + + case drivingStatus::straightForward : + // Left motor + // Too slow + if (this->left_speedometer->getSpeed() < this->wheelspeed_left_target + && this->left_speedometer->getSpeed() <= this->right_speedometer->getSpeed()) { + + this->left_motor->setTargetPower(this->left_motor->getTargetPower() + ACCELERATE_STEPS); + + // Too fast + } else if (this->left_speedometer->getSpeed() > this->wheelspeed_left_target + && this->left_speedometer->getSpeed() >= this->right_speedometer->getSpeed()) { + + this->left_motor->setTargetPower(this->left_motor->getTargetPower() - ACCELERATE_STEPS); } - } else if (tar_speed < 0) { - // Direction BACKWARD - if (cur_speed->getSpeed() - tar_speed > 0) { - // Too fast - if (motor->isAccelerationPositive() || motor->isTargetSpeedReached()) { - motor->setTargetSpeed(motor->getTargetSpeed() - SPEED_STEPS); - } - } else { - // Too slow - if (motor->isAccelerationNegative() || motor->isTargetSpeedReached()) { - motor->setTargetSpeed(motor->getTargetSpeed() + SPEED_STEPS); - } + // Right motor + // Too slow + if (this->right_speedometer->getSpeed() < this->wheelspeed_right_target + && this->right_speedometer->getSpeed() <= this->left_speedometer->getSpeed()) { + + this->right_motor->setTargetPower(this->right_motor->getTargetPower() + ACCELERATE_STEPS); + + // Too fast + } else if (this->right_speedometer->getSpeed() > this->wheelspeed_right_target + && this->right_speedometer->getSpeed() >= this->left_speedometer->getSpeed()) { + + this->right_motor->setTargetPower(this->right_motor->getTargetPower() - ACCELERATE_STEPS); } + break; + case drivingStatus::straightBackward : + /* code */ + break; - } else { - // Direction STOP - motor->setTargetSpeed(0); + case drivingStatus::arcForwardLeft : + /* code */ + break; + + case drivingStatus::arcForwardRight : + /* code */ + break; + + case drivingStatus::arcBackwardLeft : + /* code */ + break; + + case drivingStatus::arcBackwardRight : + /* code */ + break; + + case drivingStatus::rotateLeft : + /* code */ + break; + + case drivingStatus::rotateRight : + /* code */ + break; + + default: + Serial.println("Wrong drivingState in regulateMotors"); + break; } -} + + +} \ No newline at end of file diff --git a/src/moveControl.h b/src/moveControl.h index 0458cbc..4529097 100644 --- a/src/moveControl.h +++ b/src/moveControl.h @@ -7,24 +7,37 @@ #include "speedometer.h" #include "config.h" +enum drivingStatus {stop, + straightForward, + straightBackward, + arcForwardLeft, + arcForwardRight, + arcBackwardLeft, + arcBackwardRight, + rotateLeft, + rotateRight}; + class MoveControl { public: MoveControl(); void init(MotorControl *left_motor, MotorControl *right_motor, Speedometer *left_encoder, Speedometer *right_encoder); void runMoveControl(); + void setDrivingStatus(drivingStatus status); void setSpeed(double speed); void setRotationspeed(double speed); private: void calcWheelSpeed(); - static void regulateMotorPower(MotorControl *motor, Speedometer *cur_speed, double tar_speed); + void regulateMotors(); MotorControl *left_motor; MotorControl *right_motor; Speedometer *left_speedometer; Speedometer *right_speedometer; + drivingStatus driving_status = drivingStatus::stop; + double x_speed = 0; double rotation_speed = 0; diff --git a/src/speedometer.cpp b/src/speedometer.cpp index bbf2f58..3a0243e 100644 --- a/src/speedometer.cpp +++ b/src/speedometer.cpp @@ -47,9 +47,9 @@ void Speedometer::runSpeedometer() { this->speed = 0; } - Serial.printf("time: %d, ", time); - Serial.printf("count: %d, ",count); - Serial.printf("n: %f, u: %f, ms: %f \n", n, u, ms); + // Serial.printf("time: %d, ", time); + // Serial.printf("count: %d, ",count); + // Serial.printf("n: %f, u: %f, ms: %f \n", n, u, ms); } double Speedometer::getSpeed() {