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; break;
case drivingStatus::straightBackward : 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; break;
case drivingStatus::arcForwardLeft : case drivingStatus::arcForwardLeft : // Identical with arcForwardRight
/* code */ 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; break;
case drivingStatus::arcForwardRight : case drivingStatus::arcForwardRight : // Identical with arcForwardLeft
/* code */ 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; break;
case drivingStatus::arcBackwardLeft : case drivingStatus::arcBackwardLeft : // Identical with arcBackwardRight
/* code */ 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; break;
case drivingStatus::arcBackwardRight : case drivingStatus::arcBackwardRight : // Identical with arcBackwardLeft
/* code */ 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; break;
case drivingStatus::rotateLeft : 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; break;
case drivingStatus::rotateRight : 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; break;
default: default:
Serial.println("Wrong drivingState in regulateMotors"); Serial.println("Wrong drivingState in MoveControl::regulateMotors");
break; break;
} }
src/moveControl.h
+5
View File
@@ -17,6 +17,11 @@ enum drivingStatus {stop,
rotateLeft, rotateLeft,
rotateRight}; rotateRight};
enum regulateStatus {stop,
drive,
rotateLeft,
rotateRight};
class MoveControl { class MoveControl {
public: public:
MoveControl(); MoveControl();