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

implement drivingStatus in moveControl

Browse Source
This commit is contained in:
kleiax
2021-04-21 10:04:19 +02:00
parent 89050ae2d3
commit 602e586bae
2 changed files with 207 additions and 12 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/moveControl.cpp
+202 -12
View File
@@ -90,35 +90,225 @@ void MoveControl::regulateMotors() {
break;
case drivingStatus::straightBackward :
/* code */
// 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::arcForwardLeft :
/* code */
case drivingStatus::arcForwardLeft : // Identical with arcForwardRight
double 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::arcForwardRight :
/* code */
case drivingStatus::arcForwardRight : // Identical with arcForwardLeft
double 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::arcBackwardLeft :
/* code */
case drivingStatus::arcBackwardLeft : // Identical with arcBackwardRight
double 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::arcBackwardRight :
/* code */
case drivingStatus::arcBackwardRight : // Identical with arcBackwardLeft
double 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 :
/* code */
// 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 :
/* code */
// 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;
default:
Serial.println("Wrong drivingState in regulateMotors");
Serial.println("Wrong drivingState in MoveControl::regulateMotors");
break;
}