def run(drop=True):
nav.setup()
nav.depth(.4)
with sched.Timeout(60) as t:
while True:
print 'Looking for wreath'
nav.vel(.15)
vision.wait_visible(sel, 5)
with mission.State('servo'):
if not servo(sel):
print 'Failed to servo on wreath'
continue
break
if drop:
nav.down(.6)
nav.bk(.2)
sched.sleep(.5)
sub.Grabber.open()
sched.sleep(1)
sub.Grabber.disable()
return True
def run():
nav.setup()
sched.sleep(.5)
nav.depth(3)
print 'Looking for box'
with mission.State('forward'):
nav.vel(.2)
vision.wait_visible(any_box_sel, 5)
print 'Getting closer'
sched.sleep(1)
if box_sels['red'].is_visible():
firstcolor = 'red'
secondcolor = 'blue'
else:
firstcolor = 'blue'
secondcolor = 'red'
if not approach_shoot(firstcolor):
return False
with mission.State('over'):
print 'Going over window'
origdepth = nav.get_waypoint().pos.z
nav.up(1.5)
nav.fd(5)
nav.lturn(180)
nav.depth(origdepth)
if not approach_shoot(secondcolor):
return False
return True
def run():
nav.setup()
nav.depth(1)
with sched.Timeout(60) as t:
while True:
print 'Looking for board'
nav.vel(.2)
vision.wait_visible(board_sel, 5)
sched.sleep(1)
print 'Servoing to board'
if not board_servo(board_sel):
print 'Failed to servo to board'
continue
break
if t.activated:
print 'Timeout on grape board'
return False
print 'Open loop grape approach'
nav.go(x=1, y=-.1, z=.2, rel=True)
push_retry(push_horizontal, 2)
print 'Open loop grape approach'
nav.go(0, .4, -.3, rel=True)
push_retry(push_vertical, 1)
print 'Turning around'
nav.depth(.5)
nav.lturn(180)
def approach_shoot(color):
sel = small_sels[color]
with sched.Timeout(60) as t:
while True:
print 'Forward until small ' + color + ' seen'
nav.vel(.2)
vision.wait_visible(sel, 5)
print 'Servoing for ' + color
if not servo(sel):
continue
break
if t.activated:
print 'Timeout while looking for shooter'
return False
print 'Getting close'
nav.go(.5, .1 if color == 'blue' else -.1, -.15, rel=True, speed=.1)
print 'Shooting'
sched.sleep(2)
shooters[color].shoot()
sched.sleep(1)
print 'Backing up'
nav.bk(.5)
return True
def run(self):
start = time.time()
with open(
'controllertest-%s.csv' %
(time.strftime("%H:%M:%S", time.localtime())), 'w') as f:
f.write(''.join(
['time'] + [', xd%d' % i for i in xrange(6)] +
[', c_tot%d' % i
for i in xrange(6)] + [', c_pd%d' % i for i in xrange(6)] +
[', c_rise%d' % i
for i in xrange(6)] + [', c_nn%d' % i for i in xrange(6)] +
[', v%d%d' % (i, i)
for i in xrange(5)] + [', w%d%d' % (i, i)
for i in xrange(6)]) + '\n')
while True:
sched.sleep(1.0 / self.rate)
v = get_v_weights()
w = get_w_weights()
f.write(
to_csv([time.time() - start] +
list(nav.get_trajectory().pos.xyzRPY) +
list(nav.get_lposvss().pos.xyzRPY) +
list(get_control()) + list(get_pd_control()) +
list(get_rise_control()) + list(get_nn_control()) +
[v[i, i]
for i in xrange(5)] + [w[i, i]
for i in xrange(6)]) + '\n')
def run():
print 'Surfacing'
nav.depth(-.05)
print 'Waiting'
sched.sleep(2)
nav.depth(.4)
sched.sleep(2)
return True
def run():
nav.setup()
print 'Starting gate'
sched.sleep(1)
nav.depth(1)
sched.sleep(.1)
nav.depth(1) # In case of skip first waypoint bug?
# Maybe set heading
nav.fd(11)
return True
def main():
print 'Reading initial magnetometer'
sub.IMU.mag
sched.sleep(1)
for thrusternum in args.thrusters:
thruster = sub.thrusters[thrusternum]
log = Logger(thruster, 50)
sweep(thruster, .2, 1)
log.stop()
print "===Done!"
def dropball(sel):
if not down_servo(sel):
print 'Failed to servo'
return False
print 'Dropping marker!'
nav.depth(2.5)
nav.lstrafe(.025)
sched.sleep(1)
sub.BallDropper.drop()
sched.sleep(.5)
print 'Done!'
def maintask():
print 'Waiting for nav setup'
nav.setup()
print 'Beginning in 2 seconds'
sched.sleep(2)
print 'Strafe square'
nav.depth(1)
nav.fd(2)
nav.rstrafe(2)
nav.bk(2)
nav.lstrafe(2)
def run():
nav.setup()
nav.depth(1.5)
sched.sleep(1)
nav.do_a_barrel_roll()
print 'barrel roll done'
sched.sleep(1)
print 'killed'
sub.kill()
return True
def push_retry(push_func, orig_num):
while True:
push_func()
nav.bk(3.5)
nav.up(.4)
print 'Re-servoing on board'
board_servo(board_sel)
grape_count_sel.setup()
sched.sleep(1)
if len(list(grape_count_sel.get_objects())) == orig_num:
print 'Failed, retrying'
continue
break
def run(self):
with sched.Timeout(random.random() * 10 + 5) as timeout:
while True:
if random.random() < .1:
print self.num, "yield"
sched.yield_()
elif random.random() < .1:
print self.num, "exit"
break
else:
print self.num, "sleep"
sched.sleep(4 * random.random())
if timeout.activated:
print self.num, "timed out"
def log():
print 'Reading initial magnetometer'
sub.IMU.mag
print 'Waiting for %d seconds' % args.wait
sched.sleep(args.wait)
print "Logging"
with sched.Timeout(args.time):
with open(args.file + '.csv', 'w') as f:
while True:
sched.sleep(1.0 / 50)
try:
f.write('%.10e,%.10e,%.10e\n' % sub.IMU.mag)
except dds.Error as e:
pass
print "Done"
def push_horizontal():
print 'Approaching horizontal grape'
nav.vel(.2)
vision.wait_visible(horiz_grape_sel, 2)
print 'Servoing on horizontal grape'
if not grape_servo(horiz_grape_sel):
print 'Failed to servo on horizontal grape'
return False
print 'Open loop'
nav.fd(.6, speed=.05)
print 'Waiting'
sched.sleep(5)
print 'Strafing'
nav.rstrafe(.2, speed=.1)
def push_vertical():
print 'Approaching vertical grape'
nav.vel(.2)
vision.wait_visible(vert_grape_sel, 2)
print 'Servoing on vertical grape'
if not grape_servo(vert_grape_sel):
print 'Failed to servo on vertical grape'
return False
print 'Open loop'
nav.down(.1, speed=.1)
nav.fd(.6, speed=.05)
print 'Waiting'
sched.sleep(5)
print 'Going up'
nav.up(.2, speed=.1)
def maintask():
print 'Waiting for nav setup'
nav.setup()
print 'Beginning in 2 seconds'
sched.sleep(2)
print 'Running'
print 'Starting waypoint', nav.get_waypoint()
nav.depth(1)
t = 0
dt = .2
basepoint = nav.get_waypoint()
while True:
vel = .3 * math.sin(2 * math.pi * t / 25.0)
nav.vel(0, vel)
sched.sleep(dt)
t += dt
def run():
nav.setup()
print 'Going to depth'
nav.depth(.2)
with sched.Timeout(90) as t:
left = True
while True:
(ok, failtype) = allbins()
if ok:
break
if failtype == 'strafe':
# t.cancel()
if left:
nav.lstrafe(.25)
else:
nav.rstrafe(.25)
left = not left
if t.activated:
print 'Timed out looking for all bins'
return False
nav.down(.2)
center_pos = nav.get_trajectory().pos
print 'Center Pos:', center_pos
dropball(bin1_sel)
print 'Returning to center...'
nav.set_waypoint(nav.Waypoint(center_pos), speed=.3)
nav.wait()
sched.sleep(1.5)
print 'Done!'
dropball(bin2_sel)
print 'Returning to center...'
waypoint = nav.Waypoint(center_pos)
waypoint.pos.z = 2.5
nav.set_waypoint(waypoint)
nav.wait()
return True
def run(self, selector):
self.setup()
selector.setup()
sched.sleep(.5)
failctr = 0
while True:
wait()
obj = selector.get_object()
if obj is None:
failctr += 1
if failctr > 20:
return False
else:
continue
failctr = 0
if self.update(obj):
break
nav.stop()
return True
def try_grab(down_dist):
sub.Grabber.open()
print 'Servoing'
with mission.State('servo'):
if not servo(sel):
return False
print 'Open loop grab'
with mission.State('grab'):
nav.bk(.2)
nav.down(down_dist)
sched.sleep(.5)
sub.Grabber.close()
sched.sleep(1)
nav.up(down_dist)
for i in xrange(
3
): # Ugly, multiple attempts because DDS topics are lazily created
sched.sleep(.5)
if sub.Grabber.closed:
print 'Missed'
return False
print 'Success!'
return True
def run(practice):
nav.setup()
with mission.State('hydro1'):
if not hydrophone.run(practice):
return False
with mission.State('wreath_grab'):
if not wreath_grab.run():
print 'Failed to grab'
occluded_sel.setup()
sched.sleep(.5)
if occluded_sel.get_object() != None:
print 'Vision occluded'
occluded = True
else:
occluded = False
if practice:
print 'Practice mission, dropping'
sub.Grabber.open()
return True
with mission.State('hydro2'):
if not hydrophone.run(practice):
return False
if not occluded:
with mission.State('drop'):
wreath_drop.run()
else:
print 'Occluded open loop drop'
nav.fd(2)
nav.depth(2)
sub.Grabber.open()
return True
def sweep(thruster, ramptime, holdtime):
print "===Beginning thruster %d" % (thruster.num)
sched.sleep(1)
for i in xrange(0, 20):
start = i/20.0
end = (1+i)/20.0
ramp(thruster, start, end, ramptime)
print "%d" % (end*100)
sched.sleep(holdtime)
ramp(thruster, 1, 0, 2)
sched.sleep(1)
for i in xrange(0, 20):
start = -i/20.0
end = -(i+1)/20.0
ramp(thruster, start, end, ramptime)
print "%d" % (end*100)
sched.sleep(holdtime)
ramp(thruster, -1, 0, 2)
def main():
print 'Search pattern test'
print 'Waiting for sub unkilled'
sub.wait_unkilled()
nav.setup()
print 'Running'
log = logger.Logger(20)
sched.sleep(.2)
nav.depth(1) # We still somehow lose the first waypoint....
sched.sleep(.2)
nav.depth(1)
sched.Task('nn_enable', enable_nn_delayed)
for i in xrange(PASSES):
print 'Beginning pass %d/%d' % (i + 1, PASSES)
nav.fd(FD_DIST)
nav.rturn(90)
nav.fd(SIDE_DIST)
nav.rturn(90)
nav.fd(FD_DIST)
nav.lturn(90)
nav.fd(SIDE_DIST)
nav.lturn(90)
print 'Surfacing'
nav.depth(.1)
log.stop()
print 'Killing'
sub.kill()
sched.sleep(.5)
print 'Done'
while True:
sched.sleep(1)
def log():
with open('depth.csv', 'w') as f:
while True:
sched.sleep(1.0 / 50)
f.write('%f' % sub.DepthSensor.depth)
def enable_nn_delayed():
sched.sleep(3)
print 'Enabled neural network'
sub.set_controller_mode('rise_nn')
def run(self):
with open('%s_thruster_%d.csv' % (args.prefix, self.thruster.num), 'w') as f:
while True:
sched.sleep(1.0/self.rate)
f.write('%.10e,%.10e,%.10e,%.10e,%.10e\n' % (sub.IMU.mag + (self.thruster.effort, self.thruster.current)))
def ramp(thruster, start, end, time):
for i in xrange(0, 20):
thruster.effort = start + (end-start)*(i/20.0)
sched.sleep(time/20.0)
thruster.effort = end
