CACC Sim added

modules/CACC/CACC-Module-Test.py

+import matplotlib
+matplotlib.use('TkAgg')
+from matplotlib import pyplot as plt
+
+class Car(object):
+    cSpeed = None
+    cPos = None
+    cDist = None
+
+    IPD = None
+    PS = None
+
+    def __init__(self, prev, speed, position, distance, IPD, PS):
+        self.plattonPrev = prev
+        self.cSpeed = speed
+        self.cPos = position
+        self.cDist = distance
+        self.IPD = IPD
+        self.PS = PS
+        return None
+    
+    def getPos(self):
+        return self.cPos
+
+    def getSpeed(self):
+        return self.cSpeed
+
+    def getDistance(self):
+        return self.cDist
+
+    def updatePos(self):
+        self.cPos = self.cPos + self.cSpeed
+
+    def updateDis(self):
+        if not self.plattonPrev == None:
+            self.cDist = self.plattonPrev.getPos() - self.cPos
+
+    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
+        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
+
+def update(carList):
+    for item in carList:
+        item.updatePos()
+        item.updateDis()
+        item.updateSpeed()
+
+if __name__ == "__main__":
+    IPD = 60.0
+    PS = 40.0
+
+    v_0 = 78.0
+    v_1 = 80.0
+
+    pos_0 = IPD
+    pos_1 = 0.0
+    
+    dist0 = 0
+    dist1 = pos_0 - pos_1
+
+    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
+    
+    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())
+
+        y_speed0.append(car0.getSpeed())
+        y_speed1.append(car1.getSpeed())
+
+        y_dist1.append(car1.getDistance())
+
+        i = i+1
+        update(carList)
+
+
+    plt.subplot(131)
+    plt.plot(t, y_pos1)
+    plt.plot(t, y_pos0)
+    plt.title("Position")
+
+    plt.subplot(132)
+    plt.plot([0,maxT],[PS, PS], 'r--')
+    plt.plot(t, y_speed1)
+    plt.plot(t, y_speed0)
+    plt.title("Speed")
+    
+    plt.subplot(133)
+    plt.plot([0,maxT],[IPD, IPD], 'r--')
+    plt.plot(t, y_dist1)
+    plt.title("Distance")
+
+    plt.show()