def move_to_point(goal_pos, current_position, current_angle, magnitude):
distance = mathutils.distance(current_position, goal_pos)
if (distance < 60):
return True
diff = (goal_pos[0] - current_position[0],
goal_pos[1] - current_position[1])
goal_angle = math.atan2(diff[1], diff[0])
angle_diff = mathutils.wrap_angle(goal_angle - current_angle)
print(angle_diff)
angle = angle_diff / (math.pi) + 1
print(angle)
left = magnitude * (angle - 0.5)
right = magnitude * (1.5 - angle)
print(left, right)
commands.set_speed(int(left), int(right))
return False
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
y = position["center"][1]
estimate = prediction.get_estimated_position(states, [x, y, theta],
timestamps)
#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) +
import commands
import sys
import math
import statistics
commands.open_connection(("0.0.0.0", 55555))
commands.set_pid_params(10, 4, 2)
speed_a = int(sys.argv[1])
speed_b = int(sys.argv[2])
commands.set_speed(speed_a, speed_b)
avg_speed_l = 0
avg_speed_r = 0
l_speeds = []
r_speeds = []
for i in range(0, 300):
speeds = commands.get_speed()
speed_l = int(speeds["speed_a"])
speed_r = int(speeds["speed_b"])
avg_speed_l += speed_l
avg_speed_r += speed_r
l_speeds.append(speed_l)
r_speeds.append(speed_r)
avg_speed_l /= 300
avg_speed_r /= 300