y = data["pos"][1] # y+ -> pitch+
z = data["pos"][2] # z+ -> thrust+
angle = data["angle"]
detected = data["detect"]
if detected:
last_detect_ts = time.time()
#x_r = (x/f_x) * z
#y_r = (y/f_y) * z
try:
while True:
md = midi_conn.recv_json(zmq.NOBLOCK)
print(md["knobs"][0] - 0.5) * 2
r_pid.set_point = (md["knobs"][0] - 0.5) * 2
p_pid.set_point = (md["knobs"][1] - 0.5) * 2
y_pid.set_point = md["knobs"][2] * 360
rp_p = md["sliders"][0] * 40
rp_i = md["sliders"][1]
rp_d = md["sliders"][2] * 200
#r_pid.Kp = rp_p
#p_pid.Kp = rp_p
#r_pid.Ki = rp_i
#p_pid.Ki = rp_i
#r_pid.Kd = rp_d
#p_pid.Kd = rp_d
roll = data["angle"][0]
detected = data["detect"]
if detected:
last_detect_ts = time.time()
#x_r = (x/f_x) * z
#y_r = (y/f_y) * z
try:
while True:
md = midi_conn.recv_json(zmq.NOBLOCK)
print (md["knobs"][0] - 0.5) * 2
r_pid.set_point = (md["knobs"][0]-0.5) * 2
p_pid.set_point = (md["knobs"][1]-0.5) * 2
y_pid.set_point = md["knobs"][2] * 360
rp_p = md["sliders"][0] * 40
rp_i = md["sliders"][1]
rp_d = md["sliders"][2] * 200
#r_pid.Kp = rp_p
#p_pid.Kp = rp_p
#r_pid.Ki = rp_i
#p_pid.Ki = rp_i
#r_pid.Kd = rp_d
#p_pid.Kd = rp_d
Check if body is being tracked by cameras
"""
detected = unpackd[-1]
if detected:
detect_ts = int(round(time.time() * 1000))
delta = unpackd[-2]
else:
print("Not Tracking!!")
# Get the set-points (if there are any)
try:
while True:
ctrl_sp = ctrl_conn.recv_json(zmq.NOBLOCK)
yaw_sp = ctrl_sp["set-points"]["yaw"]
r_pid.set_point = ctrl_sp["set-points"]["roll"]
p_pid.set_point = ctrl_sp["set-points"]["pitch"]
midi_acc = ctrl_sp["set-points"]["velocity"]
except zmq.error.Again:
pass
#print "RP P/I/D={}/{}/{}".format(rp_p, rp_i, rp_d)
x_r = (x/f_x) * z
y_r = (y/f_y) * z
"""
Run the controller if we are getting a frame rate better than 100fps. Do not run if we are running faster than
~130 fps
"""
step = detect_ts - last_detect_ts
if (max_step > step > min_step) and detected:
