def make_buoy_sel(name, hue):
return vision.Selector(
vision.FORWARD_CAMERA, 'buoy/' + name,
vision.FilterScore(lambda obj: buoy_score(obj, hue),
min_score=0,
debug=True))
import dds
from subjugator import nav, sched, vision, sub
from missionplanner import mission
BIN_1 = 'trident'
BIN_2 = 'net'
servo = vision.BottomVisualServo(kx=.4, ky=.4, debug=True)
down_servo = vision.BottomVisualServo(kx=.4,
ky=.4,
kz=.00004,
zmax=.2,
desired_scale=16000,
debug=True)
allbins_sel = vision.Selector(vision.DOWN_CAMERA, 'bins/all')
bin1_sel = vision.Selector(vision.DOWN_CAMERA, 'bins/single',
vision.FilterCompare('item', '__eq__', BIN_1))
bin2_sel = vision.Selector(vision.DOWN_CAMERA, 'bins/single',
vision.FilterCompare('item', '__eq__', BIN_2))
@mission.State('dropball')
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)
from subjugator import sched, nav, vision
from missionplanner import mission
import math
import dds
servo = vision.StrafeVisualServo(fastvel=.3,
slowscale=140,
slowvel=.2,
maxscale=175,
ky=.4,
kz=.4,
yztol=.05,
debug=True)
hedge_sel = vision.Selector(vision.FORWARD_CAMERA, 'hedge')
def run_hedge_fix_right():
nav.setup()
nav.depth(2)
nav.set_waypoint_rel(nav.make_waypoint(y=1.5, x=-.5, Y=math.radians(-10)))
nav.wait()
# nav.rstrafe(1)
# nav.lturn(20)
return True
def run_hedge_fix_left():
nav.setup()
nav.depth(2)
from subjugator import sched, nav, vision, sub
from missionplanner import mission
import math
import dds
servo = vision.BottomVisualServo(kx=.2,
ky=.2,
kz=.004,
zmax=.1,
desired_scale=230,
debug=True)
sel = vision.Selector(vision.DOWN_CAMERA, 'wreath')
@mission.State('try_grab {:.2}')
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()
from subjugator import sched, nav, sub, vision
from missionplanner import mission
import math
import dds
servo = vision.StrafeVisualServo(fastvel=.3,
slowscale=10000,
slowvel=.05,
maxscale=55000,
ky=.15,
kz=.15,
debug=True)
small_sels = dict(red=vision.Selector(vision.FORWARD_CAMERA,
'shooter/red/small'),
blue=vision.Selector(vision.FORWARD_CAMERA,
'shooter/blue/small'))
box_sels = dict(red=vision.Selector(vision.FORWARD_CAMERA,
'shooter/red/small'),
blue=vision.Selector(vision.FORWARD_CAMERA,
'shooter/blue/small'))
shooters = dict(red=sub.RightShooter, blue=sub.LeftShooter)
any_box_sel = vision.combine_selectors(list(box_sels.itervalues()))
@mission.State('approach_shoot {}')
def approach_shoot(color):
sel = small_sels[color]
from subjugator import sched, nav, vision
from missionplanner import mission
board_servo = vision.StrafeVisualServo(fastvel=.3, slowscale=60000, slowvel=.1, maxscale=85000, ky=.5, kz=.5, yztol=.025, debug=True)
grape_servo = vision.StrafeVisualServo(fastvel=.2, slowscale=40, slowvel=.05, maxscale=80, ky=.1, kz=.1, yztol=.005, debug=True)
board_sel = vision.Selector(vision.FORWARD_CAMERA, 'grapes/board')
horiz_grape_sel = vision.Selector(vision.FORWARD_CAMERA, 'grapes/grape_close', vision.FilterSortKey(lambda obj: float(obj['center'][0]), False))
vert_grape_sel = vision.Selector(vision.FORWARD_CAMERA, 'grapes/grape_close', vision.FilterSortKey(lambda obj: float(obj['center'][0]), True))
grape_count_sel = vision.Selector(vision.FORWARD_CAMERA, 'grapes/grape')
@mission.State("push_horizontal")
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)
from subjugator import sched
from subjugator import nav
from subjugator import vision
from missionplanner import mission
import math
import dds
servo = vision.BottomVisualServo(kx=.3, ky=.3, xytol=.02, Ytol=.03, debug=True)
pipe_sels = dict(
any=vision.Selector(vision.DOWN_CAMERA, 'pipe'),
left=vision.Selector(vision.DOWN_CAMERA, 'pipe',
vision.FilterSort('angle', descending=False)),
right=vision.Selector(vision.DOWN_CAMERA, 'pipe',
vision.FilterSort('angle', descending=True)))
def run(name):
nav.setup()
nav.depth(.6)
while True:
with sched.Timeout(20) as timeout:
print 'Looking for ' + name + ' pipe'
nav.vel(.2)
vision.wait_visible(pipe_sels[name])
print 'See pipe!'
if timeout.activated:
print 'Timed out on pipe'
return False
