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