Car_Sim.py 1.92 KB
Newer Older
navidmokh's avatar
navidmokh committed
1
from Car_Dynamic_Single import *
2
# import matplotlib.pyplot as plt
navidmokh's avatar
navidmokh committed
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18

def TC_Simulate(Modes,initialCondition,time_bound):
	Modes = Modes.split(';')
	num_cars = len(Modes)
	#print initialCondition

	if len(initialCondition) == 5*num_cars:
		for car_numer in range(num_cars):
			Current_Initial = initialCondition[car_numer*5:car_numer*5+5]
			trace = Car_simulate(Modes[car_numer],Current_Initial,time_bound)
			trace = np.array(trace)
			if car_numer == 0:
				Final_trace = np.zeros(trace.size)
				Final_trace = trace
			else:
				if len(trace)!=len(Final_trace):
li213's avatar
li213 committed
19
					print(len(trace), len(Final_trace))
navidmokh's avatar
navidmokh committed
20
					for i in range(min(len(trace), len(Final_trace))):
li213's avatar
li213 committed
21
22
						print(trace[i][0], Final_trace[i][0])
					print(Final_trace)
navidmokh's avatar
navidmokh committed
23
24
25
26
27
28
29
30
31
32
				Final_trace = np.concatenate((Final_trace, trace[:,1:6]), axis=1)
	else:
		print('Number of cars does not match the initial condition')

	return Final_trace

if __name__ == "__main__":

	sol = TC_Simulate("Dec", [0, 0, 0, 1, 0.0], 30)
	# for line in sol:
li213's avatar
li213 committed
33
	# 	print(line
navidmokh's avatar
navidmokh committed
34
35
36
	#nextsol = TC_Simulate("TurnRight",sol[-1][1:],50)#2.4

	#finalsol = TC_Simulate("Const",nextsol[-1][1:],1.5)
li213's avatar
li213 committed
37
	#print(sol
navidmokh's avatar
navidmokh committed
38
	#for i in sol:
li213's avatar
li213 committed
39
	#	print(i
navidmokh's avatar
navidmokh committed
40
	#for j in nextsol:
li213's avatar
li213 committed
41
	#	print(j
navidmokh's avatar
navidmokh committed
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71

	#nextsoltime = [(row[0]+sol[-1][0]) for row in nextsol]
	#sxnext = [row[1] for row in nextsol]
	#synext = [row[2] for row in nextsol]
	sx = [row[1] for row in sol]
	sy = [row[2] for row in sol]
	#sxfinal = [row[1] for row in finalsol]
	#syfinal = [row[2] for row in finalsol]
	# plt.plot(sx, sy, "-r")
	# plt.show()

	# 	nextdelta = [row[5] for row in nextsol]
# #	for row in sol:
# #		print row
# 	#print "NEXTSOL"
# 	#for row in nextsol:
# #		print row
# 	time = [row[0] for row in sol]
# 	vx = [row[3] for row in sol]
# 	vy = [row[4] for row in sol]
# 	delta = [row[5] for row in sol]
	#plt.plot(time,sx,"-r")
	#plt.plot(time,sy,"-g")
	#plt.plot(time,vx,"-k")
	#plt.plot(time,vy,"-y")
	# plt.plot(time,delta,"-b")
	# plt.show()

	# plt.plot(nextsoltime,nextdelta)
	# plt.show()