refactore and fixes

first route successfully absolved

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:

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();
 }

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();