kleiax/Bachelorarbeit-Rover
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases 1 Wiki Activity

Added PS3 Controller

Browse Source 
New strategy in moveControl
This commit is contained in:
kleiax
2021-04-19 13:08:15 +02:00
parent a33b4a2e4c
commit 89050ae2d3
8 changed files with 221 additions and 140 deletions
Download Patch File Download Diff File Expand all files Collapse all files
platformio.ini
+1 -1
View File
@@ -15,6 +15,6 @@ framework = arduino
monitor_speed = 115200 monitor_speed = 115200
monitor_port = COM12 monitor_port = COM12
lib_deps = lib_deps =
stempedia/DabbleESP32@^1.5.1
madhephaestus/ESP32Encoder@^0.4.0 madhephaestus/ESP32Encoder@^0.4.0
jvpernis/PS3 Controller Host@^1.1.0
upload_port = COM12 upload_port = COM12
src/config.h
+14 -6
View File
@@ -1,15 +1,19 @@
// The front is where the boards are
//Pin config //Pin config
//Left
#define M_DIR_11 27 #define M_DIR_11 27
#define M_DIR_12 12 #define M_DIR_12 12
#define M_PWM_1 13 #define M_PWM_1 13
#define M_ENCODE_1A 33 #define M_ENCODE_1A 33
#define M_ENCODE_1B 32 #define M_ENCODE_1B 32
//Right
#define M_DIR_21 23 #define M_DIR_21 23
#define M_DIR_22 14 #define M_DIR_22 14
#define M_PWM_2 22 #define M_PWM_2 22
#define M_ENCODE_2A 25 #define M_ENCODE_2A 26
#define M_ENCODE_2B 26 #define M_ENCODE_2B 25
//Motors //Motors
#define LEFT_MOTOR 1 #define LEFT_MOTOR 1
@@ -22,12 +26,12 @@
#define BACKWARD 2 #define BACKWARD 2
//MotorControl config //MotorControl config
#define RUN_MOTOR_CONTROL_DELAY 20 // Normal 10 #define RUN_MOTOR_CONTROL_DELAY 10 // Normal 10
#define PWM_FREQ 16000 #define PWM_FREQ 16000
#define PWM_RES 8 #define PWM_RES 8
#define PWM_CHANNEL_M1 0 #define PWM_CHANNEL_M1 0
#define PWM_CHANNEL_M2 1 #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_MIN 120
#define PWM_MAX 245 // Max 98% of 2^PWM_RES #define PWM_MAX 245 // Max 98% of 2^PWM_RES
@@ -36,7 +40,11 @@
#define ENC_FILTER 1023 // 1023 is max #define ENC_FILTER 1023 // 1023 is max
#define ENC_STEPS 1024 #define ENC_STEPS 1024
#define WHEEL_DIAMETER 0.1263 //meter #define WHEEL_DIAMETER 0.1263 //meter
#define BUF_SIZE 40 #define BUF_SIZE 10
//MoveControl config //MoveControl config
#define RUN_MOVE_CONTROL_DELAY 600 // Normal 10 untested #define RUN_MOVE_CONTROL_DELAY 150 // Normal 10 untested
#define ACCELERATE_STEPS 1
// PS3 Controller
// ESP32 MAC BL 24:62:AB:F2:4B:3A
src/main.cpp
+53 -33
View File
@@ -1,15 +1,15 @@
#include <Arduino.h> #include <Arduino.h>
#include <Ps3Controller.h>
#include "config.h" #include "config.h"
#include "motorControl.h" #include "motorControl.h"
#include "moveControl.h" #include "moveControl.h"
#include "speedometer.h" #include "speedometer.h"
#define CUSTOM_SETTINGS void callbackControllerAction();
#define INCLUDE_GAMEPAD_MODULE void callbackControllerConnect();
#include <DabbleESP32.h> void callbackControllerDisconnect();
void controllerPrintBattery();
void gamepadInput();
MotorControl left_motor; MotorControl left_motor;
MotorControl right_motor; MotorControl right_motor;
@@ -18,10 +18,20 @@ Speedometer speedometer_right;
MoveControl moveController; MoveControl moveController;
uint64_t last_millis = 0; uint64_t last_millis = 0;
int controller_battery = -1;
uint8_t controller_led = 1;
// Temp code
double speed = 0;
void setup() { void setup() {
Serial.begin(115200); // make sure your Serial Monitor is also set at this baud rate. 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); 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); right_motor.init(M_PWM_2, PWM_CHANNEL_M2, M_DIR_21, M_DIR_22);
@@ -33,43 +43,53 @@ void setup() {
} }
void loop() { void loop() {
if (millis() - last_millis > 400) { if (millis() - last_millis > 1000) {
gamepadInput(); Serial.printf("Speed L: %f, ", speedometer_left.getSpeed());
// left_motor.toString(); left_motor.toString();
// Serial.printf("speed: %f, ", speedometer_right.getSpeed()); Serial.printf("Speed R: %f, ", speedometer_right.getSpeed());
// right_motor.toString(); right_motor.toString();
// Serial.printf("Speed L: %f, Speed R: %f \n", speedometer_left.getSpeed(), speedometer_right.getSpeed());
last_millis = millis(); last_millis = millis();
} }
// left_motor.runMotorControl(); left_motor.runMotorControl();
right_motor.runMotorControl(); right_motor.runMotorControl();
// speedometer_left.runSpeedometer(); speedometer_left.runSpeedometer();
speedometer_right.runSpeedometer(); speedometer_right.runSpeedometer();
// moveController.runMoveControl(); moveController.runMoveControl();
} }
void gamepadInput() { void callbackControllerAction() {
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. if (Ps3.event.button_down.r3) { controllerPrintBattery(); }
//Serial.print("KeyPressed: "); if (Ps3.event.button_down.l1) { moveController.setSpeed(speed -= 0.1); }
if (Ps3.event.button_down.r1) { moveController.setSpeed(speed += 0.1); }
if (GamePad.isUpPressed()) { if (Ps3.event.button_down.up) { moveController.setDrivingStatus(drivingStatus::straightForward); }
moveController.setSpeed(1.3); if (Ps3.event.button_up.up) { moveController.setDrivingStatus(drivingStatus::stop); }
} else if (GamePad.isDownPressed()) {
moveController.setSpeed(0);
} else {
} }
if (GamePad.isTrianglePressed()) { void callbackControllerConnect() {
right_motor.setTargetSpeed(20); Serial.println("Controller connected to ESP32");
} else if (GamePad.isCrossPressed()) {
right_motor.setTargetSpeed(30); delay(400);
} else if (GamePad.isCirclePressed()){ Serial.print("Setting LEDs to Status "); Serial.println(controller_led, DEC);
right_motor.setTargetSpeed(0); Ps3.setPlayer(controller_led);
} }
// int a = GamePad.getAngle(); void callbackControllerDisconnect() {
// int b = GamePad.getRadius(); Serial.println("Controller disconnected from ESP32");
// Serial.printf("Winkel: %d, Radius: %d \n", a, b); }
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");
} }
src/motorControl.cpp
+44 -44
View File
@@ -28,47 +28,47 @@ void MotorControl::init(uint8_t pwm_pin, uint8_t pwm_channel, uint8_t direction_
void MotorControl::runMotorControl() { void MotorControl::runMotorControl() {
if (millis() - this->last_millis > RUN_MOTOR_CONTROL_DELAY) { if (millis() - this->last_millis > RUN_MOTOR_CONTROL_DELAY) {
// Absolute difference between target_speed and speed // Absolute difference between target_power and speed
uint8_t abs_difference = abs(this->target_speed - this->speed); uint8_t abs_difference = abs(this->target_power - this->power);
// Difference between target_speed and speed // Difference between target_power and speed
int16_t difference = this->target_speed - this->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 // Check that the target speed is close to 0 and that the abs_difference is lower than POWER_STEPS
if (abs(this->target_speed) < SPEED_STEPS && abs_difference < SPEED_STEPS) { if (abs(this->target_power) < POWER_STEPS && abs_difference < POWER_STEPS) {
this->setSpeed(0); this->setRealPower(0);
this->last_millis = millis(); this->last_millis = millis();
return; return;
} }
// Correct speed // Correct speed
if (abs_difference < SPEED_STEPS) { if (abs_difference < POWER_STEPS) {
this->last_millis = millis(); this->last_millis = millis();
return; return;
} }
// Positive or negative tagret speed // Positive or negative tagret speed
if (this->target_speed >= 0) { if (this->target_power >= 0) {
// Positive or negative speed // Positive or negative speed
if (this->speed >= 0) { if (this->power >= 0) {
if (difference > 0) { if (difference > 0) {
this->accelerate(SPEED_STEPS); this->increasePower(POWER_STEPS);
} else { } else {
this->accelerate(-SPEED_STEPS); this->increasePower(-POWER_STEPS);
} }
} else { } else {
this->accelerate(SPEED_STEPS); this->increasePower(POWER_STEPS);
} }
} else { } else {
// Positive or negative speed // Positive or negative speed
if (this->speed >= 0) { if (this->power >= 0) {
this->accelerate(-SPEED_STEPS); this->increasePower(-POWER_STEPS);
} else { } else {
if (difference > 0) { if (difference > 0) {
this->accelerate(SPEED_STEPS); this->increasePower(POWER_STEPS);
} else { } 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 //TODO: Exceptionhandling
if (speed <= 100 && speed >= -100) { if (power <= 100 && power >= -100) {
this->target_speed = speed; this->target_power = power;
} else { } else {
Serial.println("Invalid Argument in MotorControl::setTargetSpeed"); Serial.println("Invalid Argument in MotorControl::setTargetPower");
} }
} }
void MotorControl::stop() { void MotorControl::stop() {
this->target_speed = 0; this->target_power = 0;
} }
void MotorControl::emergencyStop() { void MotorControl::emergencyStop() {
setSpeed(0); setRealPower(0);
} }
void MotorControl::toString() { 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() { int8_t MotorControl::getPower() {
return this->speed; return this->power;
} }
int16_t MotorControl::getTargetSpeed() { int8_t MotorControl::getTargetPower() {
return this->target_speed; return this->target_power;
} }
bool MotorControl::isTargetSpeedReached() { bool MotorControl::isTargetPowerReached() {
if (this->target_speed == this->speed) if (this->target_power == this->power)
return true; return true;
return false; return false;
} }
bool MotorControl::isAccelerationPositive() { bool MotorControl::isAccelerationPositive() {
if (speed < target_speed) if (power < target_power)
return true; return true;
return false; return false;
} }
bool MotorControl::isAccelerationNegative() { bool MotorControl::isAccelerationNegative() {
if (speed > target_speed) if (power > target_power)
return true; return true;
return false; return false;
} }
void MotorControl::setSpeed(int16_t speed) { void MotorControl::setRealPower(int8_t power) {
//TODO: Exceptionhandling //TODO: Exceptionhandling
if (speed <= 100 || speed <= -100) { if (power <= 100 || power <= -100) {
this->speed = speed; this->power = power;
} else { } else {
Serial.println("Invalid Argument in MotorControl::setSpeed"); Serial.println("Invalid Argument in MotorControl::setRealPower");
return; return;
} }
if (speed == 0) { if (power == 0) {
this->direction = 0; this->direction = 0;
digitalWrite(this->direction_pin_1, LOW); digitalWrite(this->direction_pin_1, LOW);
digitalWrite(this->direction_pin_2, LOW); digitalWrite(this->direction_pin_2, LOW);
@@ -140,13 +140,13 @@ void MotorControl::setSpeed(int16_t speed) {
return; 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; this->direction = 2;
digitalWrite(this->direction_pin_1, LOW); digitalWrite(this->direction_pin_1, LOW);
digitalWrite(this->direction_pin_2, HIGH); 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; this->direction = 1;
digitalWrite(this->direction_pin_1, HIGH); digitalWrite(this->direction_pin_1, HIGH);
digitalWrite(this->direction_pin_2, LOW); digitalWrite(this->direction_pin_2, LOW);
@@ -156,13 +156,13 @@ void MotorControl::setSpeed(int16_t speed) {
// Serial.printf("pwm_val: %d, ", pwm_val); // Serial.printf("pwm_val: %d, ", pwm_val);
} }
void MotorControl::accelerate(int8_t acc) { void MotorControl::increasePower(int8_t power) {
//TODO: Exceptionhandling //TODO: Exceptionhandling
//TODO: make a stop befor a direction change //TODO: make a stop befor a direction change
if (abs(acc) > 2 * SPEED_STEPS) { if (abs(power) > 2 * POWER_STEPS) {
Serial.println("Invalid Argument in MotorControl::accelerate"); Serial.println("Invalid Argument in MotorControl::increasePower");
return; return;
} }
this->setSpeed(speed + acc); this->setRealPower(this->power + power);
} }
src/motorControl.h
+8 -8
View File
@@ -9,23 +9,23 @@ class MotorControl {
MotorControl(); MotorControl();
void init(uint8_t pwm_pin, uint8_t pwm_channel, uint8_t direction_1, uint8_t direction_2); void init(uint8_t pwm_pin, uint8_t pwm_channel, uint8_t direction_1, uint8_t direction_2);
void runMotorControl(); void runMotorControl();
void setTargetSpeed(int16_t speed); void setTargetPower(int8_t power);
void stop(); void stop();
void emergencyStop(); void emergencyStop();
void toString(); void toString();
int16_t getSpeed(); int8_t getPower();
int16_t getTargetSpeed(); int8_t getTargetPower();
bool isTargetSpeedReached(); bool isTargetPowerReached();
bool isAccelerationPositive(); bool isAccelerationPositive();
bool isAccelerationNegative(); bool isAccelerationNegative();
private: private:
void setSpeed(int16_t speed); void setRealPower(int8_t power);
void accelerate(int8_t acc); void increasePower(int8_t power);
int16_t target_speed = 0; int8_t target_power = 0;
int16_t speed = 0; int8_t power = 0;
uint8_t direction = 0; // 0 = stop, 1 = forward, 2 = backward uint8_t direction = 0; // 0 = stop, 1 = forward, 2 = backward
uint64_t last_millis = 0; uint64_t last_millis = 0;
src/moveControl.cpp
+67 -27
View File
@@ -22,13 +22,16 @@ void MoveControl::runMoveControl() {
last_millis = millis(); last_millis = millis();
this->calcWheelSpeed(); this->calcWheelSpeed();
//this->regulateMotorPower(this->left_motor, this->left_speedometer, this->wheelspeed_left_target); this->regulateMotors();
this->regulateMotorPower(this->right_motor, this->right_speedometer, this->wheelspeed_right_target); }
void MoveControl::setDrivingStatus(drivingStatus status) {
this->driving_status = status;
} }
void MoveControl::setSpeed(double speed) { void MoveControl::setSpeed(double speed) {
this->x_speed = speed; this->x_speed = speed;
Serial.printf("New speed: %f in moveControll.cpp \n", speed);
} }
void MoveControl::setRotationspeed(double 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); 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) { void MoveControl::regulateMotors() {
if (tar_speed > 0) { switch (this->driving_status) {
// Direction FORWARD case drivingStatus::stop :
if (cur_speed->getSpeed() - tar_speed < 0) { this->left_motor->setTargetPower(0);
// Too fast this->right_motor->setTargetPower(0);
if (motor->isAccelerationPositive() || motor->isTargetSpeedReached()) { break;
motor->setTargetSpeed(motor->getTargetSpeed() - SPEED_STEPS);
} case drivingStatus::straightForward :
} else { // Left motor
// Too slow // Too slow
if (motor->isAccelerationNegative() || motor->isTargetSpeedReached()) { if (this->left_speedometer->getSpeed() < this->wheelspeed_left_target
motor->setTargetSpeed(motor->getTargetSpeed() + SPEED_STEPS); && 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 // Too fast
if (motor->isAccelerationPositive() || motor->isTargetSpeedReached()) { } else if (this->left_speedometer->getSpeed() > this->wheelspeed_left_target
motor->setTargetSpeed(motor->getTargetSpeed() - SPEED_STEPS); && this->left_speedometer->getSpeed() >= this->right_speedometer->getSpeed()) {
this->left_motor->setTargetPower(this->left_motor->getTargetPower() - ACCELERATE_STEPS);
} }
} else {
// Right motor
// Too slow // Too slow
if (motor->isAccelerationNegative() || motor->isTargetSpeedReached()) { if (this->right_speedometer->getSpeed() < this->wheelspeed_right_target
motor->setTargetSpeed(motor->getTargetSpeed() + SPEED_STEPS); && 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;
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;
} }
} else {
// Direction STOP
motor->setTargetSpeed(0);
}
} }
src/moveControl.h
+14 -1
View File
@@ -7,24 +7,37 @@
#include "speedometer.h" #include "speedometer.h"
#include "config.h" #include "config.h"
enum drivingStatus {stop,
straightForward,
straightBackward,
arcForwardLeft,
arcForwardRight,
arcBackwardLeft,
arcBackwardRight,
rotateLeft,
rotateRight};
class MoveControl { class MoveControl {
public: public:
MoveControl(); MoveControl();
void init(MotorControl *left_motor, MotorControl *right_motor, void init(MotorControl *left_motor, MotorControl *right_motor,
Speedometer *left_encoder, Speedometer *right_encoder); Speedometer *left_encoder, Speedometer *right_encoder);
void runMoveControl(); void runMoveControl();
void setDrivingStatus(drivingStatus status);
void setSpeed(double speed); void setSpeed(double speed);
void setRotationspeed(double speed); void setRotationspeed(double speed);
private: private:
void calcWheelSpeed(); void calcWheelSpeed();
static void regulateMotorPower(MotorControl *motor, Speedometer *cur_speed, double tar_speed); void regulateMotors();
MotorControl *left_motor; MotorControl *left_motor;
MotorControl *right_motor; MotorControl *right_motor;
Speedometer *left_speedometer; Speedometer *left_speedometer;
Speedometer *right_speedometer; Speedometer *right_speedometer;
drivingStatus driving_status = drivingStatus::stop;
double x_speed = 0; double x_speed = 0;
double rotation_speed = 0; double rotation_speed = 0;
src/speedometer.cpp
+3 -3
View File
@@ -47,9 +47,9 @@ void Speedometer::runSpeedometer() {
this->speed = 0; this->speed = 0;
} }
Serial.printf("time: %d, ", time); // Serial.printf("time: %d, ", time);
Serial.printf("count: %d, ",count); // Serial.printf("count: %d, ",count);
Serial.printf("n: %f, u: %f, ms: %f \n", n, u, ms); // Serial.printf("n: %f, u: %f, ms: %f \n", n, u, ms);
} }
double Speedometer::getSpeed() { double Speedometer::getSpeed() {