Use forward parking macro

diff --git a/api/psp.h b/api/psp.h
index b6049845a6af2cd9b3a40cf22b067d8c4c6ff035..2294a5b3f4175539062ebee59cd01b86ecc634f4 100644 (file)
--- a/api/psp.h
+++ b/api/psp.h
@@ -7,6 +7,8 @@
 #include "bcar.h"
 #include "pslot.h"
 
+#define FORWARD_PARKING 1
+
 /*! \brief Parking Slot Planner basic class.
 
 \param cc Current bicycle car.

diff --git a/src/psp.cc b/src/psp.cc
index f69e1f2da348f60c099741f4b95a8ceefab21ce5..cc11af7388cb052e0c7205afc826cdc8ef4ffcb1 100644 (file)
--- a/src/psp.cc
+++ b/src/psp.cc
@@ -21,20 +21,11 @@ bool PSPlanner::collide()
 
 bool PSPlanner::forward()
 {
-        if (this->ps().parallel())
-                return false;
-        else
-                return true;
-        double heading = atan2(
-                this->ps().y2() - this->ps().y1(),
-                this->ps().x2() - this->ps().x1()
-        );
-        while (heading < 0) heading += 2 * M_PI;
-        double h = this->gc().h();
-        while (h < 0) h += 2 * M_PI;
-        if (std::abs(heading - h) < M_PI / 4)
-                return true;
+#if FORWARD_PARKING > 0
+        return true;
+#else
         return false;
+#endif
 }
 
 void PSPlanner::gc_to_4()
@@ -102,8 +93,8 @@ void PSPlanner::guess_gc()
                 x += (this->gc().dr() + 0.01) * cos(h);
                 y += (this->gc().w() / 2 + 0.01) * sin(h + dts);
                 y += (this->gc().dr() + 0.01) * sin(h);
-#if 0
         } else {
+#if FORWARD_PARKING > 0
                 // Forward parking
                 double entry_width = edist(
                         this->ps().x1(), this->ps().y1(),
@@ -118,8 +109,7 @@ void PSPlanner::guess_gc()
                 while (h < 0) h += 2 * M_PI;
                 x += this->gc().dr() * cos(h + M_PI);
                 y += this->gc().dr() * sin(h + M_PI);
-#endif
-        } else {
+#else
                 // Backward parking
                 double entry_width = edist(
                         this->ps().x1(), this->ps().y1(),
@@ -134,6 +124,7 @@ void PSPlanner::guess_gc()
                 while (h < 0) h += 2 * M_PI;
                 x += this->gc().df() * cos(h + M_PI);
                 y += this->gc().df() * sin(h + M_PI);
+#endif
         }
         while (h > M_PI)
                 h -= 2 * M_PI;