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

fix speedometer for new encoder

Browse Source
This commit is contained in:
kleiax
2023-04-20 12:31:48 +02:00
parent 3fab7c7f83
commit 07f09d691f
4 changed files with 91 additions and 47 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/moveControl.cpp
+35 -6
View File
@@ -112,18 +112,32 @@ void MoveControl::setDrivingStatus(DrivingStatus status) {
// }
}
void MoveControl::emergencyStop() {
this->left_motor->emergencyStop();
this->right_motor->emergencyStop();
this->setSpeed(0);
this->setRotationSpeed(0);
this->setRawPowerLeft(0);
this->setRawPowerRight(0);
this->driving_status = DrivingStatus::stop;
}
void MoveControl::setSpeed(double speed) {
if (speed < 0.1 && speed > -0.1)
if (speed < 0.1 && speed > -0.1) {
this->x_speed = 0;
else
this->x_speed = speed;
return;
}
this->x_speed = speed;
}
void MoveControl::setRotationSpeed(double speed) {
if (speed < 0.1 && speed > -0.1)
if (speed < 0.1 && speed > -0.1){
this->rotation_speed = 0;
else
this->rotation_speed = speed;
return;
}
this->rotation_speed = speed;
}
void MoveControl::setRawPowerLeft(int16_t power) {
@@ -155,6 +169,15 @@ PID* MoveControl::getPID(uint8_t side) {
return nullptr;
}
void MoveControl::setSpeedometerDirection(Speedometer *speedometer, double value) {
if (value > 0)
speedometer->setDirection(Speedometer::Direction::Forward);
else if (value < 0)
speedometer->setDirection(Speedometer::Direction::Backward);
else
speedometer->setDirection(Speedometer::Direction::None);
}
void MoveControl::calcTargetWheelSpeed() {
/* original formula:
(1 / r) / 1 b \ / x \ = / Xl \
@@ -174,16 +197,22 @@ void MoveControl::regulateMotors() {
case DrivingStatus::stop :
this->left_motor->setTargetPower(0);
this->right_motor->setTargetPower(0);
this->setSpeedometerDirection(this->left_speedometer, 0);
this->setSpeedometerDirection(this->right_speedometer, 0);
break;
case DrivingStatus::drive :
this->left_motor->setTargetPower( (int8_t) this->left_pid_out);
this->right_motor->setTargetPower( (int8_t) this->right_pid_out);
this->setSpeedometerDirection(this->left_speedometer, this->left_pid_out);
this->setSpeedometerDirection(this->right_speedometer, this->right_pid_out);
break;
case DrivingStatus::raw :
this->left_motor->setTargetPower(this->rawPowerLeft);
this->right_motor->setTargetPower(this->rawPowerRight);
this->setSpeedometerDirection(this->left_speedometer, this->rawPowerLeft);
this->setSpeedometerDirection(this->right_speedometer, this->rawPowerRight);
break;
default: