currentPos['x'], currentPos['y'], currentPos['z'], desiredPos['x'], desiredPos['y'], desiredPos['z'], \
udp.message[8], udp.message[9], udp.message[10], \
udp.message[7], \
rcCMD[0], rcCMD[1], rcCMD[2], rcCMD[3])
if logging:
logger.writerow(row)
print "Mode: %s | Z: %0.3f | X: %0.3f | Y: %0.3f " % (
mode, currentPos['z'], currentPos['x'], currentPos['y'])
#print "Mode: %s | heading: %0.3f | desiredYaw: %0.3f" % (mode, heading, desiredYaw)
# Wait until the update_rate is completed
while elapsed < update_rate:
elapsed = time.time() - current
except Exception, error:
print "Error in control thread: " + str(error)
if __name__ == "__main__":
try:
logThread = threading.Thread(target=control)
logThread.daemon = True
logThread.start()
udp.startTwisted()
except Exception, error:
print "Error on main: " + str(error)
vehicle.ser.close()
except KeyboardInterrupt:
print "Keyboard Interrupt, exiting."
exit()
currentPos['x'], currentPos['y'], currentPos['z'], desiredPos['x'], desiredPos['y'], desiredPos['z'], \
udp.message[11], udp.message[12], udp.message[13], \
udp.message[4], \
rcCMD[0], rcCMD[1], rcCMD[2], rcCMD[3], \
udp.message[8], udp.message[9], udp.message[10], udp.message[14],udp.message[15], udp.message[16], \
vehicle.motor['m1'], vehicle.motor['m2'], vehicle.motor['m3'], vehicle.motor['m4'] )
if logging:
logger.writerow(row)
print "Mode: %s | X: %0.3f | Y: %0.3f | Z: %0.3f | Heading: %0.3f" % (mode, currentPos['x'], currentPos['y'], currentPos['z'], heading)
#print "Mode: %s | heading: %0.3f | desiredYaw: %0.3f" % (mode, heading, desiredYaw)
# Wait until the update_rate is completed
while elapsed < update_rate:
elapsed = time.time() - current
except Exception,error:
print "Error in control thread: "+str(error)
if __name__ == "__main__":
try:
logThread = threading.Thread(target=control)
logThread.daemon=True
logThread.start()
udp.startTwisted()
except Exception,error:
print "Error on main: "+str(error)
vehicle.ser.close()
except KeyboardInterrupt:
print "Keyboard Interrupt, exiting."
exit()
