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)
