Smoothes CACC function

modules/CACC/CACC-Module-Test.py

@@ -38,7 +38,7 @@ class Car(object):
 
         self.bcIPD = False
         
-        RAND = 0.5
+        RAND = 5
         self.SonarSystemUnc      = random.uniform(-RAND,RAND)       # in cm
         self.SonarStatisticalUnc = lambda : random.gauss(0,RAND) # in cm
         
@@ -109,22 +109,8 @@ class Car(object):
                 x = self.KF.getValue()
                 d = x[0,0]
                 self.v_v = v_v = v + x[1,0]
-                if checkInbound(d, IPD, 0.01*IPD):
-                    if checkInbound (v, v_v, 0.01*v_v):
-                        if checkInbound(v, PS, 0.01*PS):
-                            v_new = PS
-                        else:
-                            if v > PS:
-                                v_new = v_v - abs(PS-v_v)/4
-                            else:
-                                v_new = v_v + abs(PS-v_v)/4
-                    else:
-                        if v > v_v:
-                            v_new = v_v - abs(PS-v_v)/4
-                        else:
-                            v_new = v_v + abs(PS-v_v)/4
-                else:
-                    v_new = (v_v * (d/IPD)**2)      #Der Exponent gibt an, wie schnell die Aenderung umgesetzt werden soll
+
+                v_new = self.computeNewSpeed_2(d, v, v_v, IPD, PS)
 
                 inertia = 0.8                                   # in [0,1] und gibt die Traegheit des Fahrzeuges an
                 self.setSpeed(inertia * v + (1- inertia) * v_new)
@@ -137,6 +123,32 @@ class Car(object):
                 c = 0.5
                 self.v_v = 0
                 self.setSpeed(c * v + (1-c) * PS)
+                
+                
+    def computeNewSpeed_1(self, d, v, v_v, IPD, PS):
+        if checkInbound(d, IPD, 0.01*IPD):
+            if checkInbound (v, v_v, 0.01*v_v):
+                if checkInbound(v, PS, 0.01*PS):
+                    v_new = PS
+                else:
+                    if v > PS:
+                        v_new = v_v - abs(PS-v_v)/4
+                    else:
+                        v_new = v_v + abs(PS-v_v)/4
+            else:
+                if v > v_v:
+                    v_new = v_v - abs(PS-v_v)/4
+                else:
+                    v_new = v_v + abs(PS-v_v)/4
+        else:
+            v_new = (v_v * (d/IPD)**2)      #Der Exponent gibt an, wie schnell die Aenderung umgesetzt werden soll
+        return v_new
+    
+    def computeNewSpeed_2(self, d, v, v_v, IPD, PS):
+        DIST_POW = 2
+        SPEED_POW = 0.5
+        v_new = (v_v * (d/(IPD))**DIST_POW * (PS/v_v)**SPEED_POW)     #Der Exponent gibt an, wie schnell die Aenderung umgesetzt werden soll
+        return v_new
 
 
 def checkInbound(argument, target, delta):    #Ueberall ist ein delta von 0.01 hinterlegt

modules/CACC/simlifiedSpeedVisualization.py

+import numpy as np
+import matplotlib
+matplotlib.use('TkAgg')
+from matplotlib import pyplot as plt
+
+
+v   = 20
+v_v = 0.5
+PS  = 20
+IPD = 20
+
+IPD_TOL = 0.05*IPD
+
+DIST_POW = 2
+SPEED_POW = 0.5
+
+ds = np.linspace(0.*IPD, 1.5*IPD, num = 1000)
+v_news = []
+
+def checkInbound(argument, target, delta):    #Ueberall ist ein delta von 0.01 hinterlegt
+    if argument > target + delta:
+        return False
+    elif argument < target - delta:
+        return False
+    else:
+        return True
+
+def variante_1(d):
+  v_new = None
+  if checkInbound(d, IPD, IPD_TOL):
+      if checkInbound (v, v_v, 0.01*v_v):
+          if checkInbound(v, PS, 0.01*PS):
+              v_new = PS
+          else:
+              if v > PS:
+                  v_new = v_v - abs(PS-v_v)/4
+              else:
+                  v_new = v_v + abs(PS-v_v)/4
+      else:
+          if v > v_v:
+              v_new = v_v - abs(PS-v_v)/4
+          else:
+              v_new = v_v + abs(PS-v_v)/4
+  else:
+      v_new = (v_v * (d/IPD)**2)      #Der Exponent gibt an, wie schnell die Aenderung umgesetzt werden soll
+  return v_new
+
+def variante_2(d):
+  v_new = None
+  if d < IPD - IPD_TOL:
+    v_new = (v_v * (d/(IPD - IPD_TOL))**2)      #Der Exponent gibt an, wie schnell die Aenderung umgesetzt werden soll
+  elif d > IPD + IPD_TOL:
+    v_new = (v_v * (d/(IPD + IPD_TOL))**2)      #Der Exponent gibt an, wie schnell die Aenderung umgesetzt werden soll
+  else:
+    v_new = v_v
+  return v_new
+      
+def variante_3(d):
+  v_new = (v_v * (d/(IPD))**DIST_POW)      #Der Exponent gibt an, wie schnell die Aenderung umgesetzt werden soll
+  return v_new
+
+def variante_4(d):
+  v_new = (v_v * (d/(IPD))**DIST_POW * (PS/v_v)**SPEED_POW)     #Der Exponent gibt an, wie schnell die Aenderung umgesetzt werden soll
+  return v_new
+
+for d in ds:
+  v_new = variante_4(d)
+  v_news.append(v_new)
+
+plt.plot(ds, v_news,'b', label='v_new')
+plt.plot(ds, [v_v for d in ds],'tab:orange', label='v_v')
+plt.plot(ds, [PS  for d in ds], 'g', label='PS')
+plt.plot(ds, [v  for d in ds], 'b--', label='v')
+plt.plot([IPD, IPD], [0, max(v_news)], 'r', label='IPD')
+plt.plot([IPD - IPD_TOL, IPD - IPD_TOL], [0, max(v_news)], 'r--', label='IPD-')
+plt.plot([IPD + IPD_TOL, IPD + IPD_TOL], [0, max(v_news)], 'r--', label='IPD+')
+plt.legend()
+plt.show()