From bad84b1ef3c8f0c64378f7548a980bcf3b1501b4 Mon Sep 17 00:00:00 2001 From: Alexander Klein Date: Sun, 13 Aug 2023 17:04:53 +0200 Subject: [PATCH] refactore and fixes first route successfully absolved --- include/Version.h | 4 +- .../driveModi/Autopilot/menuAutopilot.cpp | 18 ++++--- src/driveModi/Modi/Autopilot/autopilot.cpp | 50 +++++++++++++++---- src/driveModi/Modi/Autopilot/autopilot.h | 4 +- version | 2 +- 5 files changed, 57 insertions(+), 21 deletions(-) diff --git a/include/Version.h b/include/Version.h index 5ed91a8..109f133 100644 --- a/include/Version.h +++ b/include/Version.h @@ -1,9 +1,9 @@ // AUTO GENERATED FILE, DO NOT EDIT #ifndef VERSION - #define VERSION "0.8.28" + #define VERSION "0.8.36" #endif #ifndef BUILD_TIMESTAMP - #define BUILD_TIMESTAMP "2023-08-09 19:28:27.390516" + #define BUILD_TIMESTAMP "2023-08-13 15:13:34.491991" #endif \ No newline at end of file diff --git a/src/SpecialMenus/driveModi/Autopilot/menuAutopilot.cpp b/src/SpecialMenus/driveModi/Autopilot/menuAutopilot.cpp index 269b195..c7f4f1f 100644 --- a/src/SpecialMenus/driveModi/Autopilot/menuAutopilot.cpp +++ b/src/SpecialMenus/driveModi/Autopilot/menuAutopilot.cpp @@ -66,6 +66,10 @@ void MenuAutopilot::printPage() const { lineTwo = "Autopilot active"; break; + case Autopilot::State::SelfDrivingRotate : + lineTwo = "Rotating"; + break; + case Autopilot::State::TargetReached : lineTwo = "Target reached"; break; @@ -204,17 +208,17 @@ void MenuAutopilot::printPage() const { case 13: lineOne = "Test rotate"; - lineTwo = "45 degree"; + lineTwo = "270 degree"; break; case 14: lineOne = "Test rotate"; - lineTwo = "15 degree"; + lineTwo = "45 degree"; break; case 15: lineOne = "Test rotate"; - lineTwo = "5 degree"; + lineTwo = "20 degree"; break; default: @@ -257,19 +261,19 @@ void MenuAutopilot::runCommand() const { break; case 12: - this->autopilot->rotate(180); + this->autopilot->testRotate(180); break; case 13: - this->autopilot->rotate(45); + this->autopilot->testRotate(270); break; case 14: - this->autopilot->rotate(15); + this->autopilot->testRotate(45); break; case 15: - this->autopilot->rotate(5); + this->autopilot->testRotate(20); break; default: diff --git a/src/driveModi/Modi/Autopilot/autopilot.cpp b/src/driveModi/Modi/Autopilot/autopilot.cpp index c36f362..004138f 100644 --- a/src/driveModi/Modi/Autopilot/autopilot.cpp +++ b/src/driveModi/Modi/Autopilot/autopilot.cpp @@ -116,9 +116,12 @@ bool Autopilot::shouldUpdate() { return false; } -void Autopilot::rotate(int16_t degree) { +void Autopilot::testRotate(int16_t degree) { + if (!degree) + return; + this->courseCorrection.correction = degree; - this->rotate(); + this->beginRotate(); } void Autopilot::init() { @@ -144,26 +147,50 @@ void Autopilot::drive() { this,moveControl->setSpeed(0); } -void Autopilot::rotate() { +void Autopilot::beginRotate() { if (this->state != State::SelfDrivingRotate) { this->lastState = this->state; this->state = State::SelfDrivingRotate; this->rotationAimAzimuth = this->navigation->getAzimuth() + this->courseCorrection.correction; + this->rotationAimAzimuth = Navigation::fixDegree(this->rotationAimAzimuth); this->moveControl->setSpeed(0); if (this->courseCorrection.correction > 0) this->moveControl->setRotationSpeed(-this->rotationSpeed); else this->moveControl->setRotationSpeed(this->rotationSpeed); - } else { - if (abs(this->navigation->getAzimuth() - this->rotationAimAzimuth) < 3) { - this->state = this->lastState; - this->navigation->drivingDirectionChange(); - this->moveControl->setRotationSpeed(0); - } } } +void Autopilot::rotate() { + if (abs(this->navigation->getAzimuth() - this->rotationAimAzimuth) < 5) { + this->endRotate(); + return; + } + + if ((abs(this->navigation->getAzimuth() - this->rotationAimAzimuth) < 15) + && + ((this->courseCorrection.correction > 0 + && this->rotationAimAzimuth < this->navigation->getAzimuth()) + || (this->courseCorrection.correction < 0 + && this->rotationAimAzimuth > this->navigation->getAzimuth()))) + { + this->endRotate(); + std::cout << "Autopilot::rotate: Rover rotated too far" << std::endl; + } +} + +void Autopilot::endRotate() { + if (this->state != State::SelfDrivingRotate) + return; + + this->state = this->lastState; + this->navigation->drivingDirectionChange(); + this->moveControl->setRotationSpeed(0); + this->moveControl->emergencyStop(); + this->moveControl->setDrivingStatus(MoveControl::Status::Drive); +} + void Autopilot::checkButtonInput() { if (ControlPadButton::isControlPadButtonPressed(this->input, ControlPadButton::PadButton::Action) && millis() - this->lastAutopilotChangeMillis > this->autopilotChangeDelayMillis) { @@ -211,8 +238,11 @@ void Autopilot::askNavigationForOrder() { } void Autopilot::selfDriving() { + if (this->state != State::SelfDriving) + return; + if (abs(this->courseCorrection.correction) >= this->maxCourseDeviationBerforeAct) - this->rotate(); + this->beginRotate(); else this->drive(); } diff --git a/src/driveModi/Modi/Autopilot/autopilot.h b/src/driveModi/Modi/Autopilot/autopilot.h index d7bc259..2fd53c5 100644 --- a/src/driveModi/Modi/Autopilot/autopilot.h +++ b/src/driveModi/Modi/Autopilot/autopilot.h @@ -107,11 +107,13 @@ class Autopilot : public ManualControl { * @return false */ bool shouldUpdate(); - void rotate(int16_t degree); + void testRotate(int16_t degree); + void endRotate(); private: void init(); void drive(); + void beginRotate(); void rotate(); void runAutopilot(); void checkButtonInput(); diff --git a/version b/version index 09318a8..98bb028 100644 --- a/version +++ b/version @@ -1 +1 @@ -0.8.28 \ No newline at end of file +0.8.36 \ No newline at end of file