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

improved moveControl

Browse Source
This commit is contained in:
kleiax
2021-04-01 20:59:51 +02:00
parent d9e64fbc4a
commit a33b4a2e4c
7 changed files with 136 additions and 69 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main.cpp
+11 -57
View File
@@ -10,7 +10,6 @@
#include <DabbleESP32.h>
void gamepadInput();
int8_t modSpeed(int8_t dir, int8_t speed);
MotorControl left_motor;
MotorControl right_motor;
@@ -34,88 +33,43 @@ void setup() {
}
void loop() {
if (millis() - last_millis > 700) {
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());
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: ");
static int8_t speed_left = 0;
if (GamePad.isUpPressed()) {
speed_left = modSpeed(1, speed_left);
moveController.setSpeed(1.3);
} else if (GamePad.isDownPressed()) {
speed_left = modSpeed(-1, speed_left);
moveController.setSpeed(0);
} else {
speed_left = modSpeed(0, speed_left);
}
static int8_t speed_right = 0;
if (GamePad.isTrianglePressed()) {
speed_right = modSpeed(1, speed_right);
right_motor.setTargetSpeed(20);
} else if (GamePad.isCrossPressed()) {
speed_right = modSpeed(-1, speed_right);
} else {
speed_right = modSpeed(0, speed_right);
right_motor.setTargetSpeed(30);
} else if (GamePad.isCirclePressed()){
right_motor.setTargetSpeed(0);
}
left_motor.setTargetSpeed(speed_left);
right_motor.setTargetSpeed(speed_right);
// int a = GamePad.getAngle();
// int b = GamePad.getRadius();
// Serial.printf("Winkel: %d, Radius: %d \n", a, b);
}
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;
}
src/motorControl.cpp
+19 -4
View File
@@ -78,7 +78,7 @@ void MotorControl::runMotorControl() {
void MotorControl::setTargetSpeed(int16_t speed) {
//TODO: Exceptionhandling
if (speed <= 100 || speed <= -100) {
if (speed <= 100 && speed >= -100) {
this->target_speed = speed;
} else {
Serial.println("Invalid Argument in MotorControl::setTargetSpeed");
@@ -101,10 +101,25 @@ int16_t MotorControl::getSpeed() {
return this->speed;
}
bool MotorControl::isTargetSpeedReached() {
if (this->target_speed == this->speed) {
return true;
int16_t MotorControl::getTargetSpeed() {
return this->target_speed;
}
bool MotorControl::isTargetSpeedReached() {
if (this->target_speed == this->speed)
return true;
return false;
}
bool MotorControl::isAccelerationPositive() {
if (speed < target_speed)
return true;
return false;
}
bool MotorControl::isAccelerationNegative() {
if (speed > target_speed)
return true;
return false;
}
src/motorControl.h
+3
View File
@@ -15,7 +15,10 @@ class MotorControl {
void toString();
int16_t getSpeed();
int16_t getTargetSpeed();
bool isTargetSpeedReached();
bool isAccelerationPositive();
bool isAccelerationNegative();
private:
void setSpeed(int16_t speed);
src/moveControl.cpp
+61 -2
View File
@@ -15,13 +15,72 @@ void MoveControl::init(MotorControl *left_motor, MotorControl *right_motor,
}
void MoveControl::runMoveControl() {
static uint64_t last_millis = 0;
if (millis() - last_millis < RUN_MOVE_CONTROL_DELAY) {
return;
}
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);
}
void MoveControl::setSpeed(int8_t speed) {
this->speed = speed;
void MoveControl::setSpeed(double speed) {
this->x_speed = speed;
}
void MoveControl::setRotationspeed(double speed) {
this->rotation_speed = speed;
}
void MoveControl::calcWheelSpeed() {
// original formula:
// (1 / r) / 1 b \ / x \ = / Xl \
// \ 1 -b / \ T / \ Xr /
// (1 / r) * 1
const static double A = 15.82278481;
// (1 / r) * b
const static double B = 2.096518987;
this->wheelspeed_right_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) {
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);
}
}
} 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);
}
}
} else {
// Direction STOP
motor->setTargetSpeed(0);
}
}
src/moveControl.h
+9 -2
View File
@@ -5,6 +5,7 @@
#include "motorControl.h"
#include "speedometer.h"
#include "config.h"
class MoveControl {
public:
@@ -12,17 +13,23 @@ class MoveControl {
void init(MotorControl *left_motor, MotorControl *right_motor,
Speedometer *left_encoder, Speedometer *right_encoder);
void runMoveControl();
void setSpeed(int8_t speed);
void setSpeed(double speed);
void setRotationspeed(double speed);
private:
void calcWheelSpeed();
static void regulateMotorPower(MotorControl *motor, Speedometer *cur_speed, double tar_speed);
MotorControl *left_motor;
MotorControl *right_motor;
Speedometer *left_speedometer;
Speedometer *right_speedometer;
int8_t speed = 0;
double x_speed = 0;
double rotation_speed = 0;
double wheelspeed_left_target = 0;
double wheelspeed_right_target = 0;
};
#endif // MOVE_CONTROL_H
src/speedometer.cpp
+27 -4
View File
@@ -1,7 +1,9 @@
#include "speedometer.h"
Speedometer::Speedometer() {
for (int i = 0; i < BUF_SIZE; i++) {
this->buf[i] = 0;
}
}
void Speedometer::init(uint8_t pinA, uint8_t pinB) {
@@ -21,7 +23,8 @@ void Speedometer::runSpeedometer() {
uint16_t elapsed_time = time - this->last_millis;
this->last_millis = time;
int64_t count = encoder.getCount();
int16_t count = encoder.getCount();
this->addValToBuf(count);
encoder.clearCount();
uint8_t direction = STOP;
@@ -31,7 +34,7 @@ void Speedometer::runSpeedometer() {
direction = BACKWARD;
}
count = abs(count); // Absolute time in milliseconds
count = abs(this->getAverage()); // Absolute time in milliseconds
double n = (double)count / ENC_STEPS; // Wheel revolutions in absolute time
double u = (double)n / ((double)elapsed_time / 1000); // Wheel revolutions per second
double ms = u * (WHEEL_DIAMETER * PI); // Speed in m/s
@@ -44,7 +47,9 @@ void Speedometer::runSpeedometer() {
this->speed = 0;
}
//Serial.printf("time: %d, count: %d, n: %f, u: %f, ms: %f \n", time, count, 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() {
@@ -60,3 +65,21 @@ uint8_t Speedometer::getDirection() {
return STOP;
}
}
void Speedometer::addValToBuf(int16_t val) {
this->buf[this->bufPos] = val;
this->bufPos++;
if (this->bufPos == BUF_SIZE) {
this->bufPos = 0;
}
}
int16_t Speedometer::getAverage() {
int16_t sum = 0;
for (int i = 0; i < BUF_SIZE; i++) {
sum += this->buf[i];
}
return sum / BUF_SIZE;
}
src/speedometer.h
+6
View File
@@ -17,10 +17,16 @@ class Speedometer {
private:
void addValToBuf(int16_t val);
int16_t getAverage();
ESP32Encoder encoder;
double speed = 0;
uint32_t last_millis = 0;
uint8_t bufPos = 0;
int16_t buf[BUF_SIZE];
};
#endif // SPEEDOMETER_H