From 602e586bae49c94ddcb63f85d59bb75d6544e808 Mon Sep 17 00:00:00 2001 From: Alexander Klein Date: Wed, 21 Apr 2021 10:04:19 +0200 Subject: [PATCH] implement drivingStatus in moveControl --- src/moveControl.cpp | 214 +++++++++++++++++++++++++++++++++++++++++--- src/moveControl.h | 5 ++ 2 files changed, 207 insertions(+), 12 deletions(-) diff --git a/src/moveControl.cpp b/src/moveControl.cpp index b3d9a5d..acdaae8 100644 --- a/src/moveControl.cpp +++ b/src/moveControl.cpp @@ -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; } diff --git a/src/moveControl.h b/src/moveControl.h index 4529097..c9dea6e 100644 --- a/src/moveControl.h +++ b/src/moveControl.h @@ -17,6 +17,11 @@ enum drivingStatus {stop, rotateLeft, rotateRight}; +enum regulateStatus {stop, + drive, + rotateLeft, + rotateRight}; + class MoveControl { public: MoveControl();