LineTracker: get_turn_direction_preference: turn mower virtually by 180

degrees if FREEWHEEL is backside
Ardumower · Apr 10, 2023 · 7ef3b9c · 7ef3b9c
1 parent ee155b7
commit 7ef3b9c
Showing 1 changed file with 10 additions and 5 deletions.
diff --git a/sunray/LineTracker.cpp b/sunray/LineTracker.cpp
@@ -39,6 +39,11 @@ int get_turn_direction_preference() {
   float center_x = stateX;
   float center_y = stateY;
   float r = 0.3;
+  float cur_angle = stateDelta;
+
+  if (FREEWHEEL_IS_AT_BACKSIDE) {
+	  cur_angle = scalePI(stateDelta + PI);
+  }
 
   // create circle / octagon around center angle 0 - "360"
   circle.points[0].setXY(center_x + cos(deg2rad(0)) * r, center_y + sin(deg2rad(0)) * r);
@@ -56,7 +61,7 @@ int get_turn_direction_preference() {
   // CONSOLE.print("/");
   // CONSOLE.print(stateY);
   // CONSOLE.print(" stateDelta: ");
-  // CONSOLE.print(stateDelta);
+  // CONSOLE.print(cur_angle);
   // CONSOLE.print(" targetDelta: ");
   // CONSOLE.println(targetDelta);
   int right = 0;
@@ -73,18 +78,18 @@ int get_turn_direction_preference() {
     if (maps.checkpoint(circle.points[i].x(), circle.points[i].y())) {
 
             // skip points in front of us
-            if (fabs(angle-stateDelta) < 0.05) {
+            if (fabs(angle-cur_angle) < 0.05) {
                     continue;
             }
 
-            if (stateDelta < targetDelta) {
-                if (angle >= stateDelta && angle <= targetDelta) {
+            if (cur_angle < targetDelta) {
+                if (angle >= cur_angle && angle <= targetDelta) {
                     left++;
                 } else {
                     right++;
                 }
             } else {
-                   if (angle <= stateDelta && angle >= targetDelta) {
+                   if (angle <= cur_angle && angle >= targetDelta) {
                     right++;
                 } else {
                     left++;