| 1234567891011121314151617181920212223242526272829303132333435363738394041424344454647484950515253545556 |
- from AGV import *
- from CBD.simulator import Simulator
- from CBD.realtime.plotting import PlotManager, LinePlot
- import matplotlib.pyplot as plt
- from itertools import cycle
- import pandas as pd
- DELTA_T = 0.2
- END = 650.0
- if __name__ == '__main__':
- fig = plt.figure(figsize=(5, 5), dpi=100)
- ax = fig.add_subplot(111)
- ax.set_xlim((-.45, .45))
- ax.set_ylim((-.35, .55))
- lines = ["-","--","-.",":"]
- linecycler = cycle(lines)
- model = AGVVirtual("AGV", path_file="paths/falcon.csv", Kp=0.0, Ki=0.0, Kd=-0.02, v=0.001, T=35,
- wheel_radius=0.016512, wheel_axis=0.1944, initial=[0, -.3, 0])
- traj = model.findBlock("environment")[0].path
- ax.plot([x for x, _ in traj], [y for _, y in traj], c='salmon', label="path")
- sets = {
- "012": -0.02,
- "072": -0.12,
- "084": -0.14,
- "102": -0.17
- }
- for d, Kd in sets.items():
- data = pd.read_csv(f"tuning6/path-00{d}.csv")
- ax.plot(data["x"], data["y"], next(linecycler), lw=2, label="Kd = " + str(Kd))
- # data012 = pd.read_csv("tuning6/path-00012.csv")
- # data072 = pd.read_csv("tuning6/path-00072.csv")
- # data082 = pd.read_csv("tuning6/path-00084.csv")
- # data102 = pd.read_csv("tuning6/path-00102.csv")
- # print(data012)
- # manager = PlotManager()
- # manager.register("plot", model.findBlock("plot")[0], (fig, ax), LinePlot())
- #
- # sim = Simulator(model)
- # sim.setDeltaT(DELTA_T)
- # # sim.setProgressBar()
- # # sim.setRealTimePlatformGameLoop()
- # sim.setRealTime(scale=.01)
- # sim.run(END)
- #
- fig.tight_layout()
- ax.legend(loc="upper left")
- plt.show()
|
