import serial
from backend.interface import Interface
#setup serial connection
arduino = Interface()
#temp robot setup
max_speed = 40 #in/s
while True:
in_left_speed = input("Enter left speed: ")
in_right_speed = input("Enter right speed: ")
print(' ')
arduino.sendWheelSpeeds(in_left_speed, in_right_speed)
import serial
from backend.interface import Interface
from backend.gen_suite import Pose, Path, Trajectory, Robot
import time
arduino = Interface()
waypoints = [Pose(0,0,0), Pose(70, 30, 90)]
path = Path(waypoints)
robot = Robot(190 * 0.03937, 20, 5)
trajectory = Trajectory(robot, path)
traj_time = time.time()
total_traj_time = trajectory.trajectory[-1].time
while (time.time() - traj_time) <= total_traj_time:
t = time.time() - traj_time
state = trajectory.sample(t)
print(state.curvature)
left_speed, right_speed = robot.get_wheel_speeds_from_state(state)
arduino.sendWheelSpeeds(left_speed, right_speed)
print('FINISHED')
