def boot_tune(ser):
# Plays a basic tune using the motors to signal the system is ready
for _ in range(5):
send_cmd(ser, 10, 0, 0)
time.sleep(0.2)
send_cmd(ser, 0, 0, 0)
time.sleep(0.2)
def send_cmd(x, y, psi):
if sock is not None:
udp_comm.send_cmd(sock, 2, x, y, psi)
else:
serial_comm.send_cmd(ser, x, y, psi)
ser = open_serial()
boot_tune(ser)
v_x = 0
v_y = 0
while True:
try:
# Read and parse command
data, addr = sock.recvfrom(COMMAND_SIZE)
(flag, f_x, f_y, psi) = struct.unpack('=Bddd', data)
if flag == 1:
# Forward euler's method
# TODO That's for later
v_x += f_x * MASS
v_y += f_y * MASS
print("Using acceleration")
elif flag == 2:
print("Using velocity")
v_x = f_x
v_y = f_y
send_cmd(ser, v_x, v_y, psi)
except BlockingIOError:
# Socket timed out, stopping motors
send_cmd(ser, 0, 0, 0)
except KeyboardInterrupt:
print()
send_cmd(ser, 0, 0, 0)
break