from robot import get_robot_position, move_to_point
from commands import get_speed, where_robot, where_markers, where_all
from prediction import get_estimated_position
from math import atan2
from time import sleep
import commands
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']
import commands
import robot
import random
import sys
import math
import mathutils
import prediction
from time import sleep
commands.open_connection(("0.0.0.0", 55555))
#commands.set_pid_params(10, 4, 2)
states = []
timestamps = []
estimate = []
position = {}
predictions = []
markers = commands.where_markers()
goal = markers["40"]["center"]
#have robot follow random vectors, updating predicted position along the way
for i in range(0, int(sys.argv[1])):
motor_speeds = commands.get_speed()
positions = commands.where_all()
timestamp = positions["time"]
position = positions["robot"]
(speed_l, speed_r) = (motor_speeds["speed_a"], motor_speeds["speed_b"])
#right motor speed, left motor speed, and motor speed diff
state = [speed_r, speed_l, speed_r - speed_l]