def get_robot_position(): location = commands.where_robot()["center"] return location
#predictions.append(estimate) #print("ERROR: ", predictions[0][0] - theta, predictions[0][1] - x, predictions[0][2] - y) if len(predictions) > 8: predictions.pop(0) #robot.move_to_point(goal, (estimate[0], estimate[1]), estimate[2], 10) print(goal, x, y, theta) robot.move_to_point(goal, (x, y), theta, 10) sleep(0.11) commands.set_speed(0, 0) sleep(1.5) if len(estimate) == 3: print( "Camera position: " + str(position["center"][0]) + ", " + str(position["center"][1]) + ", camera orientation: " + str(math.atan2(position["orientation"][1], position["orientation"][0])) ) print("Estimated position: " + str(estimate[0]) + "," + str(estimate[1]) + ", estimated orientation: " + str(estimate[2])) position = commands.where_robot() (x, y, theta) = (position["center"][0], position["center"][1], math.atan2(position["orientation"][1], position["orientation"][0])) print("Actual position: " + str(x) + "," + str(y) + ", actual orientation: " + str(theta)) print("Error: (" + str(math.fabs(estimate[1] - x)) + ", " + str(math.fabs(estimate[2] - y)) + ", " + str(math.fabs(estimate[0] - theta)) + ")")
ADDRESS = ("0.0.0.0", 55555) commands.open_connection(ADDRESS) markers = where_markers() waypoints = [] for marker_number in markers: if (marker_number == "time" or marker_number == "robot"): continue waypoints.append(markers[marker_number]["center"]) states = [] timestamps = [] while len(waypoints) > 0: robot_speed = get_speed() states.append([robot_speed['speed_a'], robot_speed['speed_b']]) timestamps.append(float(where_all()['time'])) if len(states) > 3: states.pop(0) timestamps.pop(0) sensor_position = get_robot_position() sensor_orientation = where_robot()['orientation'] angle = atan2(sensor_orientation[1], sensor_orientation[0]) (x, y, new_angle) = get_estimated_position( states, [sensor_position[0], sensor_position[1], angle], timestamps) if move_to_point((waypoints[0][0], waypoints[0][1]), (x, y), new_angle, 8): waypoints.pop(0) sleep(.3) #the follow_vector also sleeps for 0.05 seconds
from time import sleep import math import sys commands.open_connection(("0.0.0.0", 55555)) commands.set_pid_params(10, 4, 2) speed = int(sys.argv[1]) delays = [2.2, 1.6, 1.0 , 2.8] avg_speed = 0 for delay in delays: p_init = commands.where_robot()["center"] if delay == 1.0: speed = -speed print(speed) commands.set_speed(speed, speed) sleep(1.5) commands.set_speed(0, 0) sleep(1) p_next = commands.where_robot()["center"] diff_magnitude = math.fabs(mathutils.distance(p_init, p_next)) print(diff_magnitude) avg_speed += diff_magnitude / delay print(str(avg_speed / len(delays)) + " px/s") commands.close_connection