Add constants to `st1` steer procedure

diff --git a/vehicle_platform/steer.cc b/vehicle_platform/steer.cc
index 29fad36924e1fe4deadb537c39dfdd29de38f99e..b74000208d594dd0ac8ebab722fb62861ef2c711 100644 (file)
--- a/vehicle_platform/steer.cc
+++ b/vehicle_platform/steer.cc
@@ -20,6 +20,8 @@ along with I am car. If not, see <http://www.gnu.org/licenses/>.
 #include "reeds_shepp.h"
 #include "steer.h"
 
+#define ST1COUNT 10
+#define ST1STEP 0.2
 #define ST2WHEELBASE 2.450
 #define ST2TURNINGRADI 10.82
 #define ST2MAXSTEERING atan(ST2WHEELBASE / ST2TURNINGRADI)
@@ -36,10 +38,10 @@ std::vector<RRTNode *> st1(RRTNode *init, RRTNode *goal)
         float new_h;
         std::vector<RRTNode *> nodes;
         int i;
-        for (i = 1; i < 2; i++) { // TODO
+        for (i = 1; i < ST1COUNT + 1; i++) {
                 angl = atan2(goal->y() - init->y(), goal->x() - init->x());
-                new_x = init->x() + i * 1 * cos(angl); // TODO
-                new_y = init->y() + i * 1 * sin(angl); // TODO
+                new_x = init->x() + i * ST1STEP * cos(angl);
+                new_y = init->y() + i * ST1STEP * sin(angl);
                 new_h = goal->h();
                 nodes.push_back(new RRTNode(new_x, new_y, new_h));
         }