Merge pull request #111 from ShaneSmiskol/dynamic-follow

Fix array size mismatch
sshane · Mar 18, 2019 · 696060a · 696060a
2 parents 75cd52b + b9dd900
commit 696060a
Showing 1 changed file with 6 additions and 6 deletions.
diff --git a/selfdrive/controls/lib/planner.py b/selfdrive/controls/lib/planner.py
@@ -205,19 +205,19 @@ def get_acceleration(self): # calculate car's own acceleration to generate more
   def generateTR(self, velocity): # in m/s
     global relative_velocity
     x = [0, 8.9408, 22.352, 31.2928, 35.7632, 40.2336] # in mph: [0, 20, 50, 70, 80, 90]
-    y = [1.0, 1.4, 1.6, 1.7, 1.85, 2.0] # distances
+    y = [1.0, 1.35, 1.6, 1.7, 1.85, 2.0] # distances
 
     TR = interpolate.interp1d(x, y, fill_value='extrapolate')  # extrapolate above 90 mph
 
     TR = TR(velocity)[()]
 
-    x = [-8.9408, -2.2352, 0, 1.78816]  # relative velocity values, mph: [-20, -5, 0, 4]
-    y = [(TR + .375), (TR + .05), TR, (TR - .25)]  # modification values, less modification with less difference in velocity
+    x = [-11.176, 0, 1.78816]  # relative velocity values, mph: [-25, 0, 4]
+    y = [(TR + .425), TR, (TR - .25)]  # distance modification values
 
-    TR = np.interp(relative_velocity, x, y)  # interpolate as to not modify too much
+    TR = np.interp(relative_velocity, x, y)  # interpolate as to not modify too much //todo: test extrapolation
 
-    x = [-4.4704, -2.2352, 0, 1.34112]  # acceleration values, mph: [-10, -5, 0, 3]
-    y = [(TR + .375), (TR + .15), TR, (TR - .3)]  # same as above
+    x = [-4.4704, 0, 1.34112]  # acceleration values, mph: [-10, 0, 3]
+    y = [(TR + .375), TR, (TR - .3)]
 
     TR = np.interp(self.get_acceleration(), x, y)