def joystick(): """ Function to update commands and attitude to be called by a thread. """ try: while True: if udp.active: current = time.time() elapsed = 0 # Part for applying commands to the vehicle. # Channel order in mavlink: roll, pitch, throttle, yaw # Channel order in optitrack: roll, pitch, yaw, throttle roll = udp.message[0] pitch = utils.mapping(udp.message[1],1000,2000,2000,1000) # To invert channel, maybe add function throttle = utils.mapping(udp.message[3],1000,2000,968,1998) # Map it to match RC configuration yaw = utils.mapping(udp.message[2],1000,2000,968,2062) # Map it to match RC configuration vehicle.channel_override = { "1" : roll, "2" : pitch, "3" : throttle, "4" : yaw } vehicle.flush() #print "%s" % vehicle.attitude print "%s" % vehicle.channel_readback #print modules.UDPserver.message # 100hz loop while elapsed < 0.01: elapsed = time.time() - current # End of the main loop except Exception,error: print "Error on joystick thread: "+str(error) joystick()
def boxAngle(): global board, canvas, a, c, b, running canvas.delete(a) canvas.delete(c) canvas.delete(b) board.getData(MultiWii.ATTITUDE) running = 1 #attitude = {'angx':0,'angy':0,'heading':0,'elapsed':0,'timestamp':0} arcStart = board.attitude['angx']+arcwidth+90 arcEnd = board.attitude['angx']-arcwidth+90 #r = int(utils.limit(utils.mapping(board.attitude['angx'], -90, 10, 255, 0),0,255)) #g = int(utils.limit(utils.mapping(board.attitude['angx'], -20, 20, 200, 255),0,255)) #b = int(utils.limit(utils.mapping(board.attitude['angx'], -10, 90, 0, 255),0,255)) r = int(utils.limit(utils.mapping(board.attitude['angx'], -100, 10, 255, 0),0,255)) g = int(utils.limit(utils.mapping(board.attitude['angx'], -20, 20, 200, 255),0,255)) b = int(utils.limit(utils.mapping(board.attitude['angx'], -10, 100, 0, 255),0,255)) #print "angle= %0.2f r= %d g= %d b= %d" % (board.attitude['angx'],r,g,b) #canvas.create_circle_arc(100, 120, 50, fill="black", style="arc", outline=utils.rgb_to_hex((r, g, b)), width=10, start=0, end=180) c = canvas.create_circle(sizeX/2, sizeY/2, circleradius, fill="black", outline=utils.rgb_to_hex((r, g, b)), width=ringwidth) a = canvas.create_circle_arc(sizeX/2, sizeY/2, arcradius, style="arc", outline="white", width=7, start=arcStart, end=arcEnd) b = canvas.create_circle(sizeX/2, sizeY/2, 30, fill="#ED1F24") t = canvas.create_text(sizeX/2, sizeY/2,text="GO", fill="white", font=("Helvetica", 30)) canvas.tag_bind(b, "<ButtonPress-1>", click) canvas.tag_bind(t, "<ButtonPress-1>", click) tk.after(150, boxAngle)
def boxAngle(): global board, canvas, a, c, b, running canvas.delete(a) canvas.delete(c) canvas.delete(b) board.getData(MultiWii.ATTITUDE) running = 1 #attitude = {'angx':0,'angy':0,'heading':0,'elapsed':0,'timestamp':0} arcStart = board.attitude['angx'] + arcwidth + 90 arcEnd = board.attitude['angx'] - arcwidth + 90 #r = int(utils.limit(utils.mapping(board.attitude['angx'], -90, 10, 255, 0),0,255)) #g = int(utils.limit(utils.mapping(board.attitude['angx'], -20, 20, 200, 255),0,255)) #b = int(utils.limit(utils.mapping(board.attitude['angx'], -10, 90, 0, 255),0,255)) r = int( utils.limit(utils.mapping(board.attitude['angx'], -100, 10, 255, 0), 0, 255)) g = int( utils.limit(utils.mapping(board.attitude['angx'], -20, 20, 200, 255), 0, 255)) b = int( utils.limit(utils.mapping(board.attitude['angx'], -10, 100, 0, 255), 0, 255)) #print "angle= %0.2f r= %d g= %d b= %d" % (board.attitude['angx'],r,g,b) #canvas.create_circle_arc(100, 120, 50, fill="black", style="arc", outline=utils.rgb_to_hex((r, g, b)), width=10, start=0, end=180) c = canvas.create_circle(sizeX / 2, sizeY / 2, circleradius, fill="black", outline=utils.rgb_to_hex((r, g, b)), width=ringwidth) a = canvas.create_circle_arc(sizeX / 2, sizeY / 2, arcradius, style="arc", outline="white", width=7, start=arcStart, end=arcEnd) b = canvas.create_circle(sizeX / 2, sizeY / 2, 30, fill="#ED1F24") t = canvas.create_text(sizeX / 2, sizeY / 2, text="GO", fill="white", font=("Helvetica", 30)) canvas.tag_bind(b, "<ButtonPress-1>", click) canvas.tag_bind(t, "<ButtonPress-1>", click) tk.after(150, boxAngle)
def click(event): global b, update, running, printer, events, board canvas.delete(b) b = canvas.create_circle(sizeX / 2, sizeY / 2, 30, fill="red", outline="white", width=1) if board.attitude['angx'] == 0.0: valor = random.randint(0, len(events) - 1) else: valor = int( utils.limit( utils.mapping(board.attitude['angx'], -110, 110, 0, len(events) - 1), 0, len(events) - 1)) print "\nStart printing... ang %0.2f, value %d" % (board.attitude['angx'], valor) try: url = events[valor][3] except: url = "No URL... sorry" message = "%s\nOn %s\n%s\nMore info: %s\n" % ( events[valor][1], events[valor][0], events[valor][2], url) print message try: printer.justify("C") printer.inverse_on() printer.bold_on() printer.print_text("TIME BOX") printer.inverse_off() printer.print_text("\n______________\n") printer.bold_off() printer.print_text("According to:\n") printer.bold_on() printer.print_text(str(board.attitude['angx'])) printer.print_text(" degrees") printer.bold_off() printer.print_text("\nof inclination\n______________\n\n") printer.bold_on() printer.print_text(events[valor][1]) printer.bold_off() printer.print_text("\n") printer.print_text(events[valor][0]) printer.print_text("\n\n") printer.print_text(events[valor][2]) printer.print_text("\n\nMore info:\n") printer.print_text(events[valor][3]) printer.print_text("\n\n:)\n____________________________\n\n\n\n\n\n") except: print "Error while printing, perhaps no printer attached."
def click(event): global b, update, running, printer, events, board canvas.delete(b) b = canvas.create_circle(sizeX/2, sizeY/2, 30, fill="red", outline="white", width=1) if board.attitude['angx'] == 0.0: valor = random.randint(0,len(events)-1) else: valor = int(utils.limit(utils.mapping(board.attitude['angx'],-110,110,0,len(events)-1),0,len(events)-1)) print "\nStart printing... ang %0.2f, value %d" % (board.attitude['angx'],valor) try: url = events[valor][3] except: url = "No URL... sorry" message = "%s\nOn %s\n%s\nMore info: %s\n" % (events[valor][1],events[valor][0],events[valor][2],url) print message try: printer.justify("C") printer.inverse_on() printer.bold_on() printer.print_text("TIME BOX") printer.inverse_off() printer.print_text("\n______________\n") printer.bold_off() printer.print_text("According to:\n") printer.bold_on() printer.print_text(str(board.attitude['angx'])) printer.print_text(" degrees") printer.bold_off() printer.print_text("\nof inclination\n______________\n\n") printer.bold_on() printer.print_text(events[valor][1]) printer.bold_off() printer.print_text("\n") printer.print_text(events[valor][0]) printer.print_text("\n\n") printer.print_text(events[valor][2]) printer.print_text("\n\nMore info:\n") printer.print_text(events[valor][3]) printer.print_text("\n\n:)\n____________________________\n\n\n\n\n\n") except: print "Error while printing, perhaps no printer attached."
def sendCommands(): """ Function to update commands and attitude to be called by a thread. """ try: while True: if udp.active: # Timers current = time.time() elapsed = 0 # Part for applying commands to the vehicle. # Channel order in mavlink: roll, pitch, throttle, yaw # Channel order in optitrack: roll, pitch, yaw, throttle # Joystick manual commands: roll = udp.message[0] pitch = utils.mapping(udp.message[1], 1000, 2000, 2000, 1000) # To invert channel, maybe add function yaw = utils.mapping(udp.message[2], 1000, 2000, 968, 2062) # Map it to match RC configuration throttle = utils.mapping(udp.message[3], 1000, 2000, 968, 1998) # Map it to match RC configuration # Simulink automatic control: if udp.message[7] == 1: roll = udp.message[12] pitch = utils.mapping( udp.message[13], 1000, 2000, 2000, 1000 ) # To invert channel, maybe add function yaw = utils.mapping(udp.message[14], 1000, 2000, 968, 2062) # Map it to match RC configuration throttle = utils.mapping(udp.message[15], 1000, 2000, 968, 1998) # Map it to match RC configuration # Vehicle communication vehicle.channel_override = {"1": roll, "2": pitch, "3": throttle, "4": yaw} vehicle.flush() # Debug # print "%s" % vehicle.attitude # print "%s" % vehicle.channel_readback print udp.message # 100hz loop while elapsed < 0.01: elapsed = time.time() - current # End of the main loop except Exception, error: print "Error on sendCommands thread: " + str(error) sendCommands()