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