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

Added consolControl & PID

Browse Source
This commit is contained in:
kleiax
2021-07-26 20:19:33 +02:00
parent 70f6566365
commit 084c046e8c
9 changed files with 202 additions and 153 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/moveControl.cpp
+46 -127
View File
@@ -4,6 +4,24 @@
MoveControl::MoveControl() {
this->debug = new DebugMqtt("MoveControl");
this->left_pid = new PID( &this->wheelspeed_left,
&this->left_pid_out,
&this->wheelspeed_left_target,
PID_LEFT_P, PID_LEFT_I, PID_LEFT_D,
DIRECT);
this->right_pid = new PID( &this->wheelspeed_right,
&this->left_pid_out,
&this->wheelspeed_right_target,
PID_RIGHT_P, PID_RIGHT_I, PID_RIGHT_D,
DIRECT);
this->left_pid->SetOutputLimits(PID_OUT_MIN, PID_OUT_MAX);
this->left_pid->SetSampleTime(PID_SAMPLETIME);
this->left_pid->SetMode(AUTOMATIC);
this->right_pid->SetOutputLimits(PID_OUT_MIN, PID_OUT_MAX);
this->right_pid->SetSampleTime(PID_SAMPLETIME);
this->right_pid->SetMode(AUTOMATIC);
}
void MoveControl::init(MotorControl *left_motor, MotorControl *right_motor,
@@ -17,18 +35,24 @@ void MoveControl::init(MotorControl *left_motor, MotorControl *right_motor,
}
void MoveControl::runMoveControl() {
this->right_pid->Compute();
this->left_pid->Compute();
static uint64_t last_millis = 0;
if (millis() - last_millis < RUN_MOVE_CONTROL_DELAY) {
return;
}
last_millis = millis();
this->updateWheelSpeed();
this->calcWheelSpeed();
this->regulateMotors();
char str[64];
sprintf(str, "x_speed: %3.2f, rotation_speed: %3.2f, tar_left: %3.2f, tar_right: %3.2f", this->x_speed, this->rotation_speed, this->wheelspeed_left_target, this->wheelspeed_right_target);
debug->sendMsg(Loglevel::debug, str);
// char str[128];
// sprintf(str, "x_speed: %3.2f, rotation_speed: %3.2f, tar_left: %3.2f, tar_right: %3.2f",
// this->x_speed, this->rotation_speed, this->wheelspeed_left_target,
// this->wheelspeed_right_target);
// debug->sendMsg(Loglevel::debug, str);
}
void MoveControl::setDrivingStatus(DrivingStatus status) {
@@ -37,13 +61,23 @@ void MoveControl::setDrivingStatus(DrivingStatus status) {
void MoveControl::setSpeed(double speed) {
this->x_speed = speed;
Serial.printf("New speed: %f in moveControll.cpp \n", speed);
// Serial.printf("New speed: %f in moveControll.cpp \n", speed);
}
void MoveControl::setRotationspeed(double speed) {
this->rotation_speed = speed;
}
PID* MoveControl::getPID(uint8_t i) {
if (i == 0) {
return this->left_pid;
} else if (i == 1) {
return this->right_pid;
} else {
return nullptr;
}
}
void MoveControl::calcWheelSpeed() {
/* original formula:
(1 / r) / 1 b \ / x \ = / Xl \
@@ -68,134 +102,19 @@ void MoveControl::regulateMotors() {
this->right_motor->setTargetPower(0);
break;
case DrivingStatus::forward :
ratio = this->wheelspeed_left_target / this->wheelspeed_right_target;
// Left motor
// Too slow
if (this->left_speedometer->getSpeed() < this->wheelspeed_left_target
&& this->left_speedometer->getSpeed() <= (this->right_speedometer->getSpeed() * ratio)) {
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() * ratio)) {
this->left_motor->setTargetPower(this->left_motor->getTargetPower() - ACCELERATE_STEPS);
}
// Right motor
// Too slow
if (this->right_speedometer->getSpeed() < this->wheelspeed_right_target
&& (this->right_speedometer->getSpeed() * ratio) <= 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() * ratio) >= this->left_speedometer->getSpeed()) {
this->right_motor->setTargetPower(this->right_motor->getTargetPower() - ACCELERATE_STEPS);
}
break;
case DrivingStatus::backward :
ratio = this->wheelspeed_left_target / this->wheelspeed_right_target;
// Left motor
// Too slow
if (this->left_speedometer->getSpeed() > this->wheelspeed_left_target
&& this->left_speedometer->getSpeed() >= (this->right_speedometer->getSpeed() * ratio)) {
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() * ratio)) {
this->left_motor->setTargetPower(this->left_motor->getTargetPower() + ACCELERATE_STEPS);
}
// Right motor
// Too slow
if (this->right_speedometer->getSpeed() > this->wheelspeed_right_target
&& (this->right_speedometer->getSpeed() * ratio) >= 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() * ratio) <= this->left_speedometer->getSpeed()) {
this->right_motor->setTargetPower(this->right_motor->getTargetPower() + ACCELERATE_STEPS);
}
break;
case DrivingStatus::rotateLeft :
// 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);
}
// 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::rotateRight :
// 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);
}
// 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);
}
case DrivingStatus::drive :
this->left_motor->setTargetPower( (int8_t) this->left_pid_out);
this->right_motor->setTargetPower( (int8_t) this->right_pid_out);
break;
default:
Serial.println("Wrong drivingState in MoveControl::regulateMotors");
break;
}
}
void MoveControl::updateWheelSpeed() {
this->wheelspeed_left = this->left_speedometer->getSpeed();
this->wheelspeed_right = this->right_speedometer->getSpeed();
}