From: Jiri Hubacek <hubacji1@fel.cvut.cz>
Date: Tue, 3 Jul 2018 11:07:30 +0000 (+0200)
Subject: Add closed loop simulation steering procedure
X-Git-Tag: v0.1.0~20^2
X-Git-Url: https://rtime.felk.cvut.cz/gitweb/hubacji1/iamcar.git/commitdiff_plain/770942107dfe33d8e172798f6535ff86cb872b5f

Add closed loop simulation steering procedure
---

diff --git a/CHANGELOG.md b/CHANGELOG.md
index 5e15f0d..b2e18ff 100644
--- a/CHANGELOG.md
+++ b/CHANGELOG.md
@@ -27,6 +27,8 @@ The format is based on [Keep a Changelog][] and this project adheres to
 - Alternative steering procedure.
 - Reeds and Shepp cost, steering procedures.
 - Bicycle car simulation.
+- Closed loop controller with PI for speed and PurePursuit based on
+  [Coulter1992] paper for steering.
 
 ### Changed
 - Adding JSON ouput for edges, samples.
diff --git a/incl/steer.h b/incl/steer.h
index 6584ecf..3ee9adb 100644
--- a/incl/steer.h
+++ b/incl/steer.h
@@ -24,5 +24,6 @@ along with I am car. If not, see <http://www.gnu.org/licenses/>.
 std::vector<RRTNode *> st1(RRTNode *init, RRTNode *goal);
 std::vector<RRTNode *> st2(RRTNode *init, RRTNode *goal);
 std::vector<RRTNode *> st3(RRTNode *init, RRTNode *goal);
+std::vector<RRTNode *> st4(RRTNode *init, RRTNode *goal);
 
 #endif
diff --git a/vehicle_platform/steer.cc b/vehicle_platform/steer.cc
index c58b004..2414072 100644
--- a/vehicle_platform/steer.cc
+++ b/vehicle_platform/steer.cc
@@ -16,12 +16,17 @@ along with I am car. If not, see <http://www.gnu.org/licenses/>.
 */
 
 #include <cmath>
+#include "bcar.h"
 #include "reeds_shepp.h"
 #include "steer.h"
 
 #define ST2WHEELBASE 2.450
 #define ST2TURNINGRADI 10.82
 #define ST2MAXSTEERING atan(ST2WHEELBASE / ST2TURNINGRADI)
+#define ST3COUNT 10
+#define ST3KP 1
+#define ST3KI 0
+#define ST3KD 0
 
 std::vector<RRTNode *> st1(RRTNode *init, RRTNode *goal)
 {
@@ -84,3 +89,44 @@ std::vector<RRTNode *> st3(RRTNode *init, RRTNode *goal)
         rsss.sample(q0, q1, 1, cb_st3, &nodes); // TODO const
         return nodes;
 }
+
+std::vector<RRTNode *> st4(RRTNode *init, RRTNode *goal)
+{
+        std::vector<RRTNode *> nodes;
+        BicycleCar bc(init->x(), init->y(), init->h());
+
+        float angl = atan2(goal->y() - init->y(), goal->x() - init->x());
+        float co = cos(angl - init->h());
+        //float si = sin(angl - init->h());
+
+        float speed = 1; // desired
+        float gx = goal->x();
+        float gy = goal->y();
+        if (co < 0)
+                speed = -speed;
+        float cerr = 0;
+        float lerr = 0;
+        float rx = 0; // recomputed goal x
+        float ry = 0; // recomputed goal y
+        float r = 0;
+        int i = 0;
+        for (i = 0; i < ST3COUNT; i++) {
+                // speed controller
+                cerr = speed - bc.speed();
+                bc.speed(ST3KP * cerr + ST3KI * lerr);
+                lerr += cerr;
+                // steer controller
+                rx = (gx - bc.x()) * cos(-bc.h()) -
+                        (gy - bc.y()) * sin(-bc.h());
+                ry = (gx - bc.x()) * sin(-bc.h()) -
+                        (gy - bc.y()) * cos(-bc.h());
+                if (ry != 0) {
+                        r = pow(rx, 2) + pow(ry, 2) / (2 * ry);
+                        bc.steer(1 / r);
+                }
+                // next iteration
+                bc.next();
+                nodes.push_back(new RRTNode(bc.x(), bc.y(), bc.h()));
+        }
+        return nodes;
+}