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_port = COM12
lib_deps =
stempedia/DabbleESP32@^1.5.1
madhephaestus/ESP32Encoder@^0.4.0
jvpernis/PS3 Controller Host@^1.1.0
upload_port = COM12
src/config.h
+14 -6
View File
@@ -1,15 +1,19 @@
// The front is where the boards are
//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 25
#define M_ENCODE_2B 26
#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
#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 <Ps3Controller.h>
#include "config.h"
#include "motorControl.h"
#include "moveControl.h"
#include "speedometer.h"
#define CUSTOM_SETTINGS
#define INCLUDE_GAMEPAD_MODULE
#include <DabbleESP32.h>
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 {
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); }
}
if (GamePad.isTrianglePressed()) {
right_motor.setTargetSpeed(20);
} else if (GamePad.isCrossPressed()) {
right_motor.setTargetSpeed(30);
} else if (GamePad.isCirclePressed()){
right_motor.setTargetSpeed(0);
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);
}
// int a = GamePad.getAngle();
// int b = GamePad.getRadius();
// Serial.printf("Winkel: %d, Radius: %d \n", a, b);
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");
}
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() {
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);
}
src/motorControl.h
+8 -8
View File
@@ -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;
src/moveControl.cpp
+67 -27
View File
@@ -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 {
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 (motor->isAccelerationNegative() || motor->isTargetSpeedReached()) {
motor->setTargetSpeed(motor->getTargetSpeed() + SPEED_STEPS);
}
}
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 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 {
// Right motor
// Too slow
if (motor->isAccelerationNegative() || motor->isTargetSpeedReached()) {
motor->setTargetSpeed(motor->getTargetSpeed() + SPEED_STEPS);
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;
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 "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;
src/speedometer.cpp
+3 -3
View File
@@ -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() {