refactore and fixes

first route successfully absolved

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