diff --git a/modules/CACC/platoonSimulation.py b/modules/CACC/platoonSimulation.py
new file mode 100644
index 0000000000000000000000000000000000000000..367bca46a7ace3505d053650b6a0933546f6f5c3
--- /dev/null
+++ b/modules/CACC/platoonSimulation.py
@@ -0,0 +1,248 @@
+import sys
+import IPython
+import bisect
+import time
+import math
+import random
+import collections
+
+import matplotlib
+matplotlib.use('TkAgg')
+from matplotlib import pyplot as plt
+
+class CrashException(Exception):
+ pass
+
+class Vehicle(object):
+ def __init__(self,name,typ,road,initPos,initVelo):
+
+ self.name = name
+ self.typ = typ
+ self.road = road
+
+ self.velos = []
+ self.posis = []
+ self.dists = []
+ self.velosVA = []
+
+ self.posi = 1.0*initPos # to make it a float value
+ if self.typ == "stone":
+ self.velo = 0
+ else:
+ self.velo = initVelo
+
+ self.engInAcc = (100 + random.randint(-3,3))/100.0 # constant inaccuracy for my own engine
+ self.sensorInAcc = (100 + random.randint(-3,3))/100.0
+
+ @property
+ def velo(self):
+ return self.velos[-1]
+ @velo.setter
+ def velo(self,value):
+ self.velos.append(value)
+
+ @property
+ def posi(self):
+ return self.posis[-1]
+ @posi.setter
+ def posi(self,value):
+ self.posis.append(value)
+
+ @property
+ def dist(self):
+ return self.dists[-1]
+ @dist.setter
+ def dist(self,value):
+ self.dists.append(value)
+
+ @property
+ def veloVA(self):
+ return self.velosVA[-1]
+ @veloVA.setter
+ def veloVA(self,value):
+ self.velosVA.append(value)
+
+ def __str__(self):
+ retStr = "---"
+ retStr += self.name + ".posi: " + str(self.posi) + "\n"
+ retStr += self.name + ".velo: " + str(self.velo) + "\n"
+ retStr += self.name + ".dist: " + str(self.dist) + "\n"
+ retStr += self.name + ".veloVA: " + str(self.veloVA) + "\n"
+ retStr += self.name + ".engInAcc: " + str(self.engInAcc) + "\n"
+ retStr += self.name + ".sensorInAcc: " + str(self.sensorInAcc) + "\n"
+ retStr += "---\n"
+ return retStr
+
+ def update(self, tStep):
+
+ oldVelo = self.velo
+
+ # get sensor data
+ self.updateSensorData(tStep)
+
+ # udapte position
+ self.posi += tStep*(oldVelo + self.velo)/2
+
+ # decide on values
+ if self.typ == "fv":
+ if abs(self.dist - IPD) > IPD_TOL:
+ self.velo = self.engInAcc*min(self.veloVA, PS)*(self.dist / IPD)
+ else:
+ self.velo = self.engInAcc*PS
+
+ if self.typ == "lv":
+ if self.dist < IPD:
+ # self.velo = max(self.velo - 1, 0)
+ self.velo = 0
+ else:
+ self.velo = self.engInAcc*PS
+
+ def updateSensorData(self,tStep):
+ try:
+ oldDist = self.dist
+ except IndexError:
+ oldDist = self.sensorInAcc*self.road.getDistance(self)
+
+ self.dist = self.sensorInAcc*self.road.getDistance(self)
+ self.veloVA = self.velo + (self.dist - oldDist)/tStep
+ return
+
+ def __le__(self, other):
+ return self.posi <= other.posi
+
+ def __lt__(self, other):
+ return self.posi < other.posi
+
+
+class Road(object):
+ def __init__(self):
+ self.vehicles = []
+
+ def __str__(self):
+ retStr = ""
+ for vehicle in self.vehicles:
+ retStr += vehicle.__str__()
+ return retStr
+
+ def placeVehicle(self, vehicle):
+ bisect.insort(self.vehicles, vehicle)
+
+ def update(self,tStep):
+ for vIdx, vehicle in enumerate(self.vehicles):
+ vehicle.update(tStep)
+ if vIdx < len(self.vehicles)-1:
+ if vehicle.posi >= self.vehicles[vIdx+1].posi:
+ raise CrashException("Crash!")
+
+ def draw(self):
+ minPosi = self.vehicles[0].posi
+ maxPosi = self.vehicles[-1].posi
+ relPos = [int((vehicle.posi - minPosi)/(maxPosi - minPosi)*100) for vehicle in self.vehicles]
+
+ drawing = ""
+ relDrawProg = 0
+ for vIdx, vehicle in enumerate(self.vehicles):
+ spaceToFill = relPos[vIdx] - relDrawProg
+ distStr = "<" + str(int(self.vehicles[vIdx-1].dist)).zfill(3) + ">"
+ if spaceToFill >= len(distStr):
+ frontspace = int(math.ceil((spaceToFill - len(distStr))/2.0))
+ backspace = int(math.floor((spaceToFill - len(distStr))/2.0))
+ for _ in range(frontspace):
+ drawing += " "; relDrawProg += 1
+ drawing += distStr; relDrawProg += len(distStr)
+ for _ in range(backspace):
+ drawing += " "; relDrawProg += 1
+ else:
+ for _ in range(spaceToFill):
+ drawing += " "
+ relDrawProg += 1
+ drawing += vehicle.name
+ relDrawProg += 1
+
+ print("-----------------------------------------------------------------------------------------------------")
+ print(drawing)
+ print("-----------------------------------------------------------------------------------------------------")
+
+ def getDistance(self,vehicle):
+ vIdx = self.vehicles.index(vehicle)
+ if vIdx == len(self.vehicles)-1:
+ return sys.maxsize
+ else:
+ return self.vehicles[vIdx + 1].posi - self.vehicles[vIdx].posi
+
+ def getVeloVA(self, vehicle):
+ vIdx = self.vehicles.index(vehicle)
+ if vIdx == len(self.vehicles)-1:
+ return sys.maxsize
+ else:
+ return self.vehicles[vIdx + 1].velo
+
+
+# GLOBALS
+IPD = 15*100 # in millimetres
+IPD_TOL = 1*100 # in millimetres
+
+PS = 15 # in millimetres per milliseconds
+PS_TOL = 1 # in millimetres per milliseconds
+
+SENSOR_UPDATE_TIME = .5 # in milliseconds
+
+if __name__ == "__main__":
+
+ tStart = 0; tStep = 1; tEnd = 500
+
+ road = Road()
+ lv = Vehicle("L","lv",road,100*100,15); road.placeVehicle(lv)
+ fv1 = Vehicle("1","fv",road,75*100,20); road.placeVehicle(fv1)
+ # fv2 = Vehicle("2","fv",road,50*100,15); road.placeVehicle(fv2)
+
+ # startStone = Vehicle("s","stone",road,0*100,0); road.placeVehicle(startStone)
+ # endStone = Vehicle("S","stone",road,400*100,0); road.placeVehicle(endStone)
+
+ t = tStart;
+ ts = []
+ while True:
+
+ try:
+ road.update(SENSOR_UPDATE_TIME)
+ except CrashException:
+ print("CRASH!")
+ break
+
+ t += SENSOR_UPDATE_TIME; ts.append(t)
+ # print("t = ", t)
+ # road.draw()
+ # time.sleep(0.2)
+
+ if t > tEnd:
+ break
+ plt.subplot(131)
+ plt.plot([tStart] + ts, lv.posis, 'm')
+ plt.plot([tStart] + ts, fv1.posis, 'g')
+ # plt.plot([tStart] + ts, fv2.posis, 'b')
+ plt.title("Position")
+ plt.ylim(7000,18000)
+
+ plt.subplot(132)
+ plt.plot(ts, [PS for t in ts],'r', ts,[PS-PS_TOL for t in ts],'r--', ts,[PS+PS_TOL for t in ts],'r--')
+ plt.plot([tStart] + ts, lv.velos, 'm--')
+ plt.plot([tStart] + ts, [d/lv.engInAcc for d in lv.velos], 'm')
+ plt.plot([tStart] + ts, fv1.velos, 'g--')
+ plt.plot([tStart] + ts, [d/fv1.engInAcc for d in fv1.velos], 'g')
+ plt.plot(ts, fv1.velosVA, 'm.')
+ # plt.plot([tStart] + ts, fv2.velos, 'b--')
+ # plt.plot([tStart] + ts, [d/fv2.engInAcc for d in fv2.velos], 'b')
+ plt.title("Speed")
+ plt.ylim(13,26)
+
+ plt.subplot(133)
+ plt.plot(ts,[IPD for t in ts],'r', ts,[IPD-IPD_TOL for t in ts],'r--', ts,[IPD+IPD_TOL for t in ts],'r--')
+ plt.plot(ts, fv1.dists, 'g--')
+ plt.plot(ts, [d/fv1.sensorInAcc for d in fv1.dists], 'g')
+ # plt.plot(ts, fv2.dists, 'b--')
+ # plt.plot(ts, [d/fv2.sensorInAcc for d in fv2.dists], 'b')
+ plt.title("Distance")
+ plt.ylim(1300,2600)
+
+ plt.show()
+