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

Added PS3 Controller

Browse Source 
New strategy in moveControl
This commit is contained in:
kleiax
2021-04-19 13:08:15 +02:00
parent a33b4a2e4c
commit 89050ae2d3
8 changed files with 221 additions and 140 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/moveControl.cpp
+71 -31
View File
@@ -22,13 +22,16 @@ void MoveControl::runMoveControl() {
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);
this->regulateMotors();
}
void MoveControl::setDrivingStatus(drivingStatus status) {
this->driving_status = status;
}
void MoveControl::setSpeed(double speed) {
this->x_speed = speed;
Serial.printf("New speed: %f in moveControll.cpp \n", speed);
}
void MoveControl::setRotationspeed(double speed) {
@@ -49,38 +52,75 @@ void MoveControl::calcWheelSpeed() {
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);
}
void MoveControl::regulateMotors() {
switch (this->driving_status) {
case drivingStatus::stop :
this->left_motor->setTargetPower(0);
this->right_motor->setTargetPower(0);
break;
case drivingStatus::straightForward :
// 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);
}
} 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);
}
// 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::straightBackward :
/* code */
break;
} else {
// Direction STOP
motor->setTargetSpeed(0);
case drivingStatus::arcForwardLeft :
/* code */
break;
case drivingStatus::arcForwardRight :
/* code */
break;
case drivingStatus::arcBackwardLeft :
/* code */
break;
case drivingStatus::arcBackwardRight :
/* code */
break;
case drivingStatus::rotateLeft :
/* code */
break;
case drivingStatus::rotateRight :
/* code */
break;
default:
Serial.println("Wrong drivingState in regulateMotors");
break;
}
}
}