def main_loop(node_dict, episode_name, timeout, isdone=None, get_x=None,
get_y=None, get_z=None, get_r=None):
"""Mainloop is common to all mission tasks
modedict is a dictionary of all instrument nodes. one must be the pixhawk.
mainloop will run untill timeout
isdone is a function that will terminate the loop if True
get functions are called to return motor outputs
"""
# mark start time of task
starttime = time()
while True:
# mark start time of loop
currenttime = time()
for key in node_dict.keys():
# update all of the nodes in the node dictionary
node_dict[key].check_readings()
# log data for all of the nodes in the node dictionary
node_dict[key].log(episode_name)
# compute motor updates
if get_x is not None:
vx = get_x(node_dict)
else:
vx = 0
if get_y is not None:
vy = get_y(node_dict)
else:
vy = 0
if get_z is not None:
vz = get_z(node_dict)
else:
vz = 0
if get_r is not None:
vr = get_r(node_dict)
else:
vr = 0
# send motor commands to pixhawk
node_dict['pn'].send_rc(vel_forward=vx,
vel_side=vy,
vel_dive=vz,
vel_turn=vr)
# mission completion check
if isdone(node_dict):
break
# timeout condition
if (currenttime - starttime) > timeout:
break
pause(currenttime, actionperiod)
else:
vx = 0
is get_y is not None:
vy = get_y(node_dict)
else:
vy = 0
is get_z is not None:
vz = get_z(node_dict)
else:
vz = 0
is get_r is not None:
vr = get_r(node_dict)
else:
vr = 0
# send motor commands to pixhawk
node_dict['pn'].send_rc(vel_forward=vx,
vel_side=vy,
vel_dive=vz,
vel_turn=vr)
# mission completion check
if isdone(node_dict):
break
# timeout condition
if (currenttime - starttime) > timeout:
break
pause(currenttime, actionperiod)
def main_loop(node_dict,
episode_name,
timeout,
isdone=None,
get_x=None,
get_y=None,
get_z=None,
get_r=None,
detection_task=None):
"""Mainloop is common to all mission tasks
nodedict is a dictionary of all instrument nodes. one must be the pixhawk.
mainloop will run untill timeout
isdone: function that will terminate the loop if True
get functions: called to return motor outputs. The only approved way to
move the robot around
detection_task: run on the current image and depth map of the zed node
populates the detection list in vision node
"""
# mark start time of task
starttime = time()
while True:
# mark start time of loop
currenttime = time()
for key in node_dict.keys():
# update all of the nodes in the node dictionary
node_dict[key].check_readings()
# update detection list in vision node with recent zed image
if detection_task is not None:
detection_task(node_dict)
for key in node_dict.keys():
# log data for all of the nodes in the node dictionary
node_dict[key].log(episode_name)
# compute motor updates
if get_x is not None:
vx = get_x(node_dict)
else:
vx = 0
if get_y is not None:
vy = get_y(node_dict)
else:
vy = 0
if get_z is not None:
vz = get_z(node_dict)
else:
vz = 0
if get_r is not None:
vr = get_r(node_dict)
else:
vr = 0
# send motor commands to pixhawk
node_dict['pn'].send_rc(vel_forward=vx,
vel_side=vy,
vel_dive=vz,
vel_turn=vr)
# mission completion check
if isdone is not None and isdone(node_dict):
return True
# timeout condition
if (currenttime - starttime) > timeout:
return False
pause(currenttime, actionperiod)
from zoidberg import PixhawkNode, pause
from serial import SerialException
from time import time
update_period = 0.05
# Mac OSX address of pixhawk
#device = '/dev/tty.usbmodem1'
device = '/dev/ttyACM0'
pn = PixhawkNode(device)
# try loop is used to ensure that communication with pixhawk is
# terminated.
try:
# startup data stream
pn.isactive(True)
while True:
loop_start = time()
pn.check_readings()
print("Current heading: %.2f" % pn.heading)
# sleep to maintain constant rate
pause(loop_start, update_period)
except SerialException:
print('Pixhawk is not connected to %s' % device)
finally:
print('Shutting down communication with Pixhawk')
pn.isactive(False)