Add constants to `st2` steer procedure

diff --git a/vehicle_platform/steer.cc b/vehicle_platform/steer.cc
index b74000208d594dd0ac8ebab722fb62861ef2c711..10a4d92a0f4fb036312316881c892ca47ad593ed 100644 (file)
--- a/vehicle_platform/steer.cc
+++ b/vehicle_platform/steer.cc
@@ -22,9 +22,13 @@ along with I am car. If not, see <http://www.gnu.org/licenses/>.
 
 #define ST1COUNT 10
 #define ST1STEP 0.2
+
+#define ST2COUNT 1
+#define ST2STEP 1
 #define ST2WHEELBASE 2.450
 #define ST2TURNINGRADI 10.82
 #define ST2MAXSTEERING atan(ST2WHEELBASE / ST2TURNINGRADI)
+
 #define ST4COUNT 10
 #define ST4KP 1
 #define ST4KI 0
@@ -50,28 +54,39 @@ std::vector<RRTNode *> st1(RRTNode *init, RRTNode *goal)
 
 std::vector<RRTNode *> st2(RRTNode *init, RRTNode *goal)
 {
-        float speed = 1;
-        float angl = atan2(goal->y() - init->y(), goal->x() - init->x());
-        float sa = angl - init->h(); // steering angle
-        float co = cos(angl - init->h());
-        float si = sin(angl - init->h());
-        if (co < 0) {
-                speed = -speed;
-                if (si > 0)
-                        sa = -sa;
-        } else {
-                if (si < 0)
-                        sa = -sa;
-        }
-        if (sa > ST2MAXSTEERING)
-                sa = ST2MAXSTEERING;
-        if (sa < -ST2MAXSTEERING)
-                sa = -ST2MAXSTEERING;
-        float new_h = init->h() + (speed / ST2WHEELBASE) * tan(sa);
-        float new_x = init->x() + speed * cos(new_h);
-        float new_y = init->y() + speed * sin(new_h);
+        float speed;
+        float angl;
+        float steer; // steering angle
+        float co;
+        float si;
+        float new_h;
+        float new_x;
+        float new_y;
         std::vector<RRTNode *> nodes;
-        nodes.push_back(new RRTNode(new_x, new_y, new_h));
+        int i;
+        for (i = 1; i < ST2COUNT + 1; i++) {
+                speed = ST2STEP;
+                angl = atan2(goal->y() - init->y(), goal->x() - init->x());
+                steer = angl - init->h(); // steering angle
+                co = cos(angl - init->h());
+                si = sin(angl - init->h());
+                if (co < 0) {
+                        speed = -speed;
+                        if (si > 0)
+                                steer = -steer;
+                } else {
+                        if (si < 0)
+                                steer = -steer;
+                }
+                if (steer > ST2MAXSTEERING)
+                        steer = ST2MAXSTEERING;
+                if (steer < -ST2MAXSTEERING)
+                        steer = -ST2MAXSTEERING;
+                new_h = init->h() + (speed / ST2WHEELBASE) * tan(steer);
+                new_x = init->x() + speed * cos(new_h);
+                new_y = init->y() + speed * sin(new_h);
+                nodes.push_back(new RRTNode(new_x, new_y, new_h));
+        }
         return nodes;
 }