def on_run(self, *args):
if self.last_run_time is None:
return
dt = self.this_run_time - self.last_run_time
self.revolve_angle += self.degrees_per_second * dt
if self.revolve_angle > 390:
Heading(self.initial_heading)()
Pitch(0)()
Roll(0)()
self.finish()
return
rot_2d = rotate((0, 1), self.revolve_angle)
rotation_axis = np.array((rot_2d[0], rot_2d[1], 0))
conic_quat = quat_from_axis_angle(rotation_axis, self.conic_angle_rad)
# TODO How can we achieve the same effect without using heading?
heading, pitch, roll = conic_quat.hpr()
Heading(heading)()
Pitch(pitch)()
Roll(roll)()
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 enter_follow(self):
self.logi("Following a heading of %0.3f" % self.heading_to_pinger)
self.follow_change_heading = Heading()
self.follow_inital_heading = Heading(self.heading_to_pinger + heading_offset)
self.follow_vel_x = VelocityX()
self.follow_vel_y = VelocityY()
distance_to_pinger = self.elevation_to_distance(self.follow_elevation)
self.follow_vel_x(x_dir * self.get_follow_speed(distance_to_pinger))
self.follow_vel_y(0.0)
def Spin():
return Sequential(
Timed(VelocityX(1), segment_time), Zero(),
Heading(initial_heading + 90),
Timed(VelocityY(-1), segment_time), Zero(),
Heading(initial_heading + 180),
Timed(VelocityX(-1), segment_time), Zero(),
Heading(initial_heading + 270),
Timed(VelocityY(1), segment_time), Zero(),
Heading(initial_heading),
)
def on_run(self, speed):
if self.update():
VelocityX(0)()
self.finish()
return
Heading(self.get_target_heading())()
VelocityX(self.calc_speed(speed))()
def on_first_run(self):
self.update_data()
self.align = Heading(
lambda: self.pipe_results.angle + shm.kalman.heading.get(),
deadband=0.5)
self.alignment_checker = ConsistencyCheck(19, 20)
pipe_found = self.pipe_results.heuristic_score > 0
self.center = DownwardTarget(lambda self=self:
(self.pipe_results.center_x * 100, self.
pipe_results.center_y * 100),
target=(0, 0),
deadband=(1, 1),
px=0.03,
py=0.03,
dx=0.01,
dy=0.01,
valid=pipe_found)
self.logi("Beginning to align to the pipe's heading")
c_center = "Camera Center: " + str((.5, .5))
self.logi("Pipe center" + str(self.pipe_results.center_x) + " " +
str(self.pipe_results.center_y))
self.logi(c_center)
def on_run(self,
meters_per_revolution=1.3,
deadband=0.2,
spin_ratio=1,
relative_depth_range=0.0,
heading_change_scale=None,
optimize_heading=False,
min_spin_radius=None):
radius = self.calc_radius(self.theta, meters_per_revolution)
target = self.calc_position(self.theta, meters_per_revolution,
relative_depth_range)
delta = target - _sub_position()
# fake_target = target + (delta * 10)
fake_target = target
GoToPosition(fake_target[0], fake_target[1], depth=fake_target[2])()
desired_heading = (spin_ratio * self.theta) + 90
if heading_change_scale is not None:
desired_heading *= min(1.0, radius) * heading_change_scale
if optimize_heading:
desired_heading = math.degrees(math.atan2(delta[1], delta[0]))
if min_spin_radius is not None:
if (not abs_heading_sub_degrees(shm.kalman.heading.get(),
desired_heading) < 10
or not radius > min_spin_radius
) and not self.started_heading:
desired_heading = shm.navigation_desires.heading.get()
else:
self.started_heading = True
Heading(desired_heading % 360)()
if np.linalg.norm(_sub_position() - target) < deadband:
self.theta += 10
def on_first_run(self,
north,
east,
heading=None,
depth=None,
optimize=False,
rough=False,
deadband=0.05):
self.north = north
self.east = east
if heading is None:
self.heading = shm.navigation_desires.heading.get()
else:
self.heading = heading
if depth is None:
self.depth = shm.navigation_desires.depth.get()
else:
self.depth = depth
self.use_task(
WithPositionalControl(
Concurrent(PositionN(self.north, error=deadband),
PositionE(self.east, error=deadband),
Heading(self.heading, error=deadband),
Depth(self.depth, error=deadband)),
optimize=optimize,
))
def on_first_run(self, *args, **kwargs):
self.use_task(
NavigationSpeed(
Sequential(
Depth(recovery_constants.tower_depth, error=0.08),
ConsistentTask(Heading(60 * TOWARDS_BOWL_SIGN)),
MoveX(30, deadband=0.08),
), 0.4))
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 new_task(self):
self.offset_len += self.stride
new_pos = self.initial_pos + self.direction_vector * self.offset_len
self.task = Sequential(
self.Scan(self.initial_heading + self.heading_amplitude),
Timer(1.0),
self.Scan(self.initial_heading - self.heading_amplitude),
Timer(1.0),
Heading(self.initial_heading),
self.MoveIncrement(new_pos[0], new_pos[1]),
)
def __init__(self, heading=None, *args, **kwargs):
"""
heading - a heading to use as the original heading
"""
super().__init__(*args, **kwargs)
# Store the start heading of the sub
if heading is None:
self.start_heading = shm.kalman.heading.get()
else:
self.start_heading = heading
self.heading_task = Heading(self.start_heading, error=4)
def on_first_run(self, current, desire, mod=360, *args, **kwargs):
def desired_heading():
return shm.kalman.heading.get() - (heading_sub_degrees(
call_if_function(desire), call_if_function(current), mod=mod))
self.use_task(
Sequential(
GradualHeading(desired_heading),
Heading(desired_heading),
finite=False,
))
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 on_first_run(self):
self.update_data()
self.align = Heading(lambda: self.pipe_results.angle + shm.kalman.heading.get(), deadband=0.5)
self.alignment_checker = ConsistencyCheck(49, 50)
pipe_found = self.pipe_results.heuristic_score > 0
self.center = DownwardTarget(lambda self=self: (self.pipe_results.center_x, self.pipe_results.center_y),
target=get_downward_camera_center,
deadband=(10,10), px=0.001, py=0.001, dx=0.002, dy=0.002,
valid=pipe_found)
def on_first_run(self,
desire,
relative_desire=15,
relative_deadband=25,
*args,
**kwargs):
self.use_task(
Sequential(
self.GradualApproxHeading(desire, relative_desire,
relative_deadband),
Heading(desire, *args, **kwargs),
finite=False,
))
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, vision, *args, **kwargs):
initial_heading = shm.kalman.heading.get()
depth_set = DepthRestore()
self.use_task(
Conditional(
Sequential(
MasterConcurrent(
Sequential(
Retry(lambda: Sequential(
Log('Returning to initial heading'),
Heading(initial_heading),
Log('Going to depth'),
depth_set,
#Log('Moving forward away last pos'),
#Timed(VelocityX(0.5), 1),
#Zero(),
Log('Searching for gate'),
MasterConcurrent(
IdentifyGate(vision),
VelocityHeadingSearch(initial_heading=initial_heading),
),
Zero(),
Log('Found gate, aligning'),
AlignChannel(vision),
), float('inf')),
),
Fail(Timer(180)),
),
Log('Aligned to gate, moving closer and fixing depth'),
MoveCloser(2),
Log('Beginning spin'),
StyleSegmentedSpin(),
),
Sequential(
Log('Wire completed successfully!'),
Timed(VelocityX(.4), 2),
Zero(),
RelativeToInitialHeading(180),
),
Log('Traveled too far without task completion'),
)
)
def on_first_run(self, vision, reset_state=True, *args, **kwargs):
# Limit navigation speed to prevent merge cutting out computer
nav_speed = shm.navigation_settings.max_speed
orig_speed = nav_speed.get()
nav_speed.set(0.5)
if reset_state:
for loc in ['table', 'tower']:
g_name = getattr(shm, 'recovery_world_{}'.format(loc))
g = g_name.get()
g.know_pos = False
for i in range(self.MAX_COLORS):
setattr(g, 'has_tube{}'.format(i), loc == 'tower'
and i < len(constants.colors))
g_name.set(g)
for a in AMLANS:
a.shm_tube.set(-1)
self.use_task(
Conditional(
Defer(
Sequential(
Log('Starting recovery!'),
AMLANS[0].FastRetract(),
AMLANS[1].FastRetract(),
TryRecovery(vision),
),
Sequential(
AMLANS[0].FastRetract(),
AMLANS[1].FastRetract(),
Log('Returning to pre-recovery depth'),
FastDepth(shm.kalman.depth.get()),
Log('Resetting navigation speed'),
FunctionTask(lambda: nav_speed.set(orig_speed)),
Log('Moving away from table'),
Heading(WALL_TOWER_HEADING),
MoveX(5, deadband=0.08),
),
),
Log('Recovery success!'),
Log('Recovery failure!'),
))
def Full():
return Sequential(
Log('Doing balls! RIP octagon'),
Log('Turning away from wall'),
Heading(WALL_TOWER_HEADING, error=5),
Log('Surfacing'),
Depth(0.5, error=0.1),
Timed(Depth(0), 3),
Log('Going over to press octagon'),
Timed(VelocityX(0.3), 11),
Log('Pulling octagon underwater'),
Timed(Concurrent(
VelocityX(0.3),
Depth(0.5),
finite=False,
), 5),
Log('Snapping octagon back in place'),
Timed(VelocityX(-0.2), 4),
VelocityX(0),
Log('Well, I guess that\'s it for balls.'),
)
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(),
))
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))
if self.last_shmval is None or self.shmval != self.last_shmval:
Consistent(lambda: single_buoy_size() > BIG_SIZE_THRESH,
count=0.2,
total=0.3,
invert=False,
result=True), Succeed(CenterSingleBuoy()),
Succeed(VelocityX(.3))), Log('yeet'), Timed(VelocityX(0.3), 8), Zero())
# Search then approach the buoy
SearchAndApproach = lambda: Sequential(SearchCalled(), AlignCalledNormal(
), ApproachCalled()) # , AlignCalledNormal())
# The full mission for the triangular buoy
TriangleOnly = lambda: Sequential(
Log('Searching for buoy'), Timeout(SearchCalled(), 30), Log('Found buoy'),
ApproachAny(), Log('Ramming'), RamV(), Log('Vamp_Buoy Complete'),
Heading(get_heading))
# Search task for the single target buoy
SearchSingle = lambda: Sequential(
Log('Searching for singular buoy'),
SearchFor(
VelocitySwaySearch(width=3, stride=4),
shm.vamp_buoy_results.jiangshi_visible.get,
# lambda: shm.vamp_buoy_results.jiangshi_visible.get() and single_buoy_size() > SEARCH_SIZE_THRESH,
consistent_frames=(3, 5)),
Log('Singular Found'),
Zero())
DeadReckonStupid = lambda: \
Sequential(
# Timed(VelocityY(DIRECTION * 0.3, error=40), 3),
)
from mission.missions.actuate import (
FireGreen,
FireRed,
)
from mission.constants.config import cash_in as settings
from mission.missions.will_common import BigDepth
dist1 = 45
dist2 = 52
dist3 = dist1 / 2
stupid_castor = Sequential(
BigDepth(1.5),
Timed(VelocityX(0.4), dist1),
# MoveXRough(20),
VelocityX(0),
Heading(37),
Timed(VelocityX(0.4), dist2),
VelocityX(0),
finite=True)
stupid_castor_2 = Sequential(Heading(10),
Timed(VelocityX(0.4), dist3),
VelocityX(0),
finite=True)
search = SaneHeadingSearch()
def make_turn(self):
h = self.base_heading + (self.side * 90)
h %= 360
print("Turning to {}".format(h))
return cons(Heading(h))
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))
def on_run(self, pitch=True, roll=True):
Heading(shm.kalman.heading.get())()
ZeroWithoutHeading(pitch=pitch, roll=roll)()
self.finish()
Log("Picked Up Right"),
)
drop_all = Sequential(
drop_right,
Log("Dropped Right"),
drop_left,
Log("Dropped Left"),
)
do_it_all = Sequential(
search_left,
Log("Found"),
pickup_all,
Log("Picked Up"),
cons(Heading(45)),
Log("Turned to hardcoded heading"),
drop_all,
Log("Dropped"),
)
left = make_bin_chooser(True)
right = make_bin_chooser(False)
center_left = cons (
DownwardTarget(
point=(lambda: left().center_x.get(), lambda: left().center_y.get()),
target=norm_to_vision_downward(0, 0),
deadband=norm_to_vision_downward(-0.9, -0.9),
px=0.0005,
(shm.gate.rightmost_x.get() + shm.gate.middle_x.get()) / 2
if shm.gate.rightmost_visible.get() else
(shm.gate.leftmost_x.get() + shm.gate.middle_x.get()) / 2,
target=lambda: shm.gate.img_width.get() / 2,
p=0.0015,
deadband=0,
output_function=VelocityY(),
negate=True)
search_task = lambda:\
SearchFor(
Sequential(
# manual list of "check here first, then just StillHeadingSearch"
FunctionTask(save_heading),
Log('Searching for gate: using manual turning to right'),
Heading(lambda: saved_heading + 90),
Heading(lambda: saved_heading + 180),
Log('Searching for gate: didn\'t find it - turn back'),
Heading(lambda: saved_heading + 90),
Heading(lambda: saved_heading + 0),
Log('Searching for gate: didn\'t find it - spin'),
Heading(lambda: saved_heading + 90),
Heading(lambda: saved_heading + 180),
Heading(lambda: saved_heading + 270),
Heading(lambda: saved_heading + 0),
Log('Searching for gate: fall back on StillHeadingSearch'),
StillHeadingSearch()
),
shm.gate.leftmost_visible.get,
consistent_frames=(6, 10)
)
def on_first_run(self, *args, **kwargs):
#heading = Heading((kalman.heading.get() + 180) % 360, error=1)
return Sequential(Pitch(0, error=1), Roll(0, error=1), Timer(1.5), Heading(lambda: kalman.heading.get() + 180, error=1), Timer(1))
