def main(args): com = Communicator(args.module_name) controller = Controller( ["X1", "Y1", "X2", "Y2", "R2", "L2"], ["right/left", "forward/backward", "yaw", "pitch", "up", "down"], (0, 255), (-1, 1)) while True: control_packet = controller.get_values() try: outgoing_packet = { "right/left": 0.0, "forward/backward": 0.0, "yaw": 0.0, "pitch": 0.0, "up/down": 0.0, "roll": 0.0 } # Further parse controller values here # Controller's sticks Y axis are switched control_packet[ "forward/backward"] = -control_packet["forward/backward"] control_packet["pitch"] = -control_packet["pitch"] # Up and Down are not -1 to 1. Just 0 - 1 control_packet["up"] = controller.map_range( control_packet["up"], -1, 1, 0, 1) control_packet["down"] = controller.map_range( control_packet["down"], -1, 1, 0, -1) # Transferring to outgoing packet outgoing_packet["forward/backward"] = control_packet[ "forward/backward"] outgoing_packet["right/left"] = control_packet["right/left"] outgoing_packet[ "up/down"] = control_packet["up"] + control_packet["down"] outgoing_packet["yaw"] = control_packet["yaw"] outgoing_packet["pitch"] = control_packet["pitch"] #outgoing_packet["roll"] = control_packet["roll"] outgoing_packet["roll"] = 0.0 # Controller sticks are not centered very well. # TODO: Find a better way to do this (short of getting a new controller) for key in outgoing_packet.keys(): if abs(outgoing_packet[key]) < .10: outgoing_packet[key] = 0.0 print outgoing_packet Fuzzy_Sets = {"Fuzzy_Sets": outgoing_packet} com.publish_message(Fuzzy_Sets) except KeyError as i: pass sleep(args.epoch)
def main (): # Get input from the two analog sticks as yaw, throttle, roll, and pitch. Take the (0 - 255) input value and # map it to a (-1 - 1) range. controller = Controller (["X1", "Y1", "X2", "Y2"], ["yaw", "throttle", "roll", "pitch"], (0, 255), (-1, 1)) while True: control_packet = controller.get_values () print control_packet # Update at 20 messages a second sleep (.05)
def main (): # Get input from the two analog sticks as yaw, throttle, roll, and pitch. Take the (0 - 255) input value and # map it to a (-1 - 1) range. controller = Controller (["X1", "Y1", "X2", "Y2", "L2", "R2", "X", "/\\", "[]"], ["yaw", "throttle", "roll", "pitch", "descend", "ascend", "takeover", "takeoff", "land"], (0, 255), (-1, 1)) #controller = Controller (["X1", "Y1", "X2", "Y2"]) while True: control_packet = controller.get_values () os.system("clear") for i in control_packet: print i, ": ", control_packet[i] # Update at 1000 messages a second sleep (.01)
def main (): # Get input from the two analog sticks as yaw, throttle, roll, and pitch. Take the (0 - 255) input value and # map it to a (-1 - 1) range. controller = Controller (["X1", "Y1", "X2", "Y2"], ["yaw", "throttle", "roll", "pitch"], (0, 255), (-1, 1)) #ser = serial.Serial ('/./dev/ttyACM0', 9600, timeout=1) # Set the context of the server context = zmq.Context() # Set the socket to PUBlish socket = context.socket(zmq.PUB) # Bind the socket to 5556 socket.bind("tcp://*:5556") while True: control_packet = controller.get_values () print (control_packet) inp = 0 if (control_packet["yaw"] <= 1 and control_packet["yaw"] >= 0.05): inp *= 255 inp += 255 # ser.write (inp) print ("d") socket.send("%i"% inp) elif (control_packet["yaw"] >= -1 and control_packet["yaw"] <= -0.05): inp *= 255 inp -= 255 #ser.write (inp) print ("a") socket.send("%i"% inp) elif (control_packet["throttle"] >= -1 and control_packet["throttle"] <= -0.08): inp *= 255 #make the value intuitive inp = math.fabs(inp) # ser.write (inp) print ("w") socket.send("%i"% inp) elif (control_packet["throttle"] <= 1 and control_packet["throttle"] >= 0.08): inp *= 255 #make the value intuitive inp = -inp # ser.write (inp) print ("s") socket.send("%i"% inp) # Update at 20 messages a second sleep (.05)
def main (args): com = Communicator (args.module_name) controller = Controller (["X1", "Y1", "X2", "Y2", "R2", "L2"], ["right/left", "forward/backward", "yaw", "pitch", "up", "down"], (0, 255), (-1, 1)) while True: control_packet = controller.get_values () try: outgoing_packet = {"right/left": 0.0, "forward/backward": 0.0, "yaw": 0.0, "pitch": 0.0, "up/down": 0.0, "roll": 0.0} # Further parse controller values here # Controller's sticks Y axis are switched control_packet["forward/backward"] = -control_packet["forward/backward"] control_packet["pitch"] = -control_packet["pitch"] # Up and Down are not -1 to 1. Just 0 - 1 control_packet["up"] = controller.map_range(control_packet["up"], -1, 1, 0, 1) control_packet["down"] = controller.map_range(control_packet["down"], -1, 1, 0, -1) # Transferring to outgoing packet outgoing_packet["forward/backward"] = control_packet["forward/backward"] outgoing_packet["right/left"] = control_packet["right/left"] outgoing_packet["up/down"] = control_packet["up"] + control_packet["down"] outgoing_packet["yaw"] = control_packet["yaw"] outgoing_packet["pitch"] = control_packet["pitch"] #outgoing_packet["roll"] = control_packet["roll"] outgoing_packet["roll"] = 0.0 # Controller sticks are not centered very well. # TODO: Find a better way to do this (short of getting a new controller) for key in outgoing_packet.keys (): if abs (outgoing_packet[key]) < .10: outgoing_packet[key] = 0.0 print outgoing_packet Fuzzy_Sets = {"Fuzzy_Sets": outgoing_packet} com.publish_message (Fuzzy_Sets) except KeyError as i: pass sleep (args.epoch)
def main (): # Get input from the two analog sticks as yaw, throttle, roll, and pitch. Take the (0 - 255) input value and # map it to a (-1 - 1) range. controller = Controller (["X1", "Y1", "X2", "Y2"], ["yaw", "throttle", "roll", "pitch"], (0, 255), (-1, 1)) ser = serial.Serial ('/./dev/ttyACM0', 9600, timeout=1) while True: control_packet = controller.get_values () print (control_packet) input = 0 if (control_packet['yaw'] <= 1 and control_packet['yaw'] >= 0.05): input *= 255 input += 255 ser.write (input) print ("d") elif (control_packet['yaw'] >= -1 and control_packet['yaw'] <= -0.05): input *= 255 input -= 255 ser.write (input) print ("a") elif (control_packet['throttle'] >= -1 and control_packet['throttle'] <= -0.08): input *= 255 #make the value intuitive input = math.fabs(input) ser.write (input) print ("w") elif (control_packet['throttle'] <= 1 and control_packet['throttle'] >= 0.08): input *= 255 #make the value intuitive input = -input ser.write (input) print ("s") # Update at 20 messages a second sleep (.05)
def main (): com = communicator ("Controller") final_packet = {"Pitch":0, "Yaw":0, "Roll":0, "Z":0, "Arm":0, "Stabalize":0} controller = Controller (["X1", "Y1", "X2", "Y2", "[]", "/\\", "start", "R2", "L2"], ["Yaw", "Z", "Roll", "Pitch", "Unarm", "Arm", "Calibrate", "Stabalize1", "Stabalize2"]) IS_ARMED = 0 IS_STABALIZED = 0 CALIBRATE = 0 while True: inputs = controller.get_values () try: # Buttons (0 - 100) inputs["Arm"] = controller.map_range (inputs["Arm"], 0, 255, 0.0, 100.0) inputs["Unarm"] = controller.map_range (inputs["Unarm"], 0, 255, 0.0, 100.0) inputs["Calibrate"] = controller.map_range (inputs["Calibrate"], 0, 1, 0.0, 100.0) inputs["Stabalize1"] = controller.map_range (inputs["Stabalize1"], 0, 255, 0.0, 100.0) inputs["Stabalize2"] = controller.map_range (inputs["Stabalize2"], 0, 255, 0.0, 100.0) # Throttle (0 - 100) inputs["Z"] = controller.map_range (inputs["Z"], 0, 255, 100.0, 0.0) / (1.0 / SENSITIVITY) # Reversed # Directions (-100 - 100) inputs["Yaw"] = controller.map_range (inputs["Yaw"], 0, 255, -100.0, 100.0) / (1.0 / SENSITIVITY) inputs["Pitch"] = controller.map_range (inputs["Pitch"], 0, 255, 100.0, -100.0) / (1.0 / SENSITIVITY) # Reversed inputs["Roll"] = controller.map_range (inputs["Roll"], 0, 255, -100.0, 100.0) / (1.0 / SENSITIVITY) # Have to press hard to prevent accidental arm/unarm/throttle helper/throttle hold if inputs["Unarm"] == 100.0: inputs["Unarm"] = 100.0 else: inputs["Unarm"] = 0 if inputs["Arm"] == 100.0: inputs["Arm"] = 100.0 else: inputs["Arm"] = 0 if inputs["Stabalize1"] == 100.0: inputs["Stabalize1"] = 100.0 else: inputs["Stabalize1"] = 0 if inputs["Stabalize2"] == 100.0: inputs["Stabalize2"] = 100.0 else: inputs["Stabalize2"] = 0 if inputs["Calibrate"] == 100.0: inputs["Calibrate"] = 100.0 else: inputs["Calibrate"] = 0 if inputs["Stabalize1"] and inputs["Stabalize2"]: IS_STABALIZED = 1 else: IS_STABALIZED = 0 if inputs["Calibrate"]: CALIBRATE = 1 else: CALIBRATE = 0 # Logic so you don't have to hold arm/unarm/etc if IS_ARMED and inputs["Unarm"]: inputs["Arm"] = 0 IS_ARMED = 0 print "UNARMED!" if not IS_ARMED and inputs["Arm"]: inputs["Arm"] = 1 IS_ARMED = 1 print "ARMED!" for key in inputs.keys (): if abs (inputs[key]) < 10: inputs[key] = 0.0 final_packet = {"Pitch":inputs["Pitch"], "Yaw":inputs["Yaw"], "Roll":inputs["Roll"], "Z":inputs["Z"], "Arm":IS_ARMED, "Stabalize":IS_STABALIZED, "Calibrate": CALIBRATE} # Clips small controller movements to zero. Necessary especially for throttle so copter will arm com.send_message (final_packet) except KeyError: pass print final_packet # Send 5 commands/sec sleep (.1)