Merge pull request commaai#87 from ShaneSmiskol/dynamic-follow

Dynamic follow

selfdrive/controls/lib/planner.py

@@ -205,26 +205,27 @@ def split_list(self, a_list):
 
   def acceleration_status(self):
     s = self.split_list(self.speed_list)
+    percentage_value = 0.01663919826514882  # this is .5 mph/second
     percentage_change = abs(abs(self.get_average(s[0]) - self.get_average(s[1])) / self.get_average([self.get_average(s[0]), self.get_average(s[1])]))
 
-    if self.get_average(s[0]) < self.get_average(s[1]) and percentage_change > 0.01663919826514882: # this is .5 mph/second, returns true if car is accelerating at .5mph/s in two second period
-      return 1 # accelerating
-    elif self.get_average(s[0]) > self.get_average(s[1]) and percentage_change > 0.01663919826514882:
+    if (self.get_average(s[0]) > self.get_average(s[1]) and percentage_change > percentage_value) or self.lead_1.vRel <= -6.710808: # should increase following distance sooner than detecting car's own deceleration, -6.7... is -3mph
       return -1 # decelerating
+    elif self.get_average(s[0]) < self.get_average(s[1]) and percentage_change > percentage_value: # true if car is accelerating at .5mph/s in latest two second period
+      return 1 # accelerating
     else:
       return 0 # constant speed
 
   def generateTR(self, speed):
     acceleration_code = self.acceleration_status()
     if acceleration_code == 1:
       x = [0, 20, 60, 70, 90]
-      y = [1.8, 1.2, 1.4, 1.9, 2.2]
+      y = [1.0, 1.2, 1.8, 2.0, 2.2]
     elif acceleration_code == -1:
-      x = [0, 20, 40, 60, 70, 90]
-      y = [1.8, 1.7, 1.8, 1.8, 2.2, 2.5]
+      x = [0, 5, 20, 60, 70, 90]
+      y = [1.6, 1.8, 1.9, 1.8, 2.2, 2.7]
     else: # constant speed
       x = [0, 20, 60, 70, 90]
-      y = [1.8, 1.6, 1.8, 2.0, 2.5]
+      y = [1.6, 1.4, 1.8, 2.0, 2.5]
     # return round(np.interp(speed, x, y), 2)
     f = interpolate.interp1d(x, y, fill_value='extrapolate') # interpolate above array
     return round(float(f(speed)[()]), 2)
@@ -233,9 +234,9 @@ def generate_cost(self, distance):
     x = [.9, 1.8, 2.7]
     y = [1.0, .1, .05]
 
-    diff = [abs(i - distance) for i in x]
-    return y[diff.index(min(diff))] # find closest cost, should fix beow
-    #return round(float(np.interp(distance, x, y)), 2) # caused stuttering issues when changing speed
+    #diff = [abs(i - distance) for i in x]
+    #return y[diff.index(min(diff))] # find closest cost, should fix beow
+    return round(float(np.interp(distance, x, y)), 2) # used to cause stuttering, but now we're doing a percentage change check before changing
 
   def update(self, CS, lead, v_cruise_setpoint):
     # Setup current mpc state
@@ -306,7 +307,7 @@ def update(self, CS, lead, v_cruise_setpoint):
         generatedTR = self.generateTR(CS.vEgo * 2.236936)
         TR = generatedTR
 
-        if self.last_cost != self.generate_cost(generatedTR):
+        if abs(self.generate_cost(generatedTR)-self.last_cost) > .2:
           self.libmpc.init(MPC_COST_LONG.TTC, self.generate_cost(generatedTR), MPC_COST_LONG.ACCELERATION, MPC_COST_LONG.JERK)
           self.last_cost = self.generate_cost(generatedTR)
       else: