FTC 11138 Premier — INTO THE DEEP

Robot code for FTC team 11138 (Premier), 2024–2025 INTO THE DEEP season. The robot runs two scoring games: it picks samples off the floor and submersible and scores them in the high basket, and it clips specimens onto the high chamber. Both the intake and the autonomous routines are driven by an on-board OpenCV vision pipeline that finds the nearest game piece, measures its angle, and lines the claw up on it.

This document describes how the code is actually put together — the control loop, the hardware abstraction, the subsystems, the command layers, the vision pipeline, and the autonomous path system. Everything below refers to classes in TeamCode/src/main/java/org/firstinspires/ftc/teamcode.

Stack

• FTCLib command-based framework — subsystems, commands, and a command scheduler. All robot behavior is expressed as commands.
• Pedro Pathing for autonomous path following, with a goBILDA Pinpoint computer as the localizer (PinpointLocalizer). Follower and localizer constants live in pedroPathing/constants/FConstants.java and LConstants.java.
• OpenCV through the FTC VisionPortal for sample detection and alignment.
• FTC Dashboard (@Config) so tuning constants can be edited live over Wi-Fi while the robot runs.
• fast-load for faster redeploys during practice.

Control loop

Robot.java is the single owner of hardware and state. It is a singleton (Robot.getInstance()) so any command can reach the follower, the subsystems, or shared state without passing references around. On init it resets the command scheduler, builds the Pedro Pathing Follower with the Pinpoint localizer, and constructs each subsystem. Every subsystem adds itself to a subsystems list in its constructor.

Each loop runs in three phases, kept separate on purpose:

1. updateData() — every subsystem reads its encoders, servos, and sensors and writes the values into RobotData. This is the only place hardware is read.
2. periodic() — every subsystem acts on the current state, then follower.update() runs the drivetrain. This is the only place hardware is written.
3. write() — RobotData dumps the whole robot state to telemetry (loop time, pose, every subsystem state).

Splitting read from write means a command can change a target in one place and trust that every subsystem sees a consistent snapshot when it acts.

Shared state: RobotData

RobotData is the blackboard. It holds every encoder position, servo position, subsystem enum state, the current Pinpoint pose, and the camera outputs (sampleAngle, sampleDx, sampleDy). It also holds three interlock flags — intaking, scoring, and sampleLoaded — with small helpers to set them. Composite commands check these before running so mechanisms don't collide: the sample transfer, for example, only runs when both intaking and scoring are false, so the intake arm never swings while the deposit bucket is up.

Configuration split

Tunables are kept out of the logic classes so they can be changed without touching behavior code, and edited live through FTC Dashboard:

• Constants.java — encoder limits, PIDF-style ratios, servo setpoints, target positions, camera calibration, correction coefficients.
• Configuration.java — the hardware-map name strings.
• PoseConstants.java — absolute field coordinates for the autonomous paths.

Hardware abstraction: RE_DcMotorEx

Every position-controlled motor goes through RE_DcMotorEx instead of the raw SDK motor, which adds three things the mechanisms need:

• Asymmetric power. Up and down travel scale by separate upRatio and downRatio factors, and there is a third slowRatio that kicks in past a slow encoder threshold. On the vertical lift, gravity does most of the work on the way down, so the down and slow ratios pull power back near the bottom of travel instead of letting the lift slam into the frame.
• Soft limits. Targets are clipped to [minPosition, maxPosition], and the periodic() power logic refuses to drive past the limits (unless the global Globals.LIMITS override is off for testing).
• State caching. It tracks lastTarget and only calls setTargetPosition() / RUN_TO_POSITION when the target actually changes, which keeps the control loop from issuing redundant hardware writes every tick.

RE_DcMotorExParams bundles the min/max/slow limits and the three ratios so each mechanism configures its own profile. RE_SubsystemBase is the shared base that gives every subsystem the updateData() / periodic() pair.

Subsystems

• NewIntakeSubsystem — one extension motor (RE_DcMotorEx) plus a five-servo end effector: two arm servos driven together (arm2 reversed and offset from arm1), a claw, a wrist, and a rotate servo. Arm, wrist, claw, and rotation are each a small state machine (ArmState, WristState, ClawState, RotateState); periodic() maps the current state to a tuned servo setpoint. Rotation also has a continuous path — setRotate(angle) converts a measured angle into a servo position through a ticks-per-degree constant — which is what lets the claw snap to the angle the camera reports for a skewed sample.
• DepositSubsystem — the vertical scoring lift. Two motors (lift2 reversed) run as one, both through RE_DcMotorEx with the lift ratios and soft limits; a liftOffset on one side keeps the two strands aligned at height. A bucket servo has its own BucketState (intake / drop / ascent).
• SpecimenSubsystem — an independent lift and claw for pulling specimens off the wall and hanging them on the chamber.
• CameraSubsystem — wraps the VisionPortal and the sample processor, reads out the block angle and pixel offsets, and turns the vertical offset into an extension correction (see the vision section). A second variant, NewTestCameraSubsystem, does the same job by projecting pixels to field meters for a bird's-eye estimate.
• SensorSubsystem — color and distance sensors for confirming a sample is captured.

Command layers

Behavior is built in three layers on top of FTCLib commands.

Primitives (commands/subsystem/) are one-tick InstantCommands that set a single subsystem state — WristStateCommand, LiftPositionCommand, ClawStateCommand, and so on.

Drive commands (commands/drivecommand/) wrap the Pedro Pathing follower so navigation fits inside the command framework. PathCommand and StrafeCommand hand a path to the follower and finish when the follower reports it is done, so the rest of a sequence doesn't have to poll the drivetrain.

Advanced macros (commands/advancedcommand/) sequence primitives into full actions. NewSampleTransferCommand is a good example: it is a ConditionalCommand gated on !intaking && !scoring, and inside it runs a SequentialCommandGroup that moves the wrist to transfer, waits, rotates the claw vertical, waits, brings the arm to the transfer position (only if it isn't already there), marks sampleLoaded, opens the claw to drop the sample into the bucket, then stows the arm, wrist, and rotation. The waits between steps give the servos time to travel so the handoff doesn't jam.

Vision pipeline

The sample processor (NewTestingSampleAngleProcessor) is an OpenCV VisionProcessor that runs on the VisionPortal stream. It is built for the fixed geometry of the game's samples rather than a general object detector, so it stays fast enough to run every frame:

1. Undistort. Calib3d.undistort() corrects lens distortion using a calibrated camera matrix (fX, fY, cX, cY) and five distortion coefficients (k1, k2, p1, p2, k3), all stored in Constants.
2. Perspective warp. getPerspectiveTransform builds a homography from four tuned source corners to a flat 900×600 rectangle, and warpPerspective projects the frame into that top-down view so a sample's pixel geometry maps cleanly to its real orientation.
3. Alliance-aware masking. The frame is converted to HSV and thresholded for yellow plus either red or blue, chosen from Globals.ALLIANCE, so opposing alliance pieces are ignored. Red uses two hue ranges to cover the HSV wrap-around. Erode then dilate cleans up the mask.
4. Contour geometry. Contours over 500 px in area are fit with minAreaRect, and the rect closest to frame center is chosen. The angle is normalized (+90 when width < height, folded into [0, 180)) so it always describes the sample's long axis, and the center offset from frame center is reported as dx, dy.

Two consumers act on those outputs:

• Rotation — RotateAlignCommand takes the reported angle and snaps the rotate servo to it through the intake's continuous-angle path, so the claw meets the sample square regardless of how it landed.
• Extension — CameraSubsystem converts the vertical pixel offset dy into an extension-motor tick correction using a fourth-degree polynomial in dy (four tuned multipliers in Constants) plus piecewise offset corrections for near/far bands. ExtensionJumpCommand applies that correction, so the claw reaches the right distance without the driver inching the robot in.

Autonomous

Autos are single SequentialCommandGroups that interleave driving and mechanism work. Because drive commands and mechanism commands are both FTCLib commands, .alongWith() runs them together — a routine drives toward a sample while the intake is already deploying, so the arm is over the sample as the robot settles instead of after.

Paths are generated by AutonomousMethods, a set of factory methods that build Pedro Pathing Paths from BezierLines and two-, three-, or four-point BezierCurves. Each factory applies setLinearHeadingInterpolation over the path's t domain (0→1), so the robot sweeps its heading continuously as it translates instead of stopping to turn; ...TurnLater reversed variants exist for approaches that should hold heading and rotate late. Field coordinates for every path come from PoseConstants.

The routines cover both games and several field starts:

• Sample side (Auto_5Plus0, Auto_5Plus0_Reverse, Auto_0PickSub, Auto_5Plus0_SubPart) — score the preload, run vision-aligned pickups on the spike-mark samples, and pull a sample from the submersible.
• Specimen side (Auto_0Plus5, Auto_0Plus5NEW, Auto_0Plus4) — cycle specimens onto the chamber. Scoring uses SpecimenDepositCommand, which drops position control and instead drives the specimen lift down at a fixed raw power for a tuned time, seating the clip onto the bar by force so it latches reliably. The wall-pickup steps leave the claw open across a wide window so a human player has room to place the specimen.
• Pose trackers (Auto_PoseTracker_Sample, Auto_PoseTracker_Specimen) for logging and validating localization against the planned paths.

TeleOp

TeleOp_Solo (and TeleOp_Solo_New) run the robot from a single driver by binding the advanced macros to buttons — one press runs the whole transfer or deploys the intake, so the driver isn't sequencing servos by hand. The drivetrain toggles between field-centric and robot-centric (Constants.robotCentric).

Two feedback channels keep the driver's eyes on the field: the gamepad rumbles on key transitions (a short pulse on most actions, a long one on others), and the gamepad LED shows drive mode — red for robot-centric, blue for field-centric, and a green flash on reset actions. Vision assists are wired in too: RotateAlignCommand snaps the claw to the camera's measured sample angle, and ExtensionJumpCommand uses the camera offset to set extension distance. A two-driver TeleOp_Duo is also included.

Layout

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/
├── hardware/
│   ├── Robot.java               # singleton registry + control loop
│   ├── RobotData.java           # shared state, interlocks, telemetry
│   └── subsystems/              # intake, deposit, specimen, camera, sensor
├── commands/
│   ├── subsystem/               # one-tick setpoint primitives
│   ├── drivecommand/            # Pedro Pathing wrappers
│   └── advancedcommand/         # sequenced macros
├── opmode/
│   ├── TeleOp_Solo / _New / _Duo
│   └── auto/                    # routines + AutonomousMethods
├── vision/                      # OpenCV VisionProcessors
├── pedroPathing/                # follower/localizer constants, tuners
└── util/
    ├── Constants / Configuration / PoseConstants / Globals
    └── wrappers/                # RE_DcMotorEx, RE_SubsystemBase

Building

Open the project in Android Studio and deploy the TeamCode module to the REV Control Hub. OpModes appear on the Driver Station under the names in their @TeleOp / @Autonomous annotations. Pedro Pathing and the vision pipeline are tuned through the constants files and FTC Dashboard; the pedroPathing/tuners_tests OpModes are the standard Pedro tuning routines for localization and following.