diff --git a/modules/CACC/CACC-Module-Test.py b/modules/CACC/CACC-Module-Test.py index 19dae0ba2cff19f058cfa83576ba672ec75b6d55..2cee30ce2b558009dd21f70ccb2cce1712b7cf9d 100644 --- a/modules/CACC/CACC-Module-Test.py +++ b/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 diff --git a/modules/CACC/simlifiedSpeedVisualization.py b/modules/CACC/simlifiedSpeedVisualization.py new file mode 100644 index 0000000000000000000000000000000000000000..95c5a704d71e3b2a6f2285d1d238907f306b9721 --- /dev/null +++ b/modules/CACC/simlifiedSpeedVisualization.py @@ -0,0 +1,78 @@ +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()