def DoComparison(counter_a, counter_b):
return \
While(
lambda: Timed(Concurrent(
While(center_cover, lambda: True),
counter_a,
counter_b,
), 1.5),
lambda: counter_a.get_value() + counter_b.get_value() < 10
)
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 on_first_run(self, vision, *args, **kwargs):
check = lambda: checkAligned(vision)
self.use_task(
Conditional(
While(
lambda: Sequential(
Log("Restoring depth"),
Depth(constants.WIRE_DEPTH),
Log('Align Heading'),
AlignHeading(vision),
Zero(),
Log('Align Sway'),
AlignSway(vision),
Zero(),
Log('Align Heading'),
AlignHeading(vision),
Zero(),
Log('Align Fore'),
AlignFore(vision),
Zero(),
), lambda: checkNotAligned(vision)
),
on_fail=Sequential(
Log('Lost the gate, backing up and looking again'),
Succeed(Concurrent(
Fail(Timed(VelocityX(-.4), 6)),
Fail(IdentifyGate(vision)),
)),
Fail(),
)
)
)
def on_first_run(self, *args, **kwargs):
init_heading = shm.kalman.heading.get()
self.use_task(Timed(
While(lambda: Sequential(
# Pause a little to let object-recognizing tasks see the current fov
Timer(0.5),
# Check the right side
GradualHeading(init_heading + 90),
Timer(0.5),
Heading(init_heading),
# Check the left and back
GradualHeading(init_heading - 90),
GradualHeading(init_heading - 180),
GradualHeading(init_heading - 270),
Timer(0.5),
Heading(init_heading),
# Move back a bit, we might be too close
MoveX(-1),
), True),
self.TIMEOUT,
))
def Backup(speed=0.2):
return Sequential(
Timeout(SearchFor(
search_task=While(lambda: VelocityX(-speed), True),
visible=lambda: visible() and size() < ALIGN_SIZE,
consistent_frames=(1.7*60, 2.0*60),
), 15),
Zero(),
)
def SearchCalled():
return Sequential(
Log('Searching for any buoy'),
Zero(),
SearchFor(
While(lambda: VelocityY(DIRECTION * -0.2), True),
triangle_visible,
consistent_frames=(3, 5) #TODO: Check consistent frames
),
FunctionTask(set_last_seen),
Zero())
def on_first_run(self, vision, *args, **kwargs):
def AvoidWall():
if not shm.recovery_world_table.know_pos.get():
return Sequential(
Log('Moving away from wall'),
ConsistentTask(Heading(WALL_TOWER_HEADING)),
MoveXRough(2),
)
else:
return NoOp()
def Iteration(first_run=False):
return Sequential(
# Sequential(
# Log('Classifying tubes'),
# Retry(lambda: MoveAboveTower(vision), 3),
# FunctionTask(vision.classify_tubes),
# ) if first_run else NoOp(),
GetTube(vision),
Succeed(GetTube(
vision)), # It's OK if we don't grab the second tube
Surface(),
AvoidWall(),
# Initially classify ellipses as their size / color may change
# as tubes are added
Sequential(
Log('Classifying ellipses'),
Retry(lambda: MoveAboveTable(vision), 3),
FunctionTask(vision.classify_ellipses),
) if first_run else NoOp(),
PlaceTubes(vision),
)
self.use_task(
Except(
Sequential(
Retry(lambda: MoveAboveTable(vision), 1000),
Log('Moving towards wall'),
ConsistentTask(Heading((WALL_TOWER_HEADING + 180) % 360)),
MoveXRough(2),
Iteration(first_run=True),
While(
Iteration,
lambda: count_tubes(shm.recovery_world_tower) > 0,
),
),
Fail(Log('Global timeout, done trying recovery T_T')),
GlobalTimeoutError,
))
def on_first_run(self,
initial_heading=None,
stride_speed=1,
stride_time=1.5,
heading_amplitude=45,
*args,
**kwargs):
if initial_heading is None:
initial_heading = shm.desires.heading.get()
Heading(initial_heading)(),
self.use_task(
While(
lambda: Sequential(
Timed(VelocityX(stride_speed), stride_time),
Zero(),
HeadingSearch.Scan(initial_heading + heading_amplitude),
HeadingSearch.Scan(initial_heading - heading_amplitude),
Heading(initial_heading),
), True))
def one_path(grp):
return Timeout(
90,
Sequential(
Zero(),
search_task(),
While(
lambda: Sequential(
Zero(),
Log('Centering on path'),
center(grp),
Log('Going to follow depth'),
Depth(PATH_FOLLOW_DEPTH),
Log('Aligning with path'),
Heading(pathAngle(grp.get()), deadband=0.1),
Zero(),
Timer(1),
Log(grp.angle.get()),
), lambda: checkNotAligned(grp.get())),
Log('aligned'),
Zero(),
))
v = heading_to_vector(heading()) * -dst
print(shm.path_results.center_x.get(), shm.path_results.center_y.get(), v)
return abs(heading_sub_degrees(trg, heading(), math.pi*2)) < math.radians(3) and \
abs(shm.path_results.center_x.get() - v[0]) < deadband and \
abs(shm.path_results.center_y.get() - v[1]) < deadband
# def on_run(self, error, p=0.0005, i=0, d=0.0, db=0.01875, max_out=0.5, negate=False, *args, **kwargs):
FollowLine = Concurrent(
PIDSway(shm.path_results.center_x.get, negate=True, db=0, p=1.2, i=.05),
While(
lambda: Sequential(
FunctionTask(lambda: shm.navigation_desires.heading.set(
(shm.kalman.heading.get() - .95 * math.degrees(
heading_sub_degrees(
math.pi / 2,
shm.path_results.angle_1.get() % math.pi, math.pi * 2))
) % 360)),
#Log('{:03d} '.format(int(shm.navigation_desires.heading.get())) + s2(int(math.degrees(trg)), int(math.degrees(heading())), int(shm.desires.heading.get())) if shm.path_results.num_lines.get() == 2 else 'X'),
Timer(.05)),
lambda: True))
aaa = Sequential(Log("Started mission"), Depth(1.5), Log("At depth"),
FakeMoveX(2, .65), Zero(), Log("Starting tracking"), Timer(1),
Log("following"),
Timed(Concurrent(VelocityX(.5), FollowLine), 24),
Log("following done"), Timed(VelocityX(-.5), .1), Zero(),
Timer(1), FakeMoveY(-.6, .65), Timed(VelocityY(.5), .05),
Zero(), Timer(1), FakeMoveX(2.8, .65),
Timed(VelocityX(-.5), .1), Zero(), Timer(1), Log("turning 1"),
RelativeToInitialHeading(90), Log("turning 2"), Timer(1),
),
)
align_task = lambda:\
Sequential(
While(
condition=lambda: not is_aligned(),
task_func=lambda: Sequential(
Conditional(
main_task=FunctionTask(lambda: gate_elems() == 2),
on_success=align_on_two_elem(),
on_fail=Conditional(
main_task=FunctionTask(lambda: gate_elems() == 3),
on_success=align_on_three_elem(),
on_fail=Sequential(
Log('we see less than two elems, failed'),
Timed(VelocityX(-0.3), 2),
),
finite=False
),
finite=False
),
Zero(),
#Timer(1),
finite=False
),
),
finite=False,
)
align_left_width = lambda: PIDLoop(
input_value=lambda: shm.gate.middle_x.get() - shm.gate.leftmost_x.get(),
def PipeAlign(get_center,
heading_vect,
heading,
get_visible,
trg,
dst,
deadband,
angle_range=math.pi * 2,
abs_offset=(0, 0)):
return Sequential(
Log("PipeAlign start"),
MasterConcurrent(
Consistent(lambda: is_done(heading_vect, heading, get_center, trg,
dst, deadband, abs_offset),
count=.5,
total=.75,
invert=False,
result=True),
While(
lambda: Sequential(
#Log("attempt asdasd"),
Concurrent(
DownwardTarget(
lambda: get_center(),
target=lambda:
(np.float64([heading_vect()]).view(np.complex128) *
-dst).view(np.float64)[0] + abs_offset,
deadband=(deadband, deadband),
px=2,
py=2,
dx=.5,
dy=.5 #, ix=.15, iy=.15
),
While(
lambda: Sequential(
FunctionTask(
lambda: shm.navigation_desires.heading.set(
(shm.kalman.heading.get(
) - .95 * math.degrees(
heading_sub_degrees(
trg, heading(), angle_range))
) % 360) if get_visible() else None),
#Log('{:03d} '.format(int(shm.navigation_desires.heading.get())) + s2(int(math.degrees(trg)), int(math.degrees(heading())), int(shm.desires.heading.get()))),
Timer(.05)),
lambda: abs(
heading_sub_degrees(trg, heading(), math.pi * 2
)) > math.radians(5)
) #, count=6, total=8, invert=False, result=True),
)),
lambda:
True #lambda: abs(heading_sub_degrees(trg, heading(), math.pi*2)) > math.radians(5) or abs(shm.path_results.center_x.get()) > .08 or abs(shm.path_results.center_y.get()) > .08
),
#, count=3, total=4, invert=False, result=True),
#While(lambda: Log("V: {} h: {} d: {} x: {} y: {} c: {}".format(shm.path_results.num_lines.get(), heading(), math.degrees(heading_sub_degrees(trg, heading(), math.pi*2)), shm.path_results.center_x.get(), shm.path_results.center_y.get(), heading_to_vector(heading()) * dst)), lambda: True),
#While(lambda: Log(s(int(math.degrees(heading_sub_degrees(trg, heading(), math.pi*2))) + 180)), lambda: True),
#While(lambda: Log(s2(int(math.degrees(trg)), int(math.degrees(heading()))) if shm.path_results.num_lines.get() == 2 else 'X'), lambda: True),
),
Log("Centered on Pipe in PipeAlign!"),
#FunctionTask(lambda: shm.navigation_desires.heading.set(-180/math.pi*heading()+shm.kalman.heading.get()))
)
#While(lambda: Log(s(int(math.degrees(heading_sub_degrees(trg, heading(), math.pi*2))) + 180)), lambda: True),
#While(lambda: Log(s2(int(math.degrees(trg)), int(math.degrees(heading()))) if shm.path_results.num_lines.get() == 2 else 'X'), lambda: True),
),
Log("Centered on Pipe in PipeAlign!"),
#FunctionTask(lambda: shm.navigation_desires.heading.set(-180/math.pi*heading()+shm.kalman.heading.get()))
)
def t180(a):
return (a + math.pi) % (2 * math.pi)
find_bins = lambda: Sequential(
Depth(.1),
FunctionTask(VelocityX(.5)),
While(lambda: NoOp(), lambda: not shm.bins_status.cover_visible.get()),
VelocityX(0),
)
def get_cover_vect():
return np.float32(
[shm.bins_status.cover_maj_x.get(),
shm.bins_status.cover_maj_y.get()])
def get_cover_center():
return shm.bins_status.cover_x.get(), shm.bins_status.cover_y.get()
def get_wolf_center():
def PushLever():
def TargetLever(py):
return ForwardTarget(
point=lambda:
(shm.bins_status.lever_x.get(), shm.bins_status.lever_y.get() /
(1 + shm.bins_status.lever_sz.get() / 50)),
#target=lambda: (0, .3 * min(100, shm.bins_status.lever_sz.get()) / 100),
target=lambda: (0, .25 /
(1 + shm.bins_status.lever_sz.get() / 50)),
valid=shm.bins_status.lever_visible.get,
deadband=(DEADBAND, DEADBAND),
px=1,
py=py,
ix=.05,
iy=.05)
return Sequential(
Log("PushLever start"),
FunctionTask(VelocityX(.2)),
MasterConcurrent(
Consistent(lambda: shm.bins_status.lever_sz.get() > 90,
count=.5,
total=.75,
invert=False,
result=True),
While(lambda: TargetLever(1.5), lambda: True),
While(
lambda: FunctionTask(
VelocityX(.2 / (1 + 2 * (abs(shm.bins_status.lever_x.get(
)) + abs(shm.bins_status.lever_y.get() - .25))))),
lambda: True)),
#Log("Higher P"),
#MasterConcurrent(
# Consistent(lambda: shm.bins_status.lever_sz.get() > 100, count=.5, total=.75, invert=False, result=True),
# While(lambda: TargetLever(.8), lambda: True),
# While(lambda: FunctionTask(
# VelocityX(.2 / (1 + 2 * (abs(shm.bins_status.lever_x.get()) + abs(shm.bins_status.lever_y.get()-.15))))
# ), lambda: True)
#),
#Log("targeting"),
#TargetLever(),
Log("zoom zoom"),
#RelativeToInitialDepth(-.1),
#Timed(RelativeToCurrentDepth(-2), .7),
#FunctionTask(RelativeToInitialDepth(0)),
Concurrent(
Sequential(
Timer(1),
Log("Forwards!"),
FunctionTask(VelocityX(1)),
#VelocityDepth(0),
),
Sequential(
Timed(RelativeToCurrentDepth(-.4), 1),
RelativeToInitialDepth(0),
),
Timer(4),
),
#Timed(
# While(TargetLever, lambda: True),
# 5
#),
Timed(VelocityX(-.8), .5),
VelocityX(0),
Log("waiting"),
Timer(4),
#RelativeToInitialHeading(0),
#Timed(VelocityX(-.8), 1),
#RelativeToInitialHeading(0),
#FunctionTask(VelocityX(0)),
#Timer(5),
#TargetLever()
)
While(
lambda: Sequential(
Log("attempt asdasd"),
Concurrent(
DownwardTarget(lambda: (shm.path_results.center_x.get(),
shm.path_results.center_y.get()),
target=(lambda: heading_to_vector(heading())
* -dst),
deadband=(deadband, deadband),
px=1,
py=1,
ix=.05,
iy=.05),
While(
lambda: Sequential(
FunctionTask(lambda: shm.navigation_desires
.heading.set((shm.kalman.heading.get(
) - .95 * math.degrees(
heading_sub_degrees(
trg, heading(), math.pi * 2)))
% 360)
if shm.path_results.num_lines.get(
) == 2 else None),
#Log('{:03d} '.format(int(shm.navigation_desires.heading.get())) + s2(int(math.degrees(trg)), int(math.degrees(heading())), int(shm.desires.heading.get())) if shm.path_results.num_lines.get() == 2 else 'X'),
Timer(.05)),
lambda: abs(
heading_sub_degrees(trg, heading(), math.pi * 2)
) > math.radians(5)
) #, count=6, total=8, invert=False, result=True),
)),
lambda: not is_done(
heading, trg, dst, deadband
) #lambda: abs(heading_sub_degrees(trg, heading(), math.pi*2)) > math.radians(5) or abs(shm.path_results.center_x.get()) > .08 or abs(shm.path_results.center_y.get()) > .08
),
visiblef,
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'),
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,
count=0.2,
total=0.3,
invert=False,
result=True), #ADD EITHER LOSE SIGHT OF BUOY
Consistent(visiblef, count=2.5, total=3.0, invert=True, result=False),
Succeed(VelocityX(.2)),
While(lambda: CenterBuoy(centerf=centerf, visiblef=visiblef), True),
AlwaysLog(lambda: "size: {}, visible: {}".format(sizef(), visiblef()))
),
Zero())
# Approach only the called buoy
@withReSearchCalledOnFail
def ApproachCalled():
return Approach(sizef=call_buoy_size,
centerf=call_buoy_center,
visiblef=call_buoy_visible)
# Approach any side of the triangular buoy
@withAlignAnyOnFail
# This is the unholy cross between my (Will's) and Zander's styles of mission-writing
gate = Sequential(
Log('Depthing...'),
BigDepth(DEPTH_TARGET),
Log('Lining up...'),
ConsistentTask(Concurrent(
Depth(DEPTH_TARGET),
XTarget(x=results_groups.gate_center_x.get, db=0.03),
finite=False
)),
Log('Driving forward...'),
MasterConcurrent(
Consistent(test=lambda: results_groups.width.get() < settings.gate_width_threshold, count=0.2, total=0.3, invert=True, result=True),
Depth(DEPTH_TARGET),
VelocityX(0.1 if is_mainsub() else 0.1),
While(task_func=lambda: XTarget(x=results_groups.gate_center_x.get, db=0.018), condition=True),
),
# Jank
Timed(VelocityX(0 if is_mainsub() else -0.1), 2),
VelocityX(0),
Log('Lining up with red side...'),
ConsistentTask(Concurrent(
Depth(DEPTH_TARGET),
XTarget(x=results_groups.gate_center_x.get, db=0.05),
finite=False,
)),
Log('Charging...'),
Timed(VelocityX(0.5 if is_mainsub() else 0.2), settings.charge_dist),
Log('Through gate!'),
)
from mission.framework.combinators import Sequential, Retry, While
from mission.framework.movement import RelativeToInitialHeading, VelocityY, VelocityX
from mission.framework.position import MoveY
from mission.framework.primitive import Log, Fail, Succeed, FunctionTask
from mission.framework.timing import Timed
from mission.missions.will_common import FakeMoveY
sides = 6
def loop_state():
current = -1
def iterate():
nonlocal current
current += 1
return current < sides
return iterate
polygon = Sequential(
While(
lambda: Sequential(Timed(VelocityY(-0.2), 6),
RelativeToInitialHeading(360 / sides),
Timed(VelocityY(-0.2), 6)), loop_state()))
print('flipped: ' + str(h))
if h != last_pinger_heading:
last_sub_heading = math.radians(shm.kalman.heading.get())
last_pinger_heading = h
return h + last_sub_heading
def get_desired_heading():
h = shm_heading()
if h < 0:
h += math.pi * 2
return math.degrees(h)
def get_desired_vel_x():
h = shm_heading()
if -math.pi / 4 < h < math.pi / 4:
return math.cos(h) * 0.3 # maybe change this function later?
else:
return 0
track = Sequential(
Depth(1.0),
While(
lambda: Sequential(
Heading(get_desired_heading),
VelocityX(get_desired_vel_x),
), lambda: True))
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
Align = lambda closedb=20, aligndb=3: Sequential(
Log('Aligning'),
MasterConcurrent(
Consistent(lambda: close_to(center(), CAM_CENTER) and abs(angle_offset(
)) < aligndb,
count=2.3,
total=3,
invert=False,
result=True), While(Center, True), PIDHeading(angle_offset)
), Zero())
Grab = lambda: Sequential(
MoveY(-0.1),
Timeout(RelativeToInitialDepth(0.5), 20),
FireActuator(), # TODO
)
print(min((45 - diff) / 20, 0.4))
return min((45 - diff) / 20, 0.4)
else:
print("0")
return 0
def enable_hydrophones():
shm.hydrophones_settings.enabled.set(1)
track = Sequential(
# FunctionTask(enable_hydrophones),
Depth(1),
Concurrent(
While(lambda: FunctionTask(update), lambda: True),
While(lambda: Heading(get_target_heading), lambda: True),
While(lambda: VelocityX(calc_speed), lambda: True),
))
class _TrackPinger(Task):
def on_first_run(self, speed=0.4):
self.last_shmval = None
self.last_target_heading = None
self.last_target_elevation = 0
self.checker = ConsistencyCheck(count=3, total=7)
def update(self):
self.shmval = shm.hydrophones_results_track.tracked_ping_heading.get()
# print('last: ' + str(self.last_shmval) + ', new: ' + str(self.shmval))
