diff --git a/modules/CACC/CACC-Module-Test.py b/modules/CACC/CACC-Module-Test.py
index 412a2b0f50a1cac80439fb31c1dd56be776a6b75..74a8579b90eabc9fc965180affb5cd2f26957608 100644
--- a/modules/CACC/CACC-Module-Test.py
+++ b/modules/CACC/CACC-Module-Test.py
@@ -1,3 +1,4 @@
+import random
import matplotlib
matplotlib.use('TkAgg')
from matplotlib import pyplot as plt
@@ -7,17 +8,24 @@ class Car(object):
cPos = None
cDist = None
+ cIPD = None
IPD = None
PS = None
- def __init__(self, prev, speed, position, distance, IPD, PS):
+ def __init__(self, prev, speed, position, IPD, PS):
self.plattonPrev = prev
self.cSpeed = speed
self.cPos = position
- self.cDist = distance
+ self.updateDis()
+ self.cIPD = 0.01*IPD # noch keine cIPD definition existent
self.IPD = IPD
self.PS = PS
+
+ self.bcIPD = False
return None
+
+ def getCIPD(self):
+ return self.cIPD
def getPos(self):
return self.cPos
@@ -29,96 +37,135 @@ class Car(object):
return self.cDist
def updatePos(self):
- self.cPos = self.cPos + self.cSpeed
+ self.cPos = self.cPos + self.cSpeed/16 #Ticklaenge. Kann zusammen mit dem i in den Updates angepasst werden. Das Produkt beider muss 1 sein
def updateDis(self):
if not self.plattonPrev == None:
- self.cDist = self.plattonPrev.getPos() - self.cPos
+ self.cDist = - self.cPos + self.plattonPrev.getPos()#* random.uniform(0.999, 1.001) # bis zu 0.001 relative Abweichung auf die Entfernungsmessung
def updateSpeed(self):
- v_c = self.cSpeed
- v_v = None
- PS = self.PS
- c = None
- if not self.plattonPrev == None:
- v_v = self.plattonPrev.getSpeed()
- c = ((v_c - v_v)/v_c)**(v_c/v_v)
- print(c, v_v/v_c)
-
- v_z = v_v + (v_v - PS)*c
- self.cSpeed = (1-c)*v_c + c * v_z
+ if not self.bcIPD:
+ d_c = self.cDist
+ v_c = self.cSpeed
+ IPD = self.IPD
+ PS = self.PS
+ v_n = None
+
+ if not self.plattonPrev == None:
+ v_v = self.plattonPrev.getSpeed() #* random.uniform(0.9, 1.1) # bis zu 0.1 relative Abweichung auf die Geschwindigkeitsmessung
+ if self.checkInbound(d_c, IPD, 0.01*IPD):
+ if self.checkInbound (v_c, v_v, 0.01*v_v):
+ if self.checkInbound(v_c, PS, 0.01*PS):
+ v_n = PS
+ else:
+ if v_c > PS:
+ v_n = v_v - abs(PS-v_v)/4
+ else:
+ v_n = v_v + abs(PS-v_v)/4
+ else:
+ if v_c > v_v:
+ v_n = v_v - abs(PS-v_v)/4
+ else:
+ v_n = v_v + abs(PS-v_v)/4
+ else:
+ v_n = (v_v * (d_c/IPD)**2) #Der Exponent gibt an, wie schnell die Aenderung umgesetzt werden soll
+ self.cSpeed = v_n
+
+ self.cIPD = self.IPD * 0.01 #too low
+ if self.cIPD > self.cDist:
+ self.cSpeed = 0
+ self.bcIPD = True
+ self.cIPD = -10 #Zur Visualisierung
+ else:
+ v_v = self.cSpeed
+ c = 0.8
+ self.cSpeed = c * v_c + (1-c) * PS
+
+ def checkInbound(self, argument, target, delta): #Ueberall ist ein delta von 0.01 hinterlegt
+ if argument > target + delta:
+ return False
+ elif argument < target - delta:
+ return False
else:
- v_v = self.cSpeed
- c = 0.5
- v_z = v_v - (v_v - PS) * c
- self.cSpeed = (1-c) * v_c + c * v_z
+ return True
+
+
+def broadcastPlattonSettings(carList, newIPD, newPS):
+ for item in carList:
+ item.PS = newPS
+ item.IPD = newIPD
def update(carList):
for item in carList:
item.updatePos()
item.updateDis()
+ for item in reversed(carList):
item.updateSpeed()
if __name__ == "__main__":
- IPD = 60.0
- PS = 40.0
-
- v_0 = 78.0
- v_1 = 80.0
+ maxT = [20, 40, 80, 110, 130,150,170] # times for events
+ setIPD = [40, 40, 40, 60, 45, 30, 15] # event of IPD (activ until time event)
+ setPS = [20, 30, 10, 10, 10, 10, 10] # event of PS (activ until time event)
- pos_0 = IPD
- pos_1 = 0.0
+ v = [52.0, 50.0, 10.0] # startvektor (Startgeschwindikeit)
+ pos = [70, 30, 0] # startvektor (Startposition)
- dist0 = 0
- dist1 = pos_0 - pos_1
+ cars = []
+ cars.append(Car(None, v[0], pos[0], setIPD[0], setPS[0]))
+ cars.append(Car(cars[0], v[1], pos[1], setIPD[0], setPS[0]))
+ cars.append(Car(cars[1], v[2], pos[2], setIPD[0], setPS[0]))
- car0 = Car(None, v_0, pos_0, dist0, IPD, PS)
- car1 = Car(car0, v_1, pos_1, dist1, IPD, PS)
-
- i = 0
t = []
- maxT = 40
+ mesIPD= []
+ mesPS = []
+
+ y_pos = [[],[],[]]
+ y_speed = [[],[],[]]
+ y_dist = [[],[],[]]
+ y_cdist = [[],[],[]]
- y_pos0 =[]
- y_pos1 = []
-
- y_speed0 = []
- y_speed1 = []
-
- y_dist0 = []
- y_dist1 = []
-
- carList = [car0, car1]
-
- while i <= maxT:
- t.append(i)
-
- y_pos0.append(car0.getPos())
- y_pos1.append(car1.getPos())
+ i = 0.0
+ for j in range(0, len(maxT)):
+ IPD = setIPD[j]
+ PS = setPS[j]
+ broadcastPlattonSettings(cars, IPD, PS)
+
+ while i < maxT[j]:
+ t.append(i)
+ mesIPD.append(IPD)
+ mesPS.append(PS)
- y_speed0.append(car0.getSpeed())
- y_speed1.append(car1.getSpeed())
+ for k in range(0, len(cars)):
+ y_pos[k].append(cars[k].getPos())
+ y_speed[k].append(cars[k].getSpeed())
- y_dist1.append(car1.getDistance())
+ for k in range(1, len(cars)):
+ y_dist[k].append(cars[k].getDistance())
+ y_cdist[k].append(cars[k].getCIPD())
- i = i+1
- update(carList)
+ i = i+0.0625
+ update(cars)
plt.subplot(131)
- plt.plot(t, y_pos1)
- plt.plot(t, y_pos0)
+ plt.plot(t, y_pos[0], 'r')
+ plt.plot(t, y_pos[1], 'b')
+ plt.plot(t, y_pos[2], 'g')
plt.title("Position")
plt.subplot(132)
- plt.plot([0,maxT],[PS, PS], 'r--')
- plt.plot(t, y_speed1)
- plt.plot(t, y_speed0)
+ plt.plot(t, mesPS, 'r--')
+ plt.plot(t, y_speed[0], 'r-')
+ plt.plot(t, y_speed[1], 'b')
+ plt.plot(t, y_speed[2], 'g')
plt.title("Speed")
plt.subplot(133)
- plt.plot([0,maxT],[IPD, IPD], 'r--')
- plt.plot(t, y_dist1)
+ plt.plot(t, mesIPD, 'r--')
+ plt.plot(t, y_dist[1], 'b-')
+ plt.plot(t, y_cdist[1], 'b--')
+ plt.plot(t, y_dist[2], 'g-')
+ plt.plot(t, y_cdist[2], 'g--')
plt.title("Distance")
plt.show()