01 | BUILD A STATION YOU CAN MEASURE

A practice docking station gives students a repeatable target: identify a marker, align with it, approach slowly and stop at a measured gap.

The example is non-contact docking. It stops with space between the robot’s front edge and a board. It does not latch a mechanism or prove successful loading.

Combine 08.3’s detection gate, 08.4’s image alignment and 08.5’s calibrated size check with independent forward distance feedback.

02 | PLACE UNIQUE PRACTICE MARKERS

Assign a distinct supported AprilTag ID to each station. Keep a record of its print size, location, height and orientation. Use the verified VEX marker set.

Keep markers flat, clearly printed and unobstructed. Match camera height and mounting so the marker stays visible throughout the planned approach.

These are classroom practice modifications. Before adding markers to an event field or using a configuration in competition, check the applicable event rules. This lesson does not assume practice additions are permitted there.

03 | CONTROL THE ENTRY CONDITIONS

Begin inside a tested approach region, already approximately facing the board. Check the whole corridor and swept turning footprint for the chassis and attached mechanisms.

The routine is a local docking controller. It does not navigate from anywhere on the field, plan around obstacles or resolve a large sideways offset.

Place the range sensor so it sees the board during the approach. A front beam cannot identify which surface returned a range or clear the whole robot footprint.

04 | GIVE EACH SENSOR A CLEAR ROLE

The starter requires a forward range return. Missing range data stops it; no detected object is not treated as a clear corridor.

05 | CONVERT RANGE INTO FRONT-EDGE GAP

If the Distance Sensor is 100 mm behind the frontmost edge, subtract that setback from the measured surface range.

front_gap_mm = measured_range_mm - SENSOR_TO_FRONT_MM

The example wants a nominal 250 mm gap, with a ±25 mm finish window. It also stops at or below an emergency threshold of 120 mm, which is a last stop condition, not an assured collision barrier.

Validate offsets, sensor uncertainty, braking travel and latency at the chosen speed. Include anything projecting beyond the chassis in the front-edge reference.

06 | ACQUIRE, ALIGN, APPROACH AND SETTLE

Translation is allowed only within ±12 pixels of the aim point. Finish requires the tighter ±6 pixel centring window.

While inside the gap window but outside the centring window, the routine turns without translation. The turning footprint still needs verified clearance.

07 | SLOW AS THE GAP ERROR SHRINKS

gap_error = front_gap_mm - DESIRED_GAP_MM
base = min(12.0, 0.05 * gap_error)
left = base + turn
right = base - turn

This is a forward-only proportional approach. At gap 500 mm, error is 250 mm and the forward request caps at 12%. At gap 300 mm, it is 2.5%.

If already closer than the lower finish bound, the controller stops rather than reversing. A reverse escape requires its own rearward clearance policy.

A low request can stall against friction. Poor sensing or braking can still cause overshoot. Test conservative caps and tune from measured trials.

08 | USE TAG SIZE AS A CROSS-CHECK

When reasonably aligned, estimate lens-to-tag depth from the validated K / width model, subtract the camera’s setback and compare it with the range sensor’s front-edge gap.

The example rejects widths outside the calibrated interval and disagreement greater than 100 mm. The threshold is a teaching value to validate against observed errors.

The camera and range beam must refer to the same station plane under the calibrated geometry. Similar readings do not prove that both sensors observed the intended surface.

STATION_VERIFIED and CALIBRATION_VERIFIED begin False. The sample K is illustrative and must be replaced before enabling this controller.

09 | COMPLETE PRACTICE DOCKING STARTER

Configure AI Vision on port 15 as ai_vision_1 with tag detection enabled. Manual devices: Distance Sensor 16, inertial 20, left motors 3/4 and right motors 1/2.

Keep generated VEX imports and Brain above this program. Avoid duplicate device declarations. Verify drive directions, sensor offsets, calibration and the practice station before enabling the three setup flags.

# =============================================================================
# TEAM [MY TEAM NAME / NUMBER] | PRACTICE-FIELD NON-CONTACT DOCKING
# =============================================================================
# Keep generated imports, brain and camera/signature setup above this code.
# Configure AI Vision in Devices on port 15, named ai_vision_1.
# Enable AprilTag detection; keep generated camera configuration above this code.
# Manual drivetrain/inertial configuration below; do not duplicate in Devices.
import math

# =============================================================================
# FOUR-MOTOR DRIVETRAIN AND INERTIAL
# =============================================================================
left_1 = Motor(Ports.PORT3, GearSetting.RATIO_6_1, False)
left_2 = Motor(Ports.PORT4, GearSetting.RATIO_6_1, False)
right_1 = Motor(Ports.PORT1, GearSetting.RATIO_6_1, True)
right_2 = Motor(Ports.PORT2, GearSetting.RATIO_6_1, True)
left_drive = MotorGroup(left_1, left_2)
right_drive = MotorGroup(right_1, right_2)
inertial = Inertial(Ports.PORT20)
controller = Controller(PRIMARY)

# =============================================================================
# SETTINGS: EXAMPLES TO CHECK AND TUNE
# =============================================================================
front_distance = Distance(Ports.PORT16)
TARGET_TAG_ID = 7
AIM_X_PX = 160.0
LOSS_TIMEOUT_MS = 500
MOTION_CONFIGURED = False
MIN_TAG_WIDTH_PX = 12
MIN_TAG_HEIGHT_PX = 12
MAX_CENTRE_JUMP_PX = 60
STEER_KP = 0.12             # percent per horizontal pixel
MAX_TURN_PERCENT = 10.0
STEERING_SIGN = 1.0         # Verify: right-image target must turn clockwise
CENTRE_TOL_PX = 6
CONFIRM_SAMPLES = 3
SETTLE_MS = 250
MAX_SETTLE_RATE_DEG_S = 8.0
POLL_MS = 50
MAX_UPDATE_GAP_MS = 200
DOCK_TIMEOUT_MS = 12000
ready = False

# =============================================================================
# PRACTICE STATION AND RANGE SETTINGS; ALL REQUIRE PHYSICAL VALIDATION
# =============================================================================
STATION_VERIFIED = False
CALIBRATION_VERIFIED = False
SENSOR_TO_FRONT_MM = 100.0
CAMERA_TO_FRONT_MM = 100.0
DESIRED_GAP_MM = 250.0
GAP_TOL_MM = 25.0
EMERGENCY_GAP_MM = 120.0
DRIVE_ALIGN_PX = 12.0
MAX_FORWARD_PERCENT = 12.0
RANGE_KP = 0.05
RANGE_RATE_TOL_MM_S = 30.0
CAL_K_MM_PX = 30000.0        # Replace with your verified tag calibration
CAL_MIN_WIDTH_PX = 30.0
CAL_MAX_WIDTH_PX = 120.0
MAX_RANGE_DISAGREEMENT_MM = 100.0
IMAGE_WIDTH = 320
IMAGE_HEIGHT = 240
def clamp(value, low, high):
    return max(low, min(high, value))

def stop_drive():
    left_drive.stop(BRAKE)
    right_drive.stop(BRAKE)

def side_output(group, output):
    if output == 0:
        group.stop(BRAKE)
    else:
        group.spin(FORWARD if output > 0 else REVERSE, abs(output), PERCENT)

def steering_request(center_x):
    error = center_x-AIM_X_PX
    if abs(error) <= CENTRE_TOL_PX:
        return 0.0, error
    return clamp(STEERING_SIGN*STEER_KP*error,
                 -MAX_TURN_PERCENT, MAX_TURN_PERCENT), error

def select_expected_tag(objects):
    visible_ids, matches = [], []
    for obj in objects:
        if not obj.exists:
            continue
        identity = obj.id
        if not math.isfinite(identity) or identity < 0 or int(identity) != identity:
            raise ValueError("Invalid tag ID")
        visible_ids.append(int(identity))
        if int(identity) != TARGET_TAG_ID:
            continue
        values = (obj.centerX, obj.centerY, obj.width, obj.height)
        if not all(math.isfinite(v) for v in values):
            raise ValueError("Invalid target box")
        x, y, width, height = values
        if not (0 <= x <= IMAGE_WIDTH and 0 <= y <= IMAGE_HEIGHT
                and 0 < width <= IMAGE_WIDTH and 0 < height <= IMAGE_HEIGHT):
            raise ValueError("Target box outside image bounds")
        matches.append({"id": int(identity), "x": x, "y": y,
                        "width": width, "height": height})
    if len(matches) > 1:
        return "AMBIGUOUS_TAG", visible_ids, None
    if not matches:
        return ("WRONG_TAG_ONLY" if visible_ids else "NO_TAGS"), visible_ids, None
    tag = matches[0]
    if tag["width"] < MIN_TAG_WIDTH_PX or tag["height"] < MIN_TAG_HEIGHT_PX:
        return "TARGET_TOO_SMALL", visible_ids, None
    return "VALID_TARGET", visible_ids, tag

class TagGate:
    def __init__(self):
        self.streak = 0
        self.current = None
        self.last_seen_ms = None
        self.last_good = None   # History only: never returned as current.

    def update(self, observation, target, now):
        self.current = None     # Invalidate before processing EVERY observation.
        if observation != "VALID_TARGET":
            self.streak = 0
            return observation
        self.streak += 1
        self.last_seen_ms = now
        self.last_good = dict(target)
        if self.streak < CONFIRM_SAMPLES:
            return "ACQUIRING"
        self.current = dict(target)
        return "READY"

    def history_age(self, now):
        return None if self.last_seen_ms is None else now-self.last_seen_ms

def read_front_gap():
    if not front_distance.is_object_detected():
        raise ValueError("No usable forward distance return")
    distance = front_distance.object_distance(MM)
    if not math.isfinite(distance) or not 20 <= distance <= 2000:
        raise ValueError("Unusable forward range")
    return distance-SENSOR_TO_FRONT_MM

def docking_output(tag, gap):
    turn, error_x = steering_request(tag["x"])
    if gap <= EMERGENCY_GAP_MM:
        return "EMERGENCY_CLEARANCE", 0.0, 0.0, False
    if gap < DESIRED_GAP_MM-GAP_TOL_MM:
        return "TOO_CLOSE_STOP", 0.0, 0.0, False
    aligned = abs(error_x) <= DRIVE_ALIGN_PX
    if aligned:
        if not CAL_MIN_WIDTH_PX <= tag["width"] <= CAL_MAX_WIDTH_PX:
            return "SIZE_OUTSIDE_CALIBRATION", 0.0, 0.0, False
        visual_gap = CAL_K_MM_PX/tag["width"]-CAMERA_TO_FRONT_MM
        if abs(visual_gap-gap) > MAX_RANGE_DISAGREEMENT_MM:
            return "RANGE_DISAGREEMENT", 0.0, 0.0, False
    near = abs(gap-DESIRED_GAP_MM) <= GAP_TOL_MM
    if near and abs(error_x) <= CENTRE_TOL_PX:
        return "SETTLE", 0.0, 0.0, True
    base = 0.0 if not aligned or near else min(
        MAX_FORWARD_PERCENT, RANGE_KP*(gap-DESIRED_GAP_MM))
    left, right = base+turn, base-turn
    scale = max(1.0, abs(left)/20.0, abs(right)/20.0)
    return "APPROACH" if base > 0 else "ALIGN", left/scale, right/scale, False

# =============================================================================
# ONE CALLBACK OWNS ACQUISITION, ALIGNMENT, APPROACH AND SETTLING
# =============================================================================
def dock_to_station(timeout_ms=DOCK_TIMEOUT_MS):
    stop_drive()
    gate = TagGate()
    try:
        if not STATION_VERIFIED or not CALIBRATION_VERIFIED:
            return "STATION_OR_CALIBRATION_NOT_VERIFIED"
        values = (timeout_ms, SENSOR_TO_FRONT_MM, CAMERA_TO_FRONT_MM,
                  DESIRED_GAP_MM, GAP_TOL_MM, EMERGENCY_GAP_MM, DRIVE_ALIGN_PX,
                  MAX_FORWARD_PERCENT, RANGE_KP, RANGE_RATE_TOL_MM_S,
                  CAL_K_MM_PX, CAL_MIN_WIDTH_PX, CAL_MAX_WIDTH_PX,
                  MAX_RANGE_DISAGREEMENT_MM, TARGET_TAG_ID, AIM_X_PX,
                  MIN_TAG_WIDTH_PX, MIN_TAG_HEIGHT_PX, CONFIRM_SAMPLES,
                  MAX_CENTRE_JUMP_PX, STEER_KP, MAX_TURN_PERCENT, STEERING_SIGN,
                  CENTRE_TOL_PX, SETTLE_MS, MAX_SETTLE_RATE_DEG_S,
                  POLL_MS, MAX_UPDATE_GAP_MS, LOSS_TIMEOUT_MS)
        if (not all(math.isfinite(v) for v in values) or timeout_ms <= 0
                or min(SENSOR_TO_FRONT_MM, CAMERA_TO_FRONT_MM) < 0
                or not 0 < EMERGENCY_GAP_MM < DESIRED_GAP_MM-GAP_TOL_MM
                or GAP_TOL_MM <= 0 or not 0 < MAX_FORWARD_PERCENT <= 20
                or RANGE_KP <= 0 or RANGE_RATE_TOL_MM_S < 0
                or CAL_K_MM_PX <= 0 or not 0 < CAL_MIN_WIDTH_PX < CAL_MAX_WIDTH_PX <= 320
                or MAX_RANGE_DISAGREEMENT_MM <= 0
                or not 0 <= TARGET_TAG_ID <= 37 or int(TARGET_TAG_ID) != TARGET_TAG_ID
                or not 0 < AIM_X_PX < 320 or min(MIN_TAG_WIDTH_PX,MIN_TAG_HEIGHT_PX) <= 0
                or CONFIRM_SAMPLES < 1 or int(CONFIRM_SAMPLES) != CONFIRM_SAMPLES
                or STEERING_SIGN not in (-1,1) or STEER_KP <= 0
                or not 0 < MAX_TURN_PERCENT <= 20 or MAX_CENTRE_JUMP_PX <= 0
                or not 0 < CENTRE_TOL_PX <= DRIVE_ALIGN_PX < 160
                or SETTLE_MS <= 0 or MAX_SETTLE_RATE_DEG_S < 0
                or not 0 < POLL_MS < MAX_UPDATE_GAP_MS or LOSS_TIMEOUT_MS <= 0):
            return "INVALID_SETTINGS"
        started = previous_ms = brain.timer.time(MSEC)
        previous_rotation = previous_gap = previous_x = None
        settled_since = None
        terminal = ("EMERGENCY_CLEARANCE", "TOO_CLOSE_STOP",
                    "SIZE_OUTSIDE_CALIBRATION", "RANGE_DISAGREEMENT")
        while True:
            gate.current = None
            if (not ready or not MOTION_CONFIGURED or not competition.is_enabled()
                    or not competition.is_autonomous() or controller.buttonB.pressing()):
                return "CANCELLED_OR_NOT_READY"
            if (not ai_vision_1.installed() or not inertial.installed()
                    or inertial.is_calibrating()):
                return "SENSOR_NOT_READY"
            if brain.timer.time(MSEC)-started >= timeout_ms:
                return "TIMEOUT"
            objects = ai_vision_1.take_snapshot(AiVision.ALL_TAGS, 24)
            gap = read_front_gap()  # Missing range is a fault, not clear space.
            rotation = inertial.rotation(DEGREES)
            now = brain.timer.time(MSEC)
            if (not competition.is_enabled() or not competition.is_autonomous()
                    or controller.buttonB.pressing()):
                return "CANCELLED_OR_NOT_READY"
            if now-started >= timeout_ms:
                return "TIMEOUT"
            if now-previous_ms > MAX_UPDATE_GAP_MS:
                return "UPDATE_GAP"
            if not math.isfinite(rotation):
                return "INVALID_ROTATION"
            if gap <= EMERGENCY_GAP_MM:
                return "EMERGENCY_CLEARANCE"
            dt = (now-previous_ms)/1000.0
            slow = (previous_rotation is not None and previous_gap is not None and dt > 0
                    and abs(rotation-previous_rotation)/dt <= MAX_SETTLE_RATE_DEG_S
                    and abs(gap-previous_gap)/dt <= RANGE_RATE_TOL_MM_S)
            previous_rotation, previous_gap, previous_ms = rotation, gap, now
            observation, ids, target = select_expected_tag(objects)
            if observation == "AMBIGUOUS_TAG":
                return observation
            if (target is not None and previous_x is not None
                    and abs(target["x"]-previous_x) > MAX_CENTRE_JUMP_PX):
                return "TARGET_JUMP"
            gate.update(observation, target, now)
            if target is not None:
                previous_x = target["x"]
            if gate.current is None:
                stop_drive()
                settled_since = None
                if target is None:
                    previous_x = None
                age = gate.history_age(now)
                if age is not None and age >= LOSS_TIMEOUT_MS:
                    return "TARGET_LOST_TIMEOUT"
                wait(POLL_MS, MSEC)
                continue
            state, left, right, near = docking_output(gate.current, gap)
            if state in terminal:
                return state
            if near:
                stop_drive()
                if slow:
                    if settled_since is None:
                        settled_since = now
                    if now-settled_since >= SETTLE_MS:
                        return "DOCKED_ESTIMATE"
                else:
                    settled_since = None
            else:
                settled_since = None
                side_output(left_drive, left)
                side_output(right_drive, right)
            wait(POLL_MS, MSEC)
    except Exception:
        return "DOCK_SENSOR_OR_COMMAND_FAULT"
    finally:
        gate.current = None
        stop_drive()

# =============================================================================
# COMPETITION CALLBACKS
# =============================================================================
def autonomous():
    stop_drive()
    if ready and MOTION_CONFIGURED:
        print("practice docking", dock_to_station())

def driver_control():
    stop_drive()  # Teaching project: no driver movement.

competition = Competition(driver_control, autonomous)
stop_drive()
if MOTION_CONFIGURED and not competition.is_enabled() and inertial.installed():
    inertial.calibrate()  # Stationary disabled setup; may block.
    ready = not inertial.is_calibrating() and not competition.is_enabled()
while True:
    wait(100, MSEC)

One shared twelve-second deadline covers acquisition, alignment, approach, loss handling and settling. There is no second movement thread or blind search.

The routine checks mode, B and the deadline again after sensor calls. Timing checks cannot interrupt an API call that remains blocked.

10 | DEFINE WHAT DOCKED MEANS

DOCKED_ESTIMATE requires current confirmed tag data, image error within ±6 pixels, measured gap within ±25 mm, low inertial turn rate and low range change for 250 ms.

Low range change is not a complete ground-speed measurement. The robot could slide sideways without much depth change. Measure actual position and orientation in your trial log.

Centring a tag does not make the chassis perpendicular to its board. Start close to the required approach orientation; a more demanding dock needs additional pose or alignment feedback.

This finish status describes sensor conditions at a stop. Contact, mechanism engagement or scoring needs a separate verified action.

11 | STOP CONDITIONS ARE PART OF THE DESIGN

On the first unusable tag observation, the drivetrain brakes and settling resets. Brief stationary reacquisition can resume only after confirmation is rebuilt.

12 | RUN A REPEATABLE PRACTICE COURSE

1. Verify tag and range readings with movement disabled.
2. Calibrate marker size using 08.5.
3. Test turning without forward movement.
4. Try a short straight approach at a low cap.
5. Repeat from small left and right entry errors.
6. Test target loss, wrong IDs and range disagreement in simulation.
7. Repeat physical trials under changed lighting and battery state.

Record success status, actual gap, lateral offset, board-relative orientation, duration and faults. Use physical measurements to check whether DOCKED_ESTIMATE corresponds to the task you intended.

CHALLENGES | DESIGN A RELIABLE PRACTICE DOCK

1. Build: place two uniquely identified station boards and mark entry regions.
2. Calculate: convert a 430 mm sensor reading with 100 mm setback into front-edge gap.
3. Predict: find forward requests at gaps 500 and 300 mm.
4. Check: compare visual and sensor gaps using your measured K.
5. Mock: verify immediate braking on tag loss and missing range.
6. Test: measure five approaches from the same start.
7. Evaluate: explain why centred image plus stable depth does not prove square-on docking.
8. Extend: specify a separate feedback check for a successful loading or latching action.

OFFICIAL REFERENCES

• VEX AprilTag setup and supported markers
• AI Vision Python API
• Distance Sensor detection and range API

The docking policy, station layout and starter are independently written teaching examples requiring physical validation.