Car_Sim.py 1.92 KB
 navidmokh committed Feb 19, 2019 1 ``````from Car_Dynamic_Single import * `````` li213 committed Nov 10, 2021 2 ``````# import matplotlib.pyplot as plt `````` navidmokh committed Feb 19, 2019 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 committed Dec 01, 2021 19 `````` print(len(trace), len(Final_trace)) `````` navidmokh committed Feb 19, 2019 20 `````` for i in range(min(len(trace), len(Final_trace))): `````` li213 committed Dec 01, 2021 21 22 `````` print(trace[i][0], Final_trace[i][0]) print(Final_trace) `````` navidmokh committed Feb 19, 2019 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 committed Dec 01, 2021 33 `````` # print(line `````` navidmokh committed Feb 19, 2019 34 35 36 `````` #nextsol = TC_Simulate("TurnRight",sol[-1][1:],50)#2.4 #finalsol = TC_Simulate("Const",nextsol[-1][1:],1.5) `````` li213 committed Dec 01, 2021 37 `````` #print(sol `````` navidmokh committed Feb 19, 2019 38 `````` #for i in sol: `````` li213 committed Dec 01, 2021 39 `````` # print(i `````` navidmokh committed Feb 19, 2019 40 `````` #for j in nextsol: `````` li213 committed Dec 01, 2021 41 `````` # print(j `````` navidmokh committed Feb 19, 2019 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()``````