Esempio n. 1
0
 def on_first_run(self, vision, tube, amlan):
     self.use_task(
         Sequential(
             Timeout(
                 AlignAmlan(
                     vision,
                     lambda: vision.obj_with_id(vision.tubes, tube),
                     amlan,
                     Altitude(constants.tube_dive_altitude,
                              overshoot_protect=True,
                              deadband=0.07),
                     align_during_depth=True,
                 ), self.ALIGN_TIMEOUT),
             Log('Moving to tube grab altitude'),
             Conditional(
                 Timeout(Altitude(constants.tube_grab_altitude),
                         self.GRAB_ALTITUDE_TIMEOUT),
                 on_fail=Log(
                     'Warning: grab altitude not fully reached, grabbing anyway'
                 ),
             ),
             Timer(1),
             amlan.Extend(),
             Timer(1.5),
             Log('Plucking tube'),
             Timeout(RelativeToInitialDepth(-0.25, error=0.10), 10),
         ))
Esempio n. 2
0
def DeadReckonLever():
    return Retry(lambda: Sequential(
            CenterLever(),  # Approach?
            _DeadReckonLever(),
            Backup(),
            Succeed(Timeout(ApproachAlign(), 40)),
            Timeout(Consistent(lambda: shm.torpedoes_stake.lever_finished.get(), count=1.5, total=2.0, invert=False, result=True), 10)), attempts=2)
Esempio n. 3
0
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(),
            )
Esempio n. 4
0
    def on_first_run(self, timeout=60, *args, **kwargs):
        self.use_task(Timeout(Sequential(
            # Pause initially to give object-identifying tasks time to check current state
            Timer(0.5),

            SpiralSearch(
                relative_depth_range=0,
                optimize_heading=True,
                meters_per_revolution=1,
                min_spin_radius=1,
                deadband=0.2,
            )
        ), timeout))
Esempio n. 5
0
    def on_first_run(self, vision, *args, **kwargs):
        self.use_task(Zeroed(Timeout(
            Sequential(
                Log('Attempting to grab handle'),
                GetHandle(vision, AMLANS[0]),

                Log('Carrying cover away'),
                RelativeToInitialDepth(-self.PICKUP_DELTA_DEPTH, error=0.1),
                Timed(VelocityX(self.SLIDE_SPEED), self.SLIDE_TIME),
                Zero(),

                Log('Dropping cover off here'),
                AMLANS[0].Retract(),
                Timer(1.5),

                Log('Attempting to return to near pre-grab location'),
                RelativeToInitialDepth(-1),
                Timed(VelocityX(-self.SLIDE_SPEED), self.SLIDE_TIME * 5 / 6),
                Zero(),
            ), self.TOTAL_TIMEOUT)))
Esempio n. 6
0
    def on_first_run(self, vision, angle, double_align=False, *args, **kwargs):

        def CenterTargetBin(precision):
            return CenterBins(vision, lambda bin: bin.id == vision.TARGET_BIN, precision=precision)

        def AlignTargetBin():
            return AlignHeadingToAngle(lambda: vision.target_bin().obs.angle, angle, mod=180)

        def DepthAlign(depth):
            return Concurrent(
                AlignTargetBin(),
                CenterTargetBin(1),
                Depth(depth),
                finite=False,
            )

        self.task = Zeroed(Timeout(Sequential(
            Log('Centering over target bin'),
            CenterTargetBin(0),

            Log('Aligning target bin'),
            Concurrent(
                AlignTargetBin(),
                CenterTargetBin(0),
                finite=False,
            ),

            Log('Going half down to precisely align with target bin'),
            DepthAlign((constants.see_both_depth + constants.above_bin_depth) / 2),

            Sequential(
                Log('Going down to fully align to bin'),
                DepthAlign(constants.above_bin_depth),
            ) if double_align else NoOp(),

            Zero(),
            PositionalControl(),
        ), self.TIMEOUT))
Esempio n. 7
0
            shm.recovery_crucifix.offset_y.get())


def angle_offset_crucifix():
    return shm.recovery_crucifix.angle_offset.get()


def size_crucifix():
    return shm.recovery_crucifix.size.get()


GrabCrucifix = lambda: \
    Sequential(
        Search(visible_crucifix),
        Center(center_crucifix, visible_crucifix),
        Align(center_crucifix, angle_offset_crucifix, visible_crucifix),
        Center(offset_crucifix, visible_crucifix, db=10),
        MasterConcurrent(
            Sequential(
                Timer(15),
                _Grab()),
            RelativeToCurrentDepth(DESCEND_DEPTH, error=0.2),
            ),
        Depth(SEARCH_DEPTH, error=0.2),
        Timeout(Consistent(visible_crucifix, count=1.5, total=2.0, invert=True, result=True), 10),
        Log('crucifix grabbed successfully'),
    )

SearchCrucifix = lambda: Search(visible_crucifix)
CenterCrucifix = lambda: Center(center_crucifix, visible_crucifix)
Esempio n. 8
0
def _DeadReckonLever():
    return Sequential(
        Succeed(Timeout(MoveX(.65, deadband=0.05), 20)),
        Succeed(Timeout(MoveY(MOVE_DIRECTION * 0.7, deadband=0.05), 20)),
        Succeed(Timeout(MoveY(MOVE_DIRECTION * -0.30, deadband=0.1), 20)),
        )
Esempio n. 9
0
def withApproachRightHoleOnFail(task):
    return lambda *args, **kwargs: Retry(lambda: Conditional(Timeout(task(*args, **kwargs), 120), on_fail=Fail(Sequential(Zero(), Conditional(ReSearchRightHoleOnFail(), on_fail=ApproachRightHole())))), attempts=2)
Esempio n. 10
0
def TestTimeout():
    return LogSuccess(Timeout(TimedLog('takes too long to log this', 0.1), 0.05))
Esempio n. 11
0
BackUpSway = lambda backx=1.5: Sequential(
        Log('Backing up'),
        MoveX(-backx),
        Log('Sway Search in Progress'),
        SwaySearch(width=2.5, stride=2)
        )

BackUpSwayOnly = lambda backx=0.3: Sequential(
        Log('Backing up'),
        MoveX(-backx),
        )

ReSearchLeftHoleOnFail = lambda timeout=30: Sequential(
        Timeout(SearchFor(
                BackUpSwayOnly(),
                close_left_visible,
                consistent_frames=(1.8*60, 2.0*60)
            ), timeout))


ReSearchRightHoleOnFail = lambda timeout=30: Sequential(
        Timeout(SearchFor(
                BackUpSwayOnly(),
                close_right_visible,
                consistent_frames=(1.8*60, 2.0*60)
            ), timeout))


ReSearchHeartOnFail = lambda timeout=30: Sequential(
        Timeout(SearchFor(
                BackUpSwayOnly(),
Esempio n. 12
0
    Sequential(
        Search(visible_open),
        Center(center_open, visible_open, db=20),
        SetMarker('before_grab'),
        Align(centerf=center_open, anglef=angle_offset_open, visiblef=visible_open),
        Center(offset_open, visible_open, db=10),
        MasterConcurrent(
            Sequential(
                Timer(15),
                _Grab()),
            RelativeToCurrentDepth(DESCEND_DEPTH, error=0.2),
            ),
        Depth(SEARCH_DEPTH),
        GoToMarker('before_grab'),
        UnsetMarker('before_grab'),
        Timeout(Consistent(visible_open, count=1.5, total=2.0, invert=False, result=True), 10),
        # Grab(),  # ???
        # Release???
    )

LID_DEPTH = 0.4
LID_DEPTH_1 = 0.5

initial_depth = 3

def record_depth():
    global initial_depth
    initial_depth = shm.kalman.depth.get()

GrabVampireClosedCoffin = lambda: \
    Sequential(
Esempio n. 13
0
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
)
Esempio n. 14
0
    last_seen = which_buoy_visible()
    return True


def get_sway_direction():
    global last_seen
    return not (getattr(shm.vamp_buoy_results,
                        "%s_center_x" % last_seen).get() < CAM_CENTER[0])


# Search for buoy using SwayOnlySearch
TinySearch = lambda backspeed=0.2, backtime=3: Sequential(
    Zero(),
    Log('Doing TinySearch to see if we can find called'),
    Timeout(
        SearchFor(SwayOnlySearch(right_first=get_sway_direction()),
                  call_buoy_visible,
                  consistent_frames=(0.5 * 60, 1.0 * 60)), 20),
    FunctionTask(set_last_seen),
    Zero(),
)

# Back up, find the triangle buoy again and use it to find the called side
ReSearch = lambda: Sequential(SearchTriangleOnFail(), AlignAnyNormal(),
                              SearchCalled())

# Decorator that wraps a task to search for the called side of the buoy if it fails
withReSearchCalledOnFail = lambda task: lambda: Retry(lambda: \
        Conditional(main_task=task(), on_fail= \
            Fail(TinySearch())), attempts=2)

# The final fallback case. If the called side cannot be found, attempt to ram any side of the triangular buoy if possible.
Esempio n. 15
0
def withReSearchBoardOnFail(task):
    return lambda *args, **kwargs: Retry(lambda: Conditional(Timeout(task(*args, **kwargs), 60), on_fail=Fail(Sequential(Zero(), ReSearchBoardOnFail()))), attempts=2)
Esempio n. 16
0
 Log('Approach to gate complete. Beginning alignment'),
 align_task(),
 FunctionTask(save_heading),
 Log('Approaching passageway'),
 approach_side_task,
 #approach_left_passageway_task,
 #approach_right_passageway_task,
 Log('Pre Spin Charging...'),
 Depth(SPIN_DEPTH_TARGET, error=0.15),
 Timed(VelocityX(pre_spin_charge_vel), pre_spin_charge_dist),
 Log('Spin Charging...'),
 rolly_roll(),
 Log('Spin Complete, pausing...'),
 Zero(),
 Timer(1),
 Succeed(Timeout(Heading(lambda: saved_heading, error=5), 5)),
 Log('Post Spin Charging...'),
 Conditional(
     main_task=FunctionTask(lambda: gate_elems() == 2),
     on_success=While(
         condition=lambda: gate_elems() != 0,
         task_func=lambda: VelocityX(0.2),
     ),
     on_fail=Timed(VelocityX(post_spin_charge_vel),
                   post_spin_charge_dist),
 ),
 Zero(),
 Log('Restoring heading'),
 Succeed(Timeout(Heading(lambda: saved_heading, error=5), 5)),
 Depth(DEPTH_TARGET, error=0.15),
 Log('Through gate!')),
Esempio n. 17
0
def withApproachAlignOnFail(task):
    return lambda *args, **kwargs: Retry(lambda: Conditional(Timeout(task(*args, **kwargs), 60), on_fail=Fail(Sequential(Zero(), ApproachAlign(), Zero()))), attempts=2)
Esempio n. 18
0
def non_crucifix_task():
    """Do non crucifix task first. If crucifix not found, do marker 2"""
    print('why')
    if crucifix_state == 2:
        return STATES[0]
    return STATES[1]

def crucifix_task():
    """Do crucifix task second. If crucifix not found, do marker 1"""
    print('why2')
    if crucifix_state == 2:
        return STATES[1]
    return STATES[0]

Recovery = lambda: Sequential(
    Succeed(Timeout(CenterAny(), 20)),
    SetMarker('first'),
    Conditional(Timeout(SearchCrucifix(), 30), on_success=Succeed(Sequential(CenterCrucifix(), SetMarker('crucifix'), FunctionTask(lambda: change_state(1)))), on_fail=Succeed()),
    FunctionTask(reflect),
    GoToMarker('second'),
    Timeout(SearchAnyVampire(), 40),  # TODO: MAKE THIS WORK
    Succeed(Timeout(CenterAny(), 40)),
    SetMarker('second'),
    Conditional(FunctionTask(get_crucifix_found), on_success=Log('Skipping searching crucifix because found already'), on_fail=Conditional(Timeout(SearchCrucifix(), 30), on_success=Sequential(SetMarker('crucifix'), FunctionTask(lambda: change_state(2))), on_fail=Succeed())),
    GoToMarker(non_crucifix_task),
    # GrabVampire(),
    ReleaseVampire(lambda: edge(non_crucifix_task())),
    GoToMarker(crucifix_task),
    # GrabVampire(),
    ReleaseVampire(lambda: edge(crucifix_task())),
    Conditional(FunctionTask(get_crucifix_found), on_success=\
Esempio n. 19
0
def TestTimeout():
    return Timeout(Timer(2), 1)