def command_abort():
gpio.set_com()
execute = ['o', 'a', 1, 1, 1, 1, 1 ,1]
logger.info("Aborting...")
comserial.write_array(execute)
time.sleep(0.01)
logger.info(comserial.read_array())
gpio.unset_com()
def command_ping():
gpio.set_com()
execute = ['p',1,1,1,1,1,1,1]
logger.info("Sending ping");
comserial.write_array(execute)
time.sleep(0.01)
logger.info(comserial.read_array())
gpio.unset_com()
def command_odometry_ping():
gpio.set_com()
execute = ['o', 'h', 1, 1, 1, 1, 1, 1]
logger.info("Sending ping to odometry")
comserial.write_array(execute)
time.sleep(0.01)
logger.info(comserial.read_array())
gpio.unset_com()
def command_startexec():
gpio.set_com()
execute = ['s', 'e', 1, 1, 1, 1, 1, 1]
logger.info("Sending " + str(execute))
comserial.write_array(execute)
time.sleep(0.01)
logger.info(comserial.read_array())
logger.info("Starting to execute code...")
gpio.unset_com()
def command_get_info():
gpio.set_com()
execute = ['o', 'd', 1, 1, 1, 1, 1, 1]
logger.info("Sending get info")
comserial.write_array(execute)
time.sleep(0.01)
recv = comserial.read_array()
x = (recv[0] << 8) | recv[1]
y = (recv[2] << 8) | recv[3]
angle = (recv[4] << 8) | recv[5]
state = chr(recv[6])
spd = recv[7]
logger.info("X: " + str(x) +
"Y: " + str(y) +
"Angle: " + str(angle) +
"State: " + str(state) +
"Speed: " + str(spd))
gpio.unset_com()