def ApproachCenterSize(sizef, centerf, alignf, visiblef, size_thresh, p=0.000003, px=0.0009, py=0.005, dx=0.00, dy=0.01, d=0, consistent_total=2.0, closedb=20, db=30000):
return MasterConcurrent(
Consistent(lambda: abs(sizef()-size_thresh) < db and close_to(centerf(), CAM_CENTER, db=closedb), count=1.7, total=2.0, invert=False, result=True),
Consistent(visiblef, count=1.3, total=consistent_total, invert=True, result=False),
While(lambda: Center(centerf, visiblef, px=px, py=py, dx=dx, dy=dy), True),
PIDStride(lambda: sizef()-size_thresh, p=p, d=d),
AlwaysLog(lambda: "center: {}, target: {}, size{}".format(CAM_CENTER, centerf(), sizef())))
def ApproachAlignSize(sizef, centerf, alignf, visiblef, size_thresh, db=30000):
return MasterConcurrent(
Consistent(lambda: abs(sizef()-size_thresh) < db and aligned(alignf(), db=5), count=1.3, total=2.0, invert=False, result=True),
Consistent(visiblef, count=1.3, total=2.0, invert=True, result=False),
While(lambda: Align(centerf, alignf, visiblef), True),
PIDStride(lambda: sizef()-size_thresh),
AlwaysLog(lambda: "center: {}, target: {}, align: {}, size{}".format(CAM_CENTER, centerf(), alignf(), sizef())))
def Align(centerf, alignf, visiblef, px=0.14, py=0.004, p=0.009, dx=0.00, dy=0.000, dsway=0.00, db=0): # dx=0.005, dy=0.0005, dsway=0.002
return MasterConcurrent(
Consistent(lambda: close_to(centerf(), CAM_CENTER) and aligned(alignf()), count=1.5, total=2, invert=False, result=True),
Consistent(visiblef, count=1.5, total=2.0, invert=True, result=False),
HeadingTarget(point=centerf, target=CAM_CENTER, px=px, py=py, dy=dy, dx=dx, deadband=(db,db)),
PIDSway(alignf, p=p, d=dsway, db=db),
AlwaysLog(lambda: "align_h: %d" % (alignf(),)))
AlwaysLog(lambda: "align_h: %d" % (alignf(),)))
# def AlignBoard():
# return Align(board_center, align_h, visible)
# def AlignHeart():
# return Align(heart, align_h, visible)
# def AlignLeftHole():
# return Align(left_hole, align_h, visible)
# def AlignRightHole():
# return Align(right_hole, align_h, visible)
Center = lambda centerf, visiblef, targetf=CAM_CENTER, px=0.0006, py=0.003, dx=0.000, dy=0.0, db=0, iy=0.0002, closedb=5: MasterConcurrent(
Consistent(lambda: close_to(centerf(), targetf, db=closedb), count=4.0, total=5.0, invert=False, result=True),
Consistent(visiblef, count=1.5, total=2.0, invert=True, result=False),
ForwardTarget(point=centerf, target=targetf, px=px, py=py, dx=dx, dy=dy, iy=iy, deadband=(db,db)),
AlwaysLog(lambda: "center: {}, target: {}".format(targetf, centerf())))
# @withApproachHeartOnFail
# def CenterHeart():
# return Center(centerf=heart, visiblef=visible, py=0.002, closedb=20)
# def CenterBelt():
# return Center(centef=belt, visiblef=visible, closedb=15)
# @withApproachLeftHoleOnFail
# def CenterLeftHole():
# return Center(centerf=left_hole, visiblef=visible, closedb=15)
# @withApproachRightHoleOnFail
# def CenterRightHole():
# return Center(centerf=right_hole, visiblef=visible, closedb=15)
@withApproachAlignOnFail
def CenterLever():
return Center(centerf=lever, visiblef=visible, px=0.001, closedb=20)
consistent_frames=(15, 19)
),
Zero(),
Depth(INITIAL_DEPTH, error=0.2))
SearchAnyVampire = lambda: Search(any_visible)
close_to = lambda point1, point2, dbx=20, dby=20: abs(point1[0]-point2[0]) < dbx and abs(point1[1]-point2[1]) < dby
Center = lambda centerf, visiblef, db=15, px=0.001, py=0.001, dx=0.00005, dy=0.00005: Sequential(
Log('Centering'),
MasterConcurrent(
Consistent(lambda: close_to(centerf(), CAM_CENTER, db), count=2.5, total=3.0, invert=False, result=True),
Consistent(visiblef, count=2.5, total=3.0, invert=True, result=False),
While(lambda: DownwardTarget(point=centerf, target=CAM_CENTER, deadband=(0, 0), px=px, py=py), True),
AlwaysLog(lambda: 'center = {}, target = {}'.format(centerf(), CAM_CENTER))))
CenterAny = lambda: Center(center_any, any_visible)
# Descend = lambda depth=DEPTH, db=0.1, size_thresh=SIZE_THRESH: Sequential( # TODO: FIND THE ACTUAL DEPTH1!!
# Log('Descent into Madness'),
# MasterConcurrent( # TODO: TIMEOUT
# Consistent(lambda: abs(shm.kalman.depth.get() - depth) < db or size() > size_thresh, count=2.3, total=3, invert=False, result=True),
# Depth(depth)), # TODO: BigDepth?
# Zero())
close_to = lambda point1, point2, db=20: abs(point1[0]-point2[0]) < db and abs(point1[1]-point2[1]) < db
Align = lambda centerf, anglef, visiblef, closedb=10, aligndb=7: Sequential(
Log('Aligning'),
MasterConcurrent(
def size():
return shm.recovery_garlic.size.get()
Search = lambda: Sequential( # TODO: TIMEOUT?
Log('Searching'),
SearchFor(SwaySearch(width=2.5, stride=2),
visible,
consistent_frames=(5, 7)), Zero())
Center = lambda db=40, px=0.0008, py=0.0008: Sequential(
Log('Centering'),
MasterConcurrent(
DownwardTarget(
point=center, target=CAM_CENTER, deadband=(db, db), px=px, py=py),
AlwaysLog(lambda: 'center = {}, target = {}'.format(
center(), CAM_CENTER))))
Descend = lambda depth=DEPTH, db=0.1, size_thresh=SIZE_THRESH: Sequential( # TODO: FIND THE ACTUAL DEPTH1!!
Log('Descent into Madness'),
MasterConcurrent( # TODO: TIMEOUT
Consistent(lambda: abs(shm.kalman.depth.get() - depth) < db or size() >
size_thresh,
count=2.3,
total=3,
invert=False,
result=True), Depth(depth)), # TODO: BigDepth?
Zero())
close_to = lambda point1, point2, db=10: abs(point1[0] - point2[
0]) < db and abs(point1[1] - point2[1]) < db
# Centers the buoy using forward target
CenterBuoy = lambda centerf, visiblef, px=0.007, py=0.006, d=0.005, db=0: MasterConcurrent(
Consistent(lambda: close_to(centerf(), CAM_CENTER),
count=2.7,
total=3.0,
invert=False,
result=True),
#Consistent(visiblef, count=0.2, total=0.3, invert=True, result=False),
ForwardTarget(point=centerf,
target=CAM_CENTER,
px=px,
py=py,
dx=d,
dy=d,
deadband=(db, db)),
AlwaysLog(lambda: "center: {}, target: {}".format(CAM_CENTER, centerf())))
# Centers any side of the triangular buoy
CenterAnyBuoy = lambda: CenterBuoy(centerf=any_buoy_center,
visiblef=triangle_visible)
# Centers only the called side
CenterCalledBuoy = lambda: CenterBuoy(centerf=call_buoy_center,
visiblef=call_buoy_visible)
# Centers the single target buoy
CenterSingleBuoy = lambda: CenterBuoy(centerf=single_buoy_center,
visiblef=single_buoy_visible)
# Approaches a the buoy until it reaches a predetermined size threshold
Approach = lambda sizef, centerf, visiblef: Sequential(
MasterConcurrent(
Consistent(lambda: sizef() > SIZE_THRESH,