Add set goal car position method

Set goal car position near 4th slot border coordinate.

diff --git a/api/psp.h b/api/psp.h
index 2520dcb2ea0875dd325b6b6122e50264c08999cc..4c71d3e455ab53adc142526ba25c6ef9508a3d0f 100644 (file)
--- a/api/psp.h
+++ b/api/psp.h
@@ -19,11 +19,14 @@ class PSPlanner {
                 BicycleCar gc_;
                 ParkingSlot ps_;
 
-                // find entry to slot by reverse approach
+                // find entry to slot
                 void fe_parallel();
                 void fe_perpendicular();
+                // find entry to slot by reverse approach
                 void fer_parallel();
                 void fer_perpendicular();
+                // set goal car
+                void gc_to_4();
         public:
                 /*! \brief Return `true` if there is collision.
 

diff --git a/src/psp.cc b/src/psp.cc
index 7d00cc6ad9a0d142e905d45c00ddabb717f7f760..5992e3ec54a55599abd5232ac7d803bf5c82eafc 100644 (file)
--- a/src/psp.cc
+++ b/src/psp.cc
@@ -35,6 +35,26 @@ bool PSPlanner::forward()
         return false;
 }
 
+void PSPlanner::gc_to_4()
+{
+        double angl_slot = atan2(
+                this->ps().y3() - this->ps().y4(),
+                this->ps().x3() - this->ps().x4()
+        );
+        double angl_delta = M_PI / 2;
+        if (this->ps().right())
+                angl_delta = -M_PI / 2;
+        double x = this->ps().x4();
+        double y = this->ps().y4();
+        x += (this->gc().dr() + 0.01) * cos(angl_slot);
+        y += (this->gc().dr() + 0.01) * sin(angl_slot);
+        x += (this->gc().w() / 2 + 0.01) * cos(angl_slot + angl_delta);
+        y += (this->gc().w() / 2 + 0.01) * sin(angl_slot + angl_delta);
+        this->gc().x(x);
+        this->gc().y(y);
+        this->gc().h(angl_slot);
+}
+
 void PSPlanner::guess_gc()
 {
         double x = this->ps().x1();