# ax2.cla()
# ax3.cla()
# ax1.plot(x_coords, y_coords, 'o')
# ax1.set_xlim([-2, 2])
# ax1.set_ylim([0, 4])
# ax2.plot(x_traj, y_traj, 'o')
# ax2.set_xlim([-3, 3])
# ax2.set_ylim([-3, 3])
# ax3.matshow(world.world, cmap='gray')
# plt.ion()
# plt.show()
# plt.pause(10)
## CONTROL ROBOT
robot.set_goal([x_traj[traj_count],y_traj[traj_count]], th_traj[traj_count])
robot = robot.PI_control(vicon.x_v, vicon.q_v, 1)
robot = robot.write_motors()
t_send = time.time()
print('goal coordinates: ', x_traj[traj_count], y_traj[traj_count], th_traj[traj_count])
if robot.p < 0.15:
print('Reached waypoint')
time.sleep(0.1)
traj_count = traj_count + 1
robot.integral_reset()
g_err = math.sqrt((x_g[0]-vicon.x_v[0])**2 + (x_g[1]-vicon.x_v[1])**2)
if g_err < 0.1:
RUN_ROBOT = False
t_vicon = 0
t_send = 0
RUN_ROBOT = True
try:
while RUN_ROBOT:
print("\n")
if (time.time() - t_vicon) > 0.1:
vicon.get_state()
t_vicon = time.time()
robot = robot.set_goal([x_traj[traj_count], y_traj[traj_count]],
th_traj[traj_count])
robot = robot.PI_control(vicon.x_v, vicon.q_v, 1 - 0.0001)
if (time.time() - t_send) > 0.1:
robot = robot.write_motors()
t_send = time.time()
if robot.p < 0.25:
traj_count = traj_count + 1
robot.integral_reset()
if traj_count == traj_length:
RUN_ROBOT = False
except KeyboardInterrupt:
robot.stop_robot()
print("Robot stopped.. hopefully")
