# 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")