This is the simplest autonomous program: the robot simply drives straight + * forward to the parking zone and stops. It is used when the field-side + * ("far") starting position is used and there is no time to implement a more + * complex autonomous routine, or as a guaranteed scoring fallback when more + * complex autos are unreliable. + * + *
The robot starts at field position (0, 0) facing 0° (right), then + * drives straight to ({@link #PARK_X}, {@link #PARK_Y}). Both coordinates + * are tunable at runtime via FTC Dashboard (the class is annotated with + * {@link Config}). + * + *
Registered as {@code "Far Leave Auto"} in the {@code "Autonomous"} group. + */ @Config @Autonomous(name = "Far Leave Auto", group = "Autonomous") public class FarLeave extends LinearOpMode { + + /** + * Target X-coordinate (inches) for the parking position. + * Positive X moves the robot forward (away from the driver station wall). + * Default of 26 inches is enough to cross the field boundary line. + */ public static double PARK_X = 26; + + /** + * Target Y-coordinate (inches) for the parking position. + * Zero keeps the robot on the same lateral track as the starting position. + * Adjust if the robot needs to dodge field elements while leaving. + */ public static double PARK_Y = 0; + /** + * Entry point called by the FTC runtime. + * Builds the park trajectory, waits for start, then executes it. + * + * @throws InterruptedException (implicitly) if the OpMode is interrupted + */ @Override public void runOpMode() { + // Define the starting pose: origin (0,0) facing 0° (pointing in +X direction). Pose2d startPose = new Pose2d(0, 0, Math.toRadians(0)); + // Construct all hardware subsystems via the Hardware container. + // Hardware also builds the MecanumDrive and BotActions. Hardware hardware = new Hardware(hardwareMap, telemetry, this, startPose); - BotActions botActions = hardware.actions; - MecanumDrive drive = hardware.mecanumDrive; + BotActions botActions = hardware.actions; // action factory (unused here but available) + MecanumDrive drive = hardware.mecanumDrive; // Road Runner drive for trajectory building + // The destination pose: (PARK_X, PARK_Y) still facing 0°. Pose2d parkPose = new Pose2d(PARK_X, PARK_Y, Math.toRadians(0)); + // Build the single-segment trajectory: strafe straight from startPose to parkPose. + // strafeTo drives along a straight line while maintaining the current robot heading. Action toPark = drive.actionBuilder(startPose) .strafeTo( - parkPose.position + parkPose.position // target x,y; heading is held constant at 0° ) .build(); - waitForStart(); - if (isStopRequested()) return; + waitForStart(); // block here until the referee presses PLAY + if (isStopRequested()) return; // abort immediately if stop was pressed during init + // Execute the parking trajectory. + // ParallelAction with a single child is equivalent to running that child directly; + // the wrapper is kept here for structural consistency with other autonomous OpModes. Actions.runBlocking( new ParallelAction( new SequentialAction( - toPark + toPark // drive forward to the parking position ) ) ); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auto/opmodes/FasterBlueClose.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auto/opmodes/FasterBlueClose.java index f57d6dfe13db..31f751b7ba59 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auto/opmodes/FasterBlueClose.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auto/opmodes/FasterBlueClose.java @@ -15,113 +15,285 @@ import org.firstinspires.ftc.teamcode.auto.roadrunner.miscRR.MecanumDrive; import org.firstinspires.ftc.teamcode.subsystems.Indexer; +/** + * FasterBlueClose — Blue Alliance, Close-Side Autonomous WITHOUT Motif (No Color Matching) + * + *
Alliance: Blue | Starting Position: Close side, starting at + * (0, 0, 0°) — facing positive-X (field-forward).
+ * + *This is the blue alliance mirror of {@link FasterRedClose}. The robot: + *
All trajectories mirror the red no-motif variant geometrically: intake corridors sweep + * toward positive X (blue field wall), the shooting pose is at negative X, and the gate + * dodge arc angles in the opposite direction. The timing parameters are identical to the + * red variant.
+ * + *Execution Order: + *
This is the primary high-cycle autonomous OpMode for Team 5273 "Decode" on the + * Blue Alliance, close (front) side of the field. It implements a full + * three-cycle intake-and-shoot routine combined with Limelight-based Motif (AprilTag / + * colored-pattern) detection so the robot scores artifacts in the correct scoring positions.
+ * + *Road Runner's {@link ParallelAction} is used extensively so that the flywheel + * spin-up, indexer rotation, and drivetrain movement all happen simultaneously, saving + * several seconds of cycle time. The {@link #timeUntilStartOuttake} constant is the + * critical tuning value that determines when the outtake sequence begins relative to the + * start of the drive segment – too early and the robot fires before reaching the goal; + * too late and time is wasted waiting.
+ * + *All {@code public static} fields are exposed to FTC Dashboard via the {@link Config} + * annotation, enabling real-time tuning without recompiling:
+ *All trajectory construction and action composition happens here before + * {@code waitForStart()} so that the robot is ready to execute instantly when the + * match begins. After the start signal the pre-built action tree is handed to + * {@link Actions#runBlocking} which drives the robot through the entire sequence.
+ * + *Threading note: Road Runner actions run on the main loop thread. + * {@link ParallelAction} interleaves actions by calling each one's {@code run()} + * method every loop iteration – they are cooperative, not truly parallel threads.
+ */ @Override public void runOpMode() { + // The robot starts facing 180° (toward the opposing alliance wall on the blue side). + // All coordinates below are relative to this starting pose. Pose2d startPose = new Pose2d(0, 0, Math.toRadians(180)); + // Initialise all hardware (motors, servos, sensors) using the FTC hardwareMap. + // The Hardware class also wires up the Road Runner localizer with the start pose. Hardware hardware = new Hardware(hardwareMap, telemetry, this, startPose); + + // BotActions provides high-level composite actions (intake, outtake, indexer, etc.) BotActions botActions = hardware.actions; + + // MecanumDrive provides trajectory building and following for the mecanum drivetrain. MecanumDrive drive = hardware.mecanumDrive; + // Intermediate scan waypoint: the robot passes close to the Obelisk structure so + // the Limelight camera captures the Motif (AprilTag-like pattern) and determines + // which slot colour should be shot first. Pose2d obeliskPose = new Pose2d( OBELISK_X, OBELISK_Y, Math.toRadians(OBELISK_HEADING_DEG) ); + // The pose where the robot stops (or near-stops) to fire artifacts into the goal. + // The heading is angled at -45° to aim the flywheel toward the high scoring target. Pose2d shootingPose = new Pose2d( SHOOT_X, SHOOT_Y, Math.toRadians(SHOOT_HEADING_DEG) ); + // Intake row 1: start and end poses. The robot faces 0° (toward the field centre) + // during the intake sweep so the intake funnel is in the correct orientation. Pose2d intake1PoseStart = new Pose2d(INTAKE_START_X, INTAKE1_Y, Math.toRadians(0)); Pose2d intake1PoseEnd = new Pose2d(INTAKE_END_X, INTAKE1_Y, Math.toRadians(0)); + + // Gate pose (90° heading = robot facing sideways toward the gate mechanism). + // Used in the currently bypassed goToGate action. Pose2d gate = new Pose2d(gate_X, gate_Y, Math.toRadians(90)); + // Intake row 2 poses. The start X is shifted forward by INTAKE2_START_OFFSET_X + // because row 2 artifacts are positioned deeper in the field. Pose2d intake2PoseStart = new Pose2d(INTAKE_START_X + INTAKE2_START_OFFSET_X, INTAKE2_Y, Math.toRadians(0)); Pose2d intake2PoseEnd = new Pose2d(INTAKE_END_X + intake_END_2And3_XOffset, INTAKE2_Y, Math.toRadians(0)); + + // Intermediate waypoint used in backToShoot2 to steer around the physical gate + // obstacle that sits between the intake end and the shooting pose. The robot + // first drives to this dodge point before curving toward the shooting pose. Pose2d dodgeGate = new Pose2d(INTAKE_END_X + intake_END_2And3_XOffset - 16, INTAKE2_Y + 4, Math.toRadians(20)); + // Intake row 3 poses – the deepest intake position on the field. Pose2d intake3PoseStart = new Pose2d(INTAKE_START_X + INTAKE3_START_OFFSET_X, INTAKE3_Y, Math.toRadians(0)); Pose2d intake3PoseEnd = new Pose2d(INTAKE_END_X + intake_END_2And3_XOffset, INTAKE3_Y, Math.toRadians(0)); + // Final park position after all shooting cycles are complete. Pose2d parkPose = new Pose2d(PARK_X, PARK_Y, Math.toRadians(0)); // Remove actionStartOuttake in toShoot when adding this + // Startup parallel action: initialise the indexer to slot two (pre-loaded artifact) + // while simultaneously spinning the flywheel up to shoot RPM, saving ~1 second. Action start = new ParallelAction( - botActions.initializeAuto(Indexer.IndexerState.two), - botActions.actionStartOuttake(SHOOT_RPM) + botActions.initializeAuto(Indexer.IndexerState.two), // rotate indexer drum to slot 2 + botActions.actionStartOuttake(SHOOT_RPM) // spin flywheel to target RPM ); /* @@ -108,70 +263,92 @@ public void runOpMode() { */ // BE CAREFUL ABOUT THE OBELISK SCANNING AND COLOR ROTATING TIMING + // Drive from start → obelisk scan waypoint → shooting pose, while simultaneously: + // • spinning up the flywheel + // • waiting for the obelisk ID to be detected, then rotating the indexer to the + // correct colour slot, and finally firing the first quick outtake + // The 0.5-second lead time on the sleep ensures the indexer finishes rotating + // before the robot fully arrives at the shooting pose. Action toShoot = new ParallelAction( drive.actionBuilder(startPose) // obeliskPose - .strafeTo(obeliskPose.position) - .strafeToSplineHeading(shootingPose.position, shootingPose.heading) + .strafeTo(obeliskPose.position) // pass near obelisk for camera scan + .strafeToSplineHeading(shootingPose.position, shootingPose.heading) // curve into shooting pose .build(), - botActions.actionStartOuttake(SHOOT_RPM), + botActions.actionStartOuttake(SHOOT_RPM), // spin flywheel in parallel with driving new SequentialAction( - new SleepAction(timeUntilStartOuttake - 0.5), - botActions.rotateToMotifColorBeforeOuttake(0, botActions::getObeliskId, 2), - new SleepAction(0.5), - botActions.actionQuickOuttake() + new SleepAction(timeUntilStartOuttake - 0.5), // wait until robot is nearly at goal + botActions.rotateToMotifColorBeforeOuttake(0, botActions::getObeliskId, 2), // rotate indexer slot 0 to Motif-detected colour + new SleepAction(0.5), // brief pause to let indexer settle + botActions.actionQuickOuttake() // fire first artifact ) ); + // First intake sweep: uses sensor feedback (actionIntakeThreeFeedback) to drive + // from the shooting pose into row 1 and back-detect when three artifacts are loaded. Action intake1 = botActions.actionIntakeThreeFeedback(shootingPose, intake1PoseStart, intake1PoseEnd, drive, maxIntakeDrivingVel); + // Build a trajectory to the gate position and wait there (bypassed in the active run). + // Kept for reference / future use if the gate strategy is re-enabled. Action goToGate = drive.actionBuilder(intake1PoseEnd) - .strafeToLinearHeading(gate.position, gate.heading) - .waitSeconds(gateWaitTime) + .strafeToLinearHeading(gate.position, gate.heading) // drive to gate with final linear heading + .waitSeconds(gateWaitTime) // wait at gate (e.g. for gate to open) .build(); + // Return from intake row 1 to shooting pose. The heading is slightly offset from + // the base shooting heading to compensate for drift accumulated during intake cycle 1. + // Parallel: spin up flywheel, then after delay rotate indexer slot 1 to correct + // colour, wait for robot to arrive, fire second artifact, set intake to passive. Action backToShoot1 = new ParallelAction( drive.actionBuilder(intake1PoseEnd) // goToGate .strafeToSplineHeading(shootingPose.position, shootingPose.heading.plus(Math.toRadians(SHOOT_HEADING_OFFSET_AFTER_FIRSTSHOT))) .build(), //botActions.actionSetIntakeReverse(), - botActions.actionStartOuttake(SHOOT_RPM), + botActions.actionStartOuttake(SHOOT_RPM), // re-spin flywheel (may have slowed during intake) new SequentialAction( - new SleepAction(timeUntilStartOuttake - 1), - botActions.rotateToMotifColorBeforeOuttake(1, botActions::getObeliskId, 2), - new SleepAction(1), - botActions.actionQuickOuttake(), - botActions.actionSetIntakePassive() + new SleepAction(timeUntilStartOuttake - 1), // wait for robot to near shooting pose + botActions.rotateToMotifColorBeforeOuttake(1, botActions::getObeliskId, 2),// rotate indexer slot 1 to correct colour + new SleepAction(1), // wait for indexer to settle + botActions.actionQuickOuttake(), // fire second artifact + botActions.actionSetIntakePassive() // set intake to passive (no active spin) ready for next cycle ) ); + // Second intake sweep: row 2 (deeper in field). Uses the same sensor-feedback + // approach; intake start/end X offsets account for the deeper artifact positions. Action intake2 = botActions.actionIntakeThreeFeedback(shootingPose, intake2PoseStart, intake2PoseEnd, drive, maxIntakeDrivingVel); + // Return from intake row 2 to shooting pose, routing through dodgeGate waypoint + // to avoid the physical gate obstacle. Uses strafeTo for the dodge, then + // strafeToSplineHeading for a smooth curve into the shooting heading. Action backToShoot2 = new ParallelAction( drive.actionBuilder(intake2PoseEnd) - .strafeTo(dodgeGate.position) + .strafeTo(dodgeGate.position) // drive around gate obstacle .strafeToSplineHeading(shootingPose.position, shootingPose.heading.plus(Math.toRadians(SHOOT_HEADING_OFFSET_AFTER_FIRSTSHOT))) .build(), //botActions.actionSetIntakeReverse(), - botActions.actionStartOuttake(SHOOT_RPM), + botActions.actionStartOuttake(SHOOT_RPM), // spin up flywheel for third shot new SequentialAction( - new SleepAction(timeUntilStartOuttake - 1), - botActions.rotateToMotifColorBeforeOuttake(1, botActions::getObeliskId, 2), - new SleepAction(1), - botActions.actionQuickOuttake(), - botActions.actionSetIntakePassive() + new SleepAction(timeUntilStartOuttake - 1), // wait until nearly at shooting pose + botActions.rotateToMotifColorBeforeOuttake(1, botActions::getObeliskId, 2),// rotate indexer slot 1 to correct colour + new SleepAction(1), // settle pause + botActions.actionQuickOuttake(), // fire third artifact + botActions.actionSetIntakePassive() // reset intake to passive ) ); + // Alternative spline-based return from intake row 2 (not used in the active run – + // kept as a reference/fallback). Uses splineTo for a smoother arc around the dodge + // waypoint and splineToSplineHeading for a tangent-continuous arrival at shooting pose. Action backToShoot2Spline = new ParallelAction( drive.actionBuilder(intake2PoseEnd) //.setTangent() - .splineTo(dodgeGate.position, dodgeGate.heading) + .splineTo(dodgeGate.position, dodgeGate.heading) // smooth spline arc around gate .splineToSplineHeading( new Pose2d( shootingPose.position, @@ -182,36 +359,41 @@ public void runOpMode() { .build(), //botActions.actionSetIntakeReverse(), - botActions.actionStartOuttake(SHOOT_RPM), + botActions.actionStartOuttake(SHOOT_RPM), // spin up flywheel new SequentialAction( new SleepAction(timeUntilStartOuttake - 1), - botActions.rotateToMotifColorBeforeOuttake(2, botActions::getObeliskId, 2), + botActions.rotateToMotifColorBeforeOuttake(2, botActions::getObeliskId, 2), // slot 2 for third cycle new SleepAction(1), botActions.actionQuickOuttake(), botActions.actionSetIntakePassive() ) ); + // Third intake sweep at the deepest row. Deepest X start offset applied. Action intake3 = botActions.actionIntakeThreeFeedback(shootingPose, intake3PoseStart, intake3PoseEnd, drive, maxIntakeDrivingVel); + // Return from intake row 3 to shooting pose for the fourth and final shot. + // No dodge waypoint needed from row 3 (different field geometry). Action backToShoot3 = new ParallelAction( drive.actionBuilder(intake3PoseEnd) .strafeToSplineHeading(shootingPose.position, shootingPose.heading.plus(Math.toRadians(SHOOT_HEADING_OFFSET_AFTER_FIRSTSHOT))) .build(), //botActions.actionSetIntakeReverse(), - botActions.actionStartOuttake(SHOOT_RPM), + botActions.actionStartOuttake(SHOOT_RPM), // spin up flywheel for final shot new SequentialAction( new SleepAction(timeUntilStartOuttake - 1), - botActions.rotateToMotifColorBeforeOuttake(3, botActions::getObeliskId, 2), + botActions.rotateToMotifColorBeforeOuttake(3, botActions::getObeliskId, 2), // slot 3 for final cycle new SleepAction(1), - botActions.actionQuickOuttake(), - botActions.actionSetIntakePassive() + botActions.actionQuickOuttake(), // fire final artifact + botActions.actionSetIntakePassive() // disable intake after last cycle ) ); + // Drive from the shooting pose to the parking zone using a spline heading transition + // so the robot arrives facing the correct direction for the end-game. Action toPark = drive.actionBuilder(shootingPose) .strafeToSplineHeading( parkPose.position, @@ -219,28 +401,41 @@ public void runOpMode() { ) .build(); + // Block until the driver presses START on the Driver Station. waitForStart(); + + // Immediately exit if the stop button was pressed instead of start (e.g. e-stop). if (isStopRequested()) return; + // Execute the entire autonomous routine. Actions.runBlocking loops until every + // action in the tree returns false (i.e. finished). Actions.runBlocking( new ParallelAction( + // Lane 1: After a short 0.5-second delay (to let the robot move away from + // the start wall and get a clear camera view), trigger the Limelight scan + // of the Obelisk to identify the Motif colour pattern. new SequentialAction( - new SleepAction(0.5), - botActions.actionScanObelisk() + new SleepAction(0.5), // wait for robot to clear the starting wall + botActions.actionScanObelisk() // capture Motif ID from Limelight ), + + // Lane 2: Run the periodic hardware update loop (telemetry, sensor polling) + // throughout the entire autonomous period. botActions.actionPeriodic(), + + // Lane 3: Main robot drive sequence executed in order. new SequentialAction( - new InstantAction(() -> drive.localizer.setPose(startPose)), - start, - //toObelisk, - toShoot, - intake1, - backToShoot1, - intake2, - backToShoot2, - intake3, - backToShoot3, - toPark + new InstantAction(() -> drive.localizer.setPose(startPose)), // reset odometry to known start pose + start, // initialise indexer + spin up flywheel + //toObelisk, // (disabled) original separate obelisk-scan drive action + toShoot, // drive through obelisk → shoot first artifact + intake1, // sweep row 1 intake + backToShoot1, // return → shoot second artifact + intake2, // sweep row 2 intake + backToShoot2, // return (via gate dodge) → shoot third artifact + intake3, // sweep row 3 intake (deepest) + backToShoot3, // return → shoot fourth artifact + toPark // drive to parking zone ) ) ); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auto/opmodes/FasterRedClose.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auto/opmodes/FasterRedClose.java index 3f803f965ac0..9792d3a21bd0 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auto/opmodes/FasterRedClose.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auto/opmodes/FasterRedClose.java @@ -15,113 +15,288 @@ import org.firstinspires.ftc.teamcode.auto.roadrunner.miscRR.MecanumDrive; import org.firstinspires.ftc.teamcode.subsystems.Indexer; +/** + * FasterRedClose — Red Alliance, Close-Side Autonomous WITHOUT Motif (No Obelisk Color Matching) + * + *Alliance: Red | Starting Position: Close side, facing + * field-forward at 0° (positive-X direction).
+ * + *This is the simpler, faster variant of the red close-side autonomous. Unlike the "Motif" + * variants, this OpMode does NOT scan the Obelisk AprilTag or rotate the indexer drum to + * match required colors. Instead it fires whatever artifact is currently loaded in slot "two" + * unconditionally after each intake cycle. This removes ~1–2 seconds of drum rotation and + * confirmation time per shot, allowing faster cycle times at the cost of color-matching: + *
Execution Order: + *
Alliance: Red | Starting Position: Close side (robot begins near + * the red driver's station, facing the field interior at 180°).
+ * + *This OpMode executes a fully automated 30-second autonomous routine: + *
The "Motif" variants call {@code rotateToMotifColorBeforeOuttake} before every shot. + * The Limelight camera continuously decodes the Obelisk AprilTag ID ({@code getObeliskId}), + * which tells the robot which color is required at each scoring slot. The indexer drum + * then rotates to present that color before the actuator gate opens. This scores the right + * color automatically rather than relying on manual pre-loading arrangement.
+ * + *All positions are in inches relative to the robot's start pose (0, 0, 180°). + * Positive Y runs away from the starting wall toward the far side of the field. + * Positive X runs toward the red alliance wall (rightward when facing from the red side). + * Headings are measured counter-clockwise from positive X; the robot starts at 180° + * (facing toward the field interior / negative-X direction).
+ * + *{@code @Config} exposes every {@code public static} field to the FTC Dashboard web + * interface, enabling live position, heading, RPM, and timing adjustments without + * recompiling the code.
+ * + * @see FasterRedClose Non-motif variant for the red close side + * @see FasterBlueCloseMotif Blue alliance mirror of this autonomous + * @see BotActions Factory for all subsystem Road Runner actions + * @see Hardware Initializes drivetrain, indexer, intake, shooter, and camera + */ @Config @Autonomous(name = "Faster Red Auto With Motif", group = "Autonomous") public class FasterRedCloseMotif extends LinearOpMode { + /** + * Maximum translational velocity (inches per second) while the robot drives through the + * intake corridors. Set conservatively at 15 in/s (vs. 30 for non-motif) so the intake + * rollers have longer contact time with each artifact, reducing missed pickups and + * minimizing odometry slip that could corrupt the pose estimate. + */ public static double maxIntakeDrivingVel = 15; + /** + * X-coordinate (inches) of the Obelisk (Motif) AprilTag structure. + * The obelisk is the field element whose AprilTag encodes the color pattern the robot + * must score. Although the explicit drive-to-obelisk step is commented out in this + * variant, the pose is preserved for reference and potential re-enablement. + */ public static double OBELISK_X = 8; + + /** + * Y-coordinate (inches) of the Obelisk (Motif) structure. + * See {@link #OBELISK_X} for full context. + */ public static double OBELISK_Y = 36; + + /** + * Heading (degrees) the robot should face while viewing the Obelisk AprilTag. + * At -60° the robot has rotated clockwise so its Limelight camera has an optimal + * sightline to the tag mounted on the obelisk structure. + */ public static double OBELISK_HEADING_DEG = -60; + /** + * X-coordinate (inches) of the primary shooting position. + * Chosen to give a clear firing lane toward all scoring targets while remaining + * clear of the intake corridors and gate obstacle. + */ public static double SHOOT_X = 17; + + /** + * Y-coordinate (inches) of the primary shooting position. + * At Y = 40 the robot is roughly mid-field on the red alliance side with a clean + * line of sight to the scoring structures. + */ public static double SHOOT_Y = 40; + + /** + * Heading (degrees) of the robot at the shooting position. + * At -135° the robot is turned far enough clockwise that the shooter (and the + * co-located Limelight camera) can see both the scoring targets and the Obelisk + * AprilTag simultaneously, making color decisions at shot time. + */ public static double SHOOT_HEADING_DEG = -135; + + /** + * Small additional heading offset (degrees) applied to {@link #SHOOT_HEADING_DEG} for + * all shots after the first. The +2° counter-clockwise nudge compensates for accumulated + * localization drift between shots and fine-tunes aim after the robot has moved through + * multiple intake cycles. The inline comment "// -2" marks the prior value that was + * changed, preserved for tuning history context. + */ public static double SHOOT_HEADING_OFFSET_AFTER_FIRSTSHOT = 2; // -2 + /** + * X-coordinate (inches) of the intake sweep start position for all rows. + * The robot begins its intake sweep from this X value and drives toward negative X + * (toward the field wall on the red side), sweeping artifacts into the intake rollers. + */ public static double INTAKE_START_X = 12; + + /** + * Inward X offset (inches) subtracted from {@link #INTAKE_START_X} for the second + * intake row. Shifting the start point inward by 3 in. avoids re-collecting artifacts + * from row 1 and ensures better alignment with the row-2 artifact positions. + */ public static double INTAKE2_START_OFFSET_X = 3.0; + + /** + * Inward X offset (inches) subtracted from {@link #INTAKE_START_X} for the third + * intake row. The larger 5 in. shift accounts for the deeper field position of row-3 + * artifacts relative to the robot's normal start-of-sweep location. + */ public static double INTAKE3_START_OFFSET_X = 5.0; + + /** + * X-coordinate (inches) at the end of the first intake sweep. + * The robot drives from {@link #INTAKE_START_X} to this negative value, sweeping + * across the full width of the intake corridor to collect all available row-1 artifacts. + * The negative value places the robot near the far field wall on the red side. + */ public static double INTAKE_END_X = -16.5; + + /** + * Additional X distance (inches) subtracted from {@link #INTAKE_END_X} for the second + * and third intake rows. Artifacts in rows 2 and 3 are positioned further into the field + * (more negative X), so the robot must travel deeper to collect them all. + */ public static double intake_END_2And3_XOffset = 8; + /** + * Y-coordinate (inches) of the first intake row of field artifacts. + * At ~52 inches from start, this is the nearest row to the robot's launch position, + * making it the fastest to reach after the first shot. + */ public static double INTAKE1_Y = 52; + + /** + * Y-coordinate (inches) of the second intake row of field artifacts. + * At ~76 inches, this is approximately mid-field. The return path from here requires + * navigating around the gate obstacle via the {@code dodgeGate} waypoint. + */ public static double INTAKE2_Y = 76; + + /** + * Y-coordinate (inches) of the third intake row of field artifacts. + * At ~96 inches, this is the farthest row from the start — near the far end of the field. + * Despite the distance, the return path is simpler because the robot bypasses the gate. + */ public static double INTAKE3_Y = 96; + /** + * X-coordinate (inches) of the field gate obstacle. + * The gate is a physical field element situated between the intake rows and the shooting + * position. It is referenced when computing the {@code dodgeGate} intermediate waypoint + * so the robot steers around it on the return from rows 2 and 3. + */ public static double gate_X = -12; + + /** + * Y-coordinate (inches) of the field gate obstacle. + * At ~60 inches the gate sits roughly between rows 1 and 2, squarely in the most direct + * return path from rows 2 and 3 back to the shooting position. + */ public static double gate_Y = 60; + /** + * X-coordinate (inches) of the final parking position. + * After all four shots are fired, the robot drives here to earn the autonomous parking + * bonus points. + */ public static double PARK_X = 6; + + /** + * Y-coordinate (inches) of the final parking position. + * At Y = 68, the parking zone lies between the row-1 intake corridor and the gate obstacle. + */ public static double PARK_Y = 68; + /** + * Target flywheel speed in RPM for the shooter motor. + * 3580 RPM provides sufficient projectile velocity from the shooting position to reach + * the scoring targets. This is higher than the no-motif variant (3240) to maintain + * consistent shot power regardless of which drum slot is aligned — different drum + * positions slightly alter the loading geometry and thus the effective launch angle. + */ public static int SHOOT_RPM = 3580; + /** + * Time in seconds to wait after a parallel drive+spinup action begins before triggering + * the outtake (firing) sequence. This delay serves three purposes: + *Full Execution Order: + *
Alliance: Red | Starting Position: Close side, facing + * field-forward at 0° (positive-X direction).
+ * + *This is a high-throughput variant of the red close-side motif autonomous, tuned to + * score as many artifacts as possible (up to 15 balls across all three intake rows) within + * the 30-second autonomous period: + *
All positions in inches relative to start pose (0, 0, 0°). The robot faces positive-X + * (field-forward) at the start. Positive Y runs perpendicular toward the far field wall. + * The shooting position is at (17, 40) with a -140° heading.
+ * + * @see FasterRedCloseMotif Standard red close motif autonomous (slower, more accurate) + * @see FasterRedClose Red close autonomous with no motif color matching + * @see BotActions Subsystem action factories + * @see Hardware Hardware initialization and subsystem wiring + */ @Config @Autonomous(name = "Faster Red Auto With Motif 15 Ball", group = "Autonomous") public class FasterRedCloseMotif15 extends LinearOpMode { + /** + * Maximum translational velocity (in/s) while driving through intake corridors. + * Set to 30 in/s (double the standard motif variant) to maximize cycle speed and + * collect the most artifacts possible within the 30-second autonomous window. + * The trade-off is slightly reduced pickup reliability compared to slower speeds. + */ public static double maxIntakeDrivingVel = 30; + /** + * X-coordinate (inches) of the Obelisk (Motif) AprilTag structure. + * Used in the {@code toObelisk} action that is currently commented out in the sequence, + * and as context for the Limelight scan embedded inside {@code toShoot}. + */ public static double OBELISK_X = 8; + + /** + * Y-coordinate (inches) of the Obelisk (Motif) structure. + * See {@link #OBELISK_X} for context. + */ public static double OBELISK_Y = 36; + + /** + * Heading (degrees) the robot faces while pointing at the Obelisk AprilTag. + * At -60° the Limelight camera has an optimal sightline to the tag. + */ public static double OBELISK_HEADING_DEG = -60; + /** + * X-coordinate (inches) of the shooting position. + * Same as the standard motif variant; positioned for a clear shot to all scoring targets. + */ public static double SHOOT_X = 17; + + /** + * Y-coordinate (inches) of the shooting position. + */ public static double SHOOT_Y = 40; + + /** + * Heading (degrees) of the robot at the shooting position. + * At -140° (slightly less aggressive than the standard motif's -135°), tuned to balance + * aim angle and the trajectory approach smoothness for this faster configuration. + */ public static double SHOOT_HEADING_DEG = -140; + + /** + * Small heading offset (degrees) added to {@link #SHOOT_HEADING_DEG} for all shots after + * the first. +2° nudges the robot slightly counter-clockwise to compensate for accumulated + * localization drift across multiple intake-and-return cycles. + */ public static double SHOOT_HEADING_OFFSET_AFTER_FIRSTSHOT = 2; // -2 + /** + * X-coordinate (inches) of the intake sweep start for all rows. + * The robot begins sweeping from this point and drives toward negative X to collect artifacts. + */ public static double INTAKE_START_X = 12; + + /** + * Inward X offset (inches) subtracted from {@link #INTAKE_START_X} for row 2. + * Prevents re-collecting row-1 artifacts by shifting the start point inward. + */ public static double INTAKE2_START_OFFSET_X = 3.0; + + /** + * Inward X offset (inches) subtracted from {@link #INTAKE_START_X} for row 3. + * Larger shift for the deepest artifact row. + */ public static double INTAKE3_START_OFFSET_X = 5.0; + + /** + * X-coordinate (inches) at the end of the first intake sweep. + * Shallower than the standard motif variant (-13.5 vs. -16.5), reflecting this tuning's + * field/placement configuration or a narrower artifact spread. + */ public static double INTAKE_END_X = -13.5; + + /** + * Additional depth (inches) subtracted from {@link #INTAKE_END_X} for rows 2 and 3. + * Artifacts in later rows are deeper into the field and require a longer sweep. + */ public static double intake_END_2And3_XOffset = 8; + /** + * Y-coordinate (inches) of the first intake row. + * At 48.5 in. this is slightly closer to start than the standard variant (52 in.), + * shaving a small amount of transit time for the faster 15-ball tuning. + */ public static double INTAKE1_Y = 48.5; + + /** + * Y-coordinate (inches) of the second intake row (mid-field). + */ public static double INTAKE2_Y = 76; + + /** + * Y-coordinate (inches) of the third intake row (near far wall). + */ public static double INTAKE3_Y = 96; + /** + * X-coordinate (inches) of the field gate obstacle used in path planning. + * Referenced by the optional {@code goToGate} action and the {@code dodgeGate} waypoint. + */ public static double gate_X = -12; + + /** + * Y-coordinate (inches) of the field gate obstacle. + */ public static double gate_Y = 60; + /** + * Time in seconds the robot waits at the gate position if the {@code goToGate} action is + * enabled. Currently not used in the active sequence (goToGate is commented out), but + * preserved for testing alternate gate-approach strategies. + */ public static double gateWaitTime = 1.5; + /** + * X-coordinate (inches) of the final parking position. + */ public static double PARK_X = 6; + + /** + * Y-coordinate (inches) of the final parking position. + */ public static double PARK_Y = 68; + /** + * Target flywheel speed in RPM. + * 3580 RPM provides the power needed to reliably reach the scoring targets regardless + * of drum rotation position, same as the standard motif variant. + */ public static int SHOOT_RPM = 3580; + /** + * Base delay (seconds) before triggering the outtake sequence inside each parallel action. + * At 3.0 s this is longer than the standard motif's 2.5 s because this variant drives + * directly to the shooting pose in a single segment (no intermediate waypoint), and the + * single-segment trajectory to (17, 40, -140°) takes slightly more time from (0, 0, 0°). + * Subsequent shots add additional time: backToShoot1 uses {@code +1}, backToShoot2 uses + * {@code +2}, and backToShoot3 uses {@code +1} to handle the longer return paths. + */ public static double timeUntilStartOuttake = 3.0; // Time until you start the outtake action, which still includes the wait for actuator // bunch of compensations for bad rr // has quick outtake and quick intake + + /** + * Main autonomous routine. Pre-compiles all trajectories and action sequences before + * {@code waitForStart}, then runs the full 15-ball routine via {@code Actions.runBlocking}. + * + *Execution Order: + *
Alliance: Red | Starting Position: Close side, facing + * field-forward at 0° (positive-X direction).
+ * + *Road Runner offers two heading interpolation strategies along a trajectory: + *
start pose (0,0,0°) → toObelisk → toShoot (from obeliskPose) → [intake/shoot ×3] → park
+ * + * @see FasterRedCloseMotif Standard motif variant (spline heading, 180° start) + * @see FasterRedCloseMotif15 High-throughput motif variant (no explicit obelisk visit) + * @see BotActions Subsystem action factory + * @see Hardware Hardware initialization + */ @Config @Autonomous(name = "Faster Red Auto With Motif Linear Heading", group = "Autonomous") public class FasterRedCloseMotifLinear extends LinearOpMode { + /** + * Maximum translational velocity (in/s) during intake driving segments. + * 30 in/s matches the 15-ball variant — prioritising speed for high cycle count. + */ public static double maxIntakeDrivingVel = 30; + /** + * X-coordinate (inches) of the Obelisk (Motif) structure. + * This is actually visited in the active sequence ({@code toObelisk} is not commented out + * in the {@code runBlocking} call), so the robot physically drives here first. + */ public static double OBELISK_X = 8; + + /** Y-coordinate (inches) of the Obelisk structure. */ public static double OBELISK_Y = 36; + + /** + * Heading (degrees) the robot faces at the obelisk to optimally view the AprilTag + * with the Limelight camera. + */ public static double OBELISK_HEADING_DEG = -60; + /** X-coordinate (inches) of the shooting position. */ public static double SHOOT_X = 17; + + /** Y-coordinate (inches) of the shooting position. */ public static double SHOOT_Y = 40; + + /** + * Heading (degrees) of the robot at the shooting position. + * At -145° this is the steepest clockwise rotation of any red motif variant, tuned + * specifically for the approach from the obelisk pose using linear heading interpolation. + */ public static double SHOOT_HEADING_DEG = -145; + + /** + * Heading offset (degrees) applied after the first shot. Set to 0° — linear heading + * interpolation provides sufficient accuracy that no correction offset is needed. + */ public static double SHOOT_HEADING_OFFSET_AFTER_FIRSTSHOT = 0; // -2 + /** X-coordinate (inches) of the intake sweep start. */ public static double INTAKE_START_X = 12; + + /** Inward X offset (inches) for the row-2 sweep start position. */ public static double INTAKE2_START_OFFSET_X = 3.0; + + /** Inward X offset (inches) for the row-3 sweep start position. */ public static double INTAKE3_START_OFFSET_X = 5.0; + + /** + * X-coordinate (inches) at the end of the row-1 intake sweep. + * Same depth as the 15-ball variant (-13.5 in.) rather than the standard motif (-16.5 in.). + */ public static double INTAKE_END_X = -13.5; + + /** Additional depth (inches) for the row-2 and row-3 intake end positions. */ public static double intake_END_2And3_XOffset = 8; + /** Y-coordinate (inches) of the first intake row (same as 15-ball at 48.5 in.). */ public static double INTAKE1_Y = 48.5; + + /** Y-coordinate (inches) of the second intake row. */ public static double INTAKE2_Y = 76; + + /** Y-coordinate (inches) of the third intake row. */ public static double INTAKE3_Y = 96; + /** X-coordinate (inches) of the gate obstacle reference. */ public static double gate_X = -12; + + /** Y-coordinate (inches) of the gate obstacle reference. */ public static double gate_Y = 60; + /** + * Dwell time (seconds) at the gate position when {@code goToGate} is used. + * Currently the action is commented out in the active sequence. + */ public static double gateWaitTime = 1.5; + /** X-coordinate (inches) of the final parking position. */ public static double PARK_X = 6; + + /** Y-coordinate (inches) of the final parking position. */ public static double PARK_Y = 68; + /** + * Target flywheel speed in RPM. + * 3480 RPM — slightly lower than the other motif variants (3580) and fine-tuned for + * the specific geometry of this variant's linear-heading approach to the shooting pose. + */ public static int SHOOT_RPM = 3480; + /** + * Base delay (seconds) before triggering the outtake sequence. + * At 1.0 s this is the shortest of all motif variants. Because the robot visits the + * obelisk first and the flywheel is already spinning during that transit, it is closer + * to target RPM when {@code toShoot} begins — less spinup time is needed. + * Subsequent shots: backToShoot1/3 use {@code +0} (same 1.0 s); backToShoot2 uses {@code +1.0}. + */ public static double timeUntilStartOuttake = 1.0; // Time until you start the outtake action, which still includes the wait for actuator // bunch of compensations for bad rr // has quick outtake and quick intake + + /** + * Main autonomous routine. Pre-compiles all trajectories and actions, then executes + * the full sequence after {@code waitForStart}. + * + *Key difference from other variants: This routine actually runs {@code toObelisk} + * as the first driving step (it is not commented out in the {@code SequentialAction} inside + * {@code runBlocking}). The {@code toShoot} action therefore starts its trajectory from + * {@code obeliskPose} rather than {@code startPose}.
+ * + *Execution Order: + *
An {@code Action} holds an ordered list of {@link Runnable} steps. + * Calling {@link #run()} executes every step in sequence, blocking until + * all steps complete. + * + *
Instances are constructed exclusively through the nested {@link Builder} + * to enforce a clear, readable construction pattern: + *
+ * Action myAction = new Action.Builder() + * .add(() -> subsystem.doSomething()) + * .moveTo(() -> drive.moveTo(targetPose)) + * .add(() -> shooter.fire()) + * .build(); + * myAction.run(); + *+ * + *
Note: This class predates the Road Runner-based
+ * {@link com.acmerobotics.roadrunner.Action} pipeline and is used in
+ * helper utilities that need a simple imperative list of operations.
+ */
public class Action {
+ // -----------------------------------------------------------------------
+ // Steps
+ // -----------------------------------------------------------------------
+
+ /**
+ * The ordered list of {@link Runnable} steps to execute when {@link #run()}
+ * is called. Populated only by the {@link Builder}.
+ */
private final List Steps are run synchronously on the calling thread; there is no
+ * timeout or interruption mechanism.
+ */
// Run all steps in order
public void run() {
for (Runnable step : steps) {
- step.run();
+ step.run(); // execute each step sequentially
}
}
+ // -----------------------------------------------------------------------
+ // Builder
+ // -----------------------------------------------------------------------
+
+ /**
+ * Fluent builder for constructing {@link Action} instances.
+ *
+ * Steps can be added in any order using {@link #add(Runnable)} or
+ * {@link #moveTo(Runnable)}. Call {@link #build()} to produce the
+ * immutable {@link Action}.
+ */
// Builder nested class
public static class Builder {
+
+ /** Accumulates steps in insertion order. */
private final List Every method in this class returns a Road Runner {@link Action} that
+ * encapsulates one logical autonomous step (intake a set of artifacts, perform
+ * an outtake dump, scan the obelisk tag, etc.). The actions are composed
+ * together in individual auto OpModes using
+ * {@link SequentialAction} / {@link ParallelAction}.
+ *
+ * The class is annotated with {@link Config} so all {@code public static}
+ * tuning constants (spin times, powers, RPM scales) are editable via FTC
+ * Dashboard without re-deploying code.
+ *
+ * All subsystem references are injected through the {@link Hardware} data
+ * class so that the same hardware objects are shared with the OpMode.
+ */
@Config
public class BotActions {
+
+ // -----------------------------------------------------------------------
+ // Injected dependencies
+ // -----------------------------------------------------------------------
+
+ /** The running autonomous OpMode; used to check stop/isActive in loop Actions. */
private final LinearOpMode opMode;
+
+ /** FTC telemetry used to log debug data during Action execution. */
private final Telemetry telemetry;
+
+ /** Conveyor/intake roller subsystem reference. */
private final Intake intake;
+
+ /** Three-slot indexer drum subsystem reference. */
private final Indexer indexer;
+
+ /** Flywheel shooter subsystem reference. */
private final Outtake outtake;
+
+ /** Actuator (launch ramp or ball-gate) subsystem reference. */
private final Actuator actuator;
+
+ /** Limelight AprilTag scanner (public so OpModes can call scan/pipeline directly). */
public final AprilTag aprilTag;
+
+ /** Localization-based heading aimer for auto heading lock. */
private final Aimer aprilAimer;
+
+ /** Road Runner drive for trajectory actions and pose estimation. */
private final MecanumDrive drive;
- public static double NON_INDEX_SPIN_TIME = 2.0; //seconds of full-power indexer blast
+ // -----------------------------------------------------------------------
+ // Tuning constants (Dashboard-editable)
+ // -----------------------------------------------------------------------
+
+ /** Duration (seconds) of the full-power indexer blast during the quick outtake.
+ * Long enough to eject all three artifacts; shorter = less dead time. */
+ public static double NON_INDEX_SPIN_TIME = 2.0;
+
+ /** Raw motor power applied to the indexer drum during the blast phase.
+ * Higher values push artifacts through faster but may cause jamming. */
public static double FULL_BLAST_POWER = 0.30;
+ /** Time (seconds) after the start of the intake trajectory before the indexer
+ * automatically advances to slot 1 to accept the first ball. */
public static double ball1TimeDisp = 0.66;
- public static double ball2TimeDisp = 1.10;
- public static double timeToIntake = 2.50;
+ /** Time (seconds) after the start of the intake trajectory before the indexer
+ * automatically advances to slot 2 to accept the second ball. */
+ public static double ball2TimeDisp = 1.10;
+
+ /** Total time (seconds) the robot spends over the intake zone before it starts
+ * the return trajectory toward the shooting position. */
+ public static double timeToIntake = 2.50;
+
+ /** Whether to maintain continuous AprilTag heading lock during an action sequence.
+ * When {@code true}, {@link #actionPeriodic()} applies heading corrections. */
private boolean continuousAprilTagLock;
+
+ /** Minimum time (ms) between successive artifact acquisitions in the feedback
+ * intake action. Prevents double-counting a single artifact that briefly
+ * trips the sensor twice. */
public static double cooldownFeedbackIntake = 0;
+
+ /** Scale factor applied to the target RPM during the quick-spin outtake.
+ * Values < 1.0 give slightly lower RPM but allow faster spin-up. */
public static double quickspinRpmScale = 0.93;
+
+ /** The most recent heading turn correction from the AprilTag aimer.
+ * Held between update cycles so correction is not dropped between frames. */
private double lastTurnCorrection;
+
+ /** The most recently detected obelisk AprilTag ID.
+ * 0 = no detection; 21/22/23 = valid motif tag IDs. */
private int obeliskId = 0;
+ // -----------------------------------------------------------------------
+ // Constructor
+ // -----------------------------------------------------------------------
+
+ /**
+ * Constructs a BotActions by extracting all subsystem references from the
+ * provided {@link Hardware} container.
+ *
+ * @param hardware the robot hardware container (all subsystems already built)
+ * @param telemetry FTC telemetry for debug output during actions
+ * @param opMode the running autonomous LinearOpMode (for stop checks)
+ */
public BotActions(
Hardware hardware,
Telemetry telemetry,
LinearOpMode opMode
) {
- this.intake = hardware.intake;
- this.indexer = hardware.indexer;
- this.outtake = hardware.outtake;
- this.actuator = hardware.actuator;
- this.aprilTag = hardware.aprilTag;
+ // Unpack each subsystem reference from the hardware container.
+ this.intake = hardware.intake;
+ this.indexer = hardware.indexer;
+ this.outtake = hardware.outtake;
+ this.actuator = hardware.actuator;
+ this.aprilTag = hardware.aprilTag;
this.aprilAimer = hardware.aprilAimer;
- this.drive = hardware.mecanumDrive;
+ this.drive = hardware.mecanumDrive;
this.telemetry = telemetry;
- this.opMode = opMode;
+ this.opMode = opMode;
}
+ // -----------------------------------------------------------------------
+ // Outtake actions
+ // -----------------------------------------------------------------------
+
+ /**
+ * Returns an instantaneous {@link Action} that simultaneously enables the
+ * indexer's auto-outtake mode and sets the shooter to the given RPM
+ * (scaled by {@link #quickspinRpmScale}).
+ *
+ * Call this before a trajectory to pre-spin the shooter so it is at
+ * speed by the time the robot reaches the shooting position.
+ *
+ * @param rpm the base target RPM for the shooter flywheel
+ * @return a {@link ParallelAction} that starts indexer auto-outtake and shooter spin-up
+ */
public Action actionStartOuttake(double rpm) {
return new ParallelAction(
- new InstantAction(() -> indexer.setAutoOuttaking(true)),
- new InstantAction(() -> outtake.set(rpm * quickspinRpmScale))
+ new InstantAction(() -> indexer.setAutoOuttaking(true)), // enable auto-outtake mode
+ new InstantAction(() -> outtake.set(rpm * quickspinRpmScale)) // spin up to scaled RPM
);
}
+ /**
+ * Returns an {@link Action} that performs a full "quick dump":
+ * raise the actuator, blast all artifacts out with the indexer, then lower
+ * and reset everything for the next intake cycle.
+ *
+ * Sequence:
+ * The action reads the obelisk tag ID lazily (at action-run time, not at
+ * method-call time) via the {@code id} {@link IntSupplier} so the ID is
+ * available only after {@link #actionScanObelisk()} has completed.
+ *
+ * Algorithm:
+ * The indexer advancement is time-based (not sensor-based); adjust
+ * {@link #ball1TimeDisp} and {@link #ball2TimeDisp} to match the actual
+ * spacing between field artifacts.
+ *
+ * @param startActionPose the robot pose at which to begin this action
+ * @param startIntakePose the pose where the intake port aligns with the first artifact
+ * @param endPose the pose to return to after intake (typically shooting position)
+ * @param drive the Road Runner drive used to build the trajectory
+ * @param maxVel maximum translational velocity (in/s) for the return segment
+ * @return the built trajectory {@link Action}
+ */
public Action actionIntakeThree(Pose2d startActionPose, Pose2d startIntakePose, Pose2d endPose, MecanumDrive drive, double maxVel) {
+ // Limit return-segment velocity to maxVel so the robot doesn't overshoot.
TranslationalVelConstraint velConstraint = new TranslationalVelConstraint(maxVel);
return drive.actionBuilder(startActionPose)
- .strafeToSplineHeading(startIntakePose.position, startIntakePose.heading)
- .afterTime(0, intake::run)
- .afterTime(ball1TimeDisp, () -> indexer.moveTo(indexer.getState().next()))
- .afterTime(ball2TimeDisp, () -> indexer.moveTo(indexer.getState().next()))
- .afterTime(timeToIntake, intake::runSlow)
- .strafeToLinearHeading(endPose.position, endPose.heading, velConstraint)
+ .strafeToSplineHeading(startIntakePose.position, startIntakePose.heading) // approach intake zone
+ .afterTime(0, intake::run) // start intake immediately
+ .afterTime(ball1TimeDisp, () -> indexer.moveTo(indexer.getState().next())) // advance to slot 1 after first artifact time
+ .afterTime(ball2TimeDisp, () -> indexer.moveTo(indexer.getState().next())) // advance to slot 2 after second artifact time
+ .afterTime(timeToIntake, intake::runSlow) // slow down intake at end of dwell time
+ .strafeToLinearHeading(endPose.position, endPose.heading, velConstraint) // return to shooting position
.build();
}
-
-
+ // -----------------------------------------------------------------------
+ // Auto initialization
+ // -----------------------------------------------------------------------
+
+ /**
+ * Returns an {@link Action} that initializes the robot for autonomous:
+ * clears all indexer slot colours, enables intake auto-advance at the
+ * specified starting slot, lowers the actuator, and starts passive intake.
+ *
+ * Note: this is a temporary action for testing; in a real auto run the
+ * initialization is performed as part of {@link #actionQuickOuttake()}.
+ *
+ * @param indexerInitializedSlot the indexer slot to start intake at (ensures
+ * consistent starting position regardless of where
+ * the drum was left)
+ * @return a {@link ParallelAction} that resets the indexer, lowers the actuator,
+ * and starts slow intake simultaneously
+ */
public Action initializeAuto(Indexer.IndexerState indexerInitializedSlot) { // only temporary for testing, this is done in actionQuickOuttake
return new ParallelAction(
new SequentialAction(
- new InstantAction(() -> indexer.initializeColors(Indexer.ArtifactColor.EMPTY)),
- new InstantAction(() -> indexer.setIntaking(true, indexerInitializedSlot)) // just to make it so it starts at a consistent spot
+ new InstantAction(() -> indexer.initializeColors(Indexer.ArtifactColor.EMPTY)), // mark all slots empty
+ new InstantAction(() -> indexer.setIntaking(true, indexerInitializedSlot)) // start intake at specified slot
),
- new InstantAction(actuator::down),
- new InstantAction(intake::runSlow)
+ new InstantAction(actuator::down), // lower the actuator to intake position
+ new InstantAction(intake::runSlow) // begin slow passive intake
);
}
+ // -----------------------------------------------------------------------
+ // Feedback-based intake
+ // -----------------------------------------------------------------------
+
+ /**
+ * Returns an {@link Action} that drives to the intake zone and indexes
+ * artifacts using sensor feedback rather than fixed time offsets.
+ *
+ * The robot drives the trajectory (approach → return) while a parallel
+ * action monitors the indexer sensor. Each time the sensor detects an
+ * artifact aligned with the intake port (rising edge), the indexer advances
+ * to the next slot after a cooldown period. The parallel action ends when
+ * three artifacts have been acquired or the drive trajectory completes.
+ *
+ * Compared to {@link #actionIntakeThree}, this is more robust to
+ * varying artifact positions because advancement is triggered by detection
+ * rather than elapsed time.
+ *
+ * @param startActionPose starting pose for the trajectory
+ * @param startIntakePose pose where the robot begins actively intaking
+ * @param endPose return pose after intake
+ * @param drive Road Runner drive for trajectory execution
+ * @param maxVel maximum return velocity (in/s)
+ * @return a {@link SequentialAction}: start intake → parallel(drive, feedback-index) → slow intake
+ */
//feedback based version of actionIntakeT
public Action actionIntakeThreeFeedback(
Pose2d startActionPose,
@@ -169,149 +402,224 @@ public Action actionIntakeThreeFeedback(
MecanumDrive drive,
double maxVel
) {
+ // Limit return-segment velocity so the robot doesn't overshoot the shoot position.
VelConstraint velConstraint = new TranslationalVelConstraint(maxVel);
+ // Build the drive trajectory: approach intake zone, then return to end pose.
Action driveAction = drive.actionBuilder(startActionPose)
- .strafeToSplineHeading(startIntakePose.position, startIntakePose.heading)
- .strafeToLinearHeading(endPose.position, endPose.heading, velConstraint)
+ .strafeToSplineHeading(startIntakePose.position, startIntakePose.heading) // approach
+ .strafeToLinearHeading(endPose.position, endPose.heading, velConstraint) // return
.build();
+ // Parallel action that manages intake speed and indexer advancement based on sensor.
Action manageIntakeAndIndexing = new Action() {
- private boolean lastAlignedNonEmpty = false;
- private int acquired = 0;
- private double timeoutDuration = 5.0;
- private final ElapsedTime acquireCooldown = new ElapsedTime();
+ private boolean lastAlignedNonEmpty = false; // sensor state from the previous iteration
+ private int acquired = 0; // number of artifacts acquired so far (max 3)
+ private double timeoutDuration = 5.0; // (unused) fallback timeout in seconds
+ private final ElapsedTime acquireCooldown = new ElapsedTime(); // cooldown between advancements
@Override
public boolean run(@NonNull TelemetryPacket p) {
- // stop if opmode ends
+ // Abort if the OpMode has been stopped (e.g., end of auto period).
if (!opMode.opModeIsActive() || opMode.isStopRequested()) {
- intake.runSlow(); // or intake.stop()
- return false;
+ intake.runSlow(); // leave intake in slow mode to avoid sharp stop
+ return false; // signal that this action is done
}
- // keep indexer logic alive
+ // Tick the indexer so its internal PD control and sensor reads stay current.
indexer.update();
+ // Check if an artifact is currently aligned with the intake port AND present.
boolean alignedNonEmpty = indexer.artifactPresentAndAligned();
- // intake mode continuously
- if (alignedNonEmpty) intake.runSlow(); // Lowkey the play so that once one is intaken another doesn't get stuck until indexer moves
- else intake.run();
+ // Modulate intake speed: slow down once an artifact is detected to prevent
+ // the next one from jamming while the drum rotates.
+ if (alignedNonEmpty) intake.runSlow(); // slow: artifact present, wait for indexer to advance
+ else intake.run(); // full speed: no artifact yet, keep pulling
+ // Rising-edge detection: only count when the sensor just became true.
+ // Also enforce the cooldown so a single artifact can't be counted twice.
if (!lastAlignedNonEmpty
&& alignedNonEmpty
&& acquireCooldown.milliseconds() > cooldownFeedbackIntake) {
- acquired++;
- acquireCooldown.reset();
+ acquired++; // increment the acquired count
+ acquireCooldown.reset(); // restart the cooldown timer
if (acquired < 3) {
+ // Advance to the next slot to make room for the next artifact.
indexer.moveTo(indexer.getState().next());
}
}
- lastAlignedNonEmpty = alignedNonEmpty;
+ lastAlignedNonEmpty = alignedNonEmpty; // store for rising-edge detection next iter
- // keep running until we've acquired 3
+ // End condition: all three slots are filled.
if (acquired >= 3) {
- intake.runSlow(); // or intake.stop()
- return false;
+ intake.runSlow(); // transition to slow intake to avoid jamming the third ball
+ return false; // signal that intake is complete
}
+ // Write debug data to the telemetry packet for the FTC Dashboard.
p.put("acquired", acquired);
p.put("alignedNonEmpty", alignedNonEmpty);
p.put("indexerState", indexer.getState());
- return true;
+ return true; // still collecting; keep running this iteration
}
};
+ // Wire everything together: start intake, then drive and index in parallel, then slow.
return new SequentialAction(
- new InstantAction(intake::run),
+ new InstantAction(intake::run), // start intake before entering the parallel block
new ParallelAction(
- driveAction,
- manageIntakeAndIndexing
+ driveAction, // drive the intake trajectory
+ manageIntakeAndIndexing // concurrently manage intake speed & indexer
),
- new InstantAction(intake::runSlow)
+ new InstantAction(intake::runSlow) // after both finish, settle into slow intake
);
}
+ // -----------------------------------------------------------------------
+ // Obelisk scan
+ // -----------------------------------------------------------------------
+
+ /**
+ * Returns an {@link Action} that continuously calls the Limelight to scan
+ * for the obelisk AprilTag and terminates as soon as a valid motif tag
+ * (ID 21, 22, or 23) is detected.
+ *
+ * The detected ID is stored in {@link #obeliskId} for later retrieval
+ * via {@link #getObeliskId()} or via the {@link AprilTag#obeliskId} static
+ * field.
+ *
+ * Note: Running this as an {@link InstantAction} and wrapping with
+ * a while loop is not ideal (the action blocks the Road Runner scheduler on
+ * the same thread); this is a known issue noted in the source.
+ *
+ * @return an {@link Action} that loops until a valid obelisk tag is found
+ */
// bad to do instant action and while loop
public Action actionScanObelisk() {
return new Action() {
- private final ElapsedTime timer = new ElapsedTime();
+ private final ElapsedTime timer = new ElapsedTime(); // (unused) reserved for timeout
@Override
public boolean run(@NonNull TelemetryPacket telemetryPacket) {
- /*if (!opMode.opModeIsActive() || opMode.isStopRequested()) {
- return false;
- }*/
-
- /*if (timer.seconds() > 8.0) {
- telemetry.addLine("Obelisk scan timed out");
- telemetry.update();
- return false;
- }*/
-
+ // Scan the limelight for the obelisk tag on pipeline 2.
aprilTag.scanObeliskTag();
- obeliskId = aprilTag.getObeliskId();
+ obeliskId = aprilTag.getObeliskId(); // cache locally for retrieval
+ // Stop scanning once a valid motif tag is found.
if (obeliskId == 21 || obeliskId == 22 || obeliskId == 23)
- return false;
+ return false; // valid tag found: action complete
- return true;
+ return true; // still searching: run again next iteration
}
};
}
+ // -----------------------------------------------------------------------
+ // Periodic background action
+ // -----------------------------------------------------------------------
+
+ /**
+ * Returns a continuous background {@link Action} that runs every loop
+ * iteration to keep subsystems updated.
+ *
+ * This action is designed to be raced or run in parallel with trajectory
+ * actions so that the indexer, outtake RPM controller, and pose estimator
+ * remain current even while Road Runner is executing a path.
+ *
+ * The action terminates when the OpMode is stopped.
+ *
+ * @return a looping {@link Action} that ticks pose estimation, outtake, and indexer
+ */
public Action actionPeriodic() {
return new Action() {
@Override
public boolean run(@NonNull TelemetryPacket telemetryPacket) {
+ // Stop if the OpMode ends so this action doesn't outlive the OpMode.
if (!opMode.opModeIsActive() || opMode.isStopRequested()) {
- return false;
+ return false; // terminate the periodic action
}
- drive.updatePoseEstimate();
- outtake.periodic();
- indexer.update();
+ // Tick all subsystems that need per-loop updates.
+ drive.updatePoseEstimate(); // integrate odometry/IMU into the pose estimate
+ outtake.periodic(); // update flywheel RPM measurement and controller
+ indexer.update(); // run indexer PD control and color sensing
- telemetry.addData("obelisk id: ", obeliskId);
- telemetry.update(); // could remove later
+ telemetry.addData("obelisk id: ", obeliskId); // show last obelisk ID on dashboard
+ telemetry.update();
+ // If continuous heading lock is enabled, scan the goal tag and compute correction.
if (continuousAprilTagLock) {
- aprilTag.scanGoalTag();
+ aprilTag.scanGoalTag(); // update bearing from Limelight
double bearing = aprilTag.getBearing();
double turn = 0;
if (!Double.isNaN(bearing)) {
+ // Compute turn correction but (currently) not applied to trajectory.
turn = aprilAimer.calculateTurnPowerFromBearing(bearing);
}
-
-
}
- return true;
+ return true; // still active; run again next iteration
}
};
}
+ // -----------------------------------------------------------------------
+ // Park action
+ // -----------------------------------------------------------------------
+
+ /**
+ * Returns an {@link Action} representing the end-of-auto parking sequence.
+ * Currently a placeholder (1-second sleep); future implementation will
+ * raise vertical slides or drive to the parking zone.
+ *
+ * @return a parking {@link Action} (placeholder)
+ */
public Action actionPark() {
// vert slides
return new ParallelAction(
- new SleepAction(1)
+ new SleepAction(1) // placeholder: 1-second wait for park position
);
}
+ // -----------------------------------------------------------------------
+ // Accessors
+ // -----------------------------------------------------------------------
+
+ /**
+ * Returns the most recently detected obelisk AprilTag ID.
+ * Valid motif IDs are 21, 22, and 23. Returns 0 if no tag has been detected.
+ *
+ * @return the obelisk tag ID, or 0 if not yet detected
+ */
public int getObeliskId() {
- return obeliskId;
+ return obeliskId; // most recently scanned obelisk AprilTag ID
}
-
+ // -----------------------------------------------------------------------
+ // Private helpers
+ // -----------------------------------------------------------------------
+
+ /**
+ * Checks whether the indexer slot at {@code stateIndex} and the next slot
+ * (modulo 3) match the first two colours of {@code desired}.
+ *
+ * Used by {@link #rotateToMotifColorBeforeOuttake} to find the starting
+ * rotation that aligns with the desired firing order.
+ *
+ * @param stateIndex zero-based indexer slot index to check
+ * @param desired the desired shooting order (must have at least 2 elements)
+ * @return {@code true} if slot[stateIndex] == desired[0] AND slot[(stateIndex+1)%3] == desired[1]
+ */
private boolean matchesOrder(int stateIndex, Indexer.ArtifactColor[] desired) {
+ // Check the first slot against desired[0] and the next slot (wrapping) against desired[1].
return indexer.getColorAt(Indexer.IndexerState.values()[stateIndex % 3]) == desired[0]
&& indexer.getColorAt(Indexer.IndexerState.values()[(stateIndex + 1) % 3]) == desired[1];
}
@@ -319,59 +627,96 @@ private boolean matchesOrder(int stateIndex, Indexer.ArtifactColor[] desired) {
// HELPERS
+ /**
+ * Applies per-row colour assignments to the indexer, rotated so that the
+ * first intaken artifact lands in {@code startingSlot}.
+ *
+ * Row-to-colour mapping (based on field artifact placement):
+ * The rotation ensures that if the first artifact is intaken into slot
+ * {@code startingSlot}, the subsequent slots are assigned colours in the
+ * order they will be collected.
+ *
+ * @param row field row number (0–3); determines the colour pattern
+ * @param startingSlot the indexer slot index where the first intaken artifact
+ * will land (basically {@code (lastMovedSlot + 1) % 3} in intake mode)
+ */
// Sets the indexer's color configuration based on a given row,
// rotated so that the first intaken ball is placed in startingSlot
private void applyCurrentColorsFromRow(int row, int startingSlot /* basically (the last moved to slot + 1) % 3 [in intaking mode]*/) {
- Indexer.ArtifactColor[] intakeOrder;
+ Indexer.ArtifactColor[] intakeOrder; // colour order for this row (index 0 = first intaken)
+ // Map the row number to the known artifact color sequence for that row.
switch (row) {
- case 1: // P P G
+ case 1: // Row 1 artifact pattern: Purple, Purple, Green
intakeOrder = new Indexer.ArtifactColor[] {Indexer.ArtifactColor.PURPLE, Indexer.ArtifactColor.PURPLE, Indexer.ArtifactColor.GREEN};
break;
- case 2: // P G P
+ case 2: // Row 2 artifact pattern: Purple, Green, Purple
intakeOrder = new Indexer.ArtifactColor[] {Indexer.ArtifactColor.PURPLE, Indexer.ArtifactColor.GREEN, Indexer.ArtifactColor.PURPLE};
break;
case 0:
- case 3: // G P P
+ case 3: // Row 0 or 3 artifact pattern: Green, Purple, Purple
intakeOrder = new Indexer.ArtifactColor[] {Indexer.ArtifactColor.GREEN, Indexer.ArtifactColor.PURPLE, Indexer.ArtifactColor.PURPLE};
break;
default:
- return;
+ return; // unknown row: do not change the indexer colour state
}
- // Rotate array so the first intaken ball lands in startingSlot
+ // Rotate the colour array so the first intaken artifact lands in startingSlot.
+ // rotated[startingSlot + i] = intakeOrder[i] (modulo 3)
Indexer.ArtifactColor[] rotated = new Indexer.ArtifactColor[3];
for (int i = 0; i < 3; i++) {
- rotated[(startingSlot + i) % 3] = intakeOrder[i];
+ rotated[(startingSlot + i) % 3] = intakeOrder[i]; // place each colour in its rotated slot
}
+ // Apply the rotated colour assignments to the indexer's internal colour table.
indexer.initializeColors(rotated[0], rotated[1], rotated[2]);
}
+ /**
+ * Converts an obelisk AprilTag ID to the corresponding desired artifact
+ * shooting order (which colour should be fired first, second, and third).
+ *
+ * Tag-to-order mapping:
+ * Creating a single {@code Hardware} object in the OpMode's {@code runOpMode()}
+ * method ensures that:
+ * All fields are {@code public final} so they can be read directly without
+ * getters; this keeps autonomous OpMode code concise.
+ *
+ * Typical usage:
+ * Autonomous-specific settings applied to the indexer:
+ * {@link #buildPath} constructs a single chained trajectory that:
+ * Many intermediate steps (obelisk scan, shoot with lock, individual intake
+ * cycles) are currently commented out while the path structure is being tuned.
+ * These are preserved in comments so they can be re-enabled as subsystems are
+ * validated.
+ *
+ * Coordinate system: all positions are Road Runner field-frame inches,
+ * with X pointing right and Y pointing forward.
+ */
public class Paths {
+ /**
+ * Builds the full autonomous path as a single chained Road Runner
+ * {@link Action} that can be passed directly to
+ * {@code Actions.runBlocking()}.
+ *
+ * Path outline:
+ * Three positions are defined:
+ * The servo is mapped by the hardware configuration name {@code "actuator"} and
+ * operates over a 0–360-degree range (FTCLib {@link SimpleServo}). The position
+ * values below are fractional (0.0–1.0) within that range.
+ *
+ * Annotated with {@link Config} so every {@code public static} field can be
+ * tuned live in the FTC Dashboard without redeploying.
+ */
@Config
public class Actuator {
- public static double DOWN = 0.45; // flush with the floor of platform
- public static double UP_QUICK = 0.22; // used for quick (non-indexed) outtake
- public static double UP_INDEXED = 0.13; // used for indexed outtake\
- public enum ActuatorState
- {
+ // -----------------------------------------------------------------------
+ // Dashboard-tunable servo position constants
+ // -----------------------------------------------------------------------
+
+ /** Servo position (fractional 0–1) for the fully-lowered ramp.
+ * At this position the ramp is flush with the floor of the launch platform,
+ * allowing artifacts to sit flat while the robot intakes or drives. */
+ public static double DOWN = 0.45;
+
+ /** Servo position (fractional 0–1) for the mid-height "quick outtake" ramp.
+ * Slightly lower than {@link #UP_INDEXED}, used when all artifacts are
+ * dumped at once via full-power indexer blast rather than precise indexing. */
+ public static double UP_QUICK = 0.22;
+
+ /** Servo position (fractional 0–1) for the highest "indexed outtake" ramp.
+ * Raises the ramp to the steepest angle so a single aligned artifact slides
+ * directly into the shooter's feed zone during precision firing. */
+ public static double UP_INDEXED = 0.13;
+
+ // -----------------------------------------------------------------------
+ // State machine enum
+ // -----------------------------------------------------------------------
+
+ /**
+ * Represents the three discrete positions of the actuator servo.
+ * The state is tracked internally so that callers can query the current
+ * position without needing to read back hardware values.
+ */
+ public enum ActuatorState {
+ /** Ramp fully lowered – safe for intake and driving. */
DOWN,
+ /** Ramp at mid-height – used for quick/non-indexed shooting. */
UP_QUICK,
+ /** Ramp at maximum height – used for single-artifact indexed shooting. */
UP_INDEXED
}
+ // -----------------------------------------------------------------------
+ // Fields
+ // -----------------------------------------------------------------------
+
+ /** Tracks the last commanded position of the actuator servo. */
private ActuatorState state;
+
+ /** The physical servo hardware object. Mapped to the name {@code "actuator"}
+ * with a 0–360-degree range via FTCLib's {@link SimpleServo}. */
private final SimpleServo servo;
+
+ /** Approximate time in seconds for the servo to travel between positions.
+ * Callers that need to wait for the ramp to reach its target before
+ * firing can sleep for this duration. */
private double waitTime = 0.5; // seconds
+ // -----------------------------------------------------------------------
+ // Constructor
+ // -----------------------------------------------------------------------
+
+ /**
+ * Initialises the Actuator by fetching the servo from the hardware map.
+ *
+ * @param hardwareMap the robot's hardware map, used to look up the servo
+ * device registered under the name {@code "actuator"}
+ */
public Actuator(HardwareMap hardwareMap) {
+ // Create a SimpleServo wrapper that expects a 0–360° operating range.
+ // FTCLib converts the 0–1 fractional position values to the actual
+ // PWM range required by the underlying servo hardware.
servo = new SimpleServo(hardwareMap, "actuator", 0, 360);
}
+ // -----------------------------------------------------------------------
+ // Actuator movement methods
+ // -----------------------------------------------------------------------
+
+ /**
+ * Lowers the ramp to its fully-down position ({@link #DOWN}).
+ * Should be called after every shooting sequence or during intake so
+ * artifacts can collect properly.
+ */
public void down() {
- servo.setPosition(DOWN);
- state = ActuatorState.DOWN;
+ servo.setPosition(DOWN); // command servo to the lowered position
+ state = ActuatorState.DOWN; // update cached state to reflect new position
}
- //default up is indexed
+ /**
+ * Raises the ramp to the default "up" position, which is the indexed
+ * (highest) angle. Delegates to {@link #upIndexed()}.
+ *
+ * Using indexed as the default ensures that callers who don't care about
+ * the specific mode always get the most precise shooting angle.
+ */
public void up() {
+ // Default up = indexed (most precise / highest angle)
upIndexed();
}
- //higher position
+ /**
+ * Raises the ramp to the indexed outtake position ({@link #UP_INDEXED}).
+ * This is the steepest angle, used when the indexer is precisely aligned
+ * so that exactly one artifact enters the shooter barrel.
+ */
public void upIndexed() {
- servo.setPosition(UP_INDEXED);
- state = ActuatorState.UP_INDEXED;
+ servo.setPosition(UP_INDEXED); // command servo to the indexed (highest) position
+ state = ActuatorState.UP_INDEXED; // update cached state
}
- //lower position
+ /**
+ * Raises the ramp to the quick outtake position ({@link #UP_QUICK}).
+ * This is a lower angle than {@link #upIndexed()}, used during the
+ * "quick dump" sequence where the indexer spins at full power to eject
+ * all three artifacts simultaneously.
+ */
public void upQuick() {
- servo.setPosition(UP_QUICK);
- state = ActuatorState.UP_QUICK;
+ servo.setPosition(UP_QUICK); // command servo to the quick-dump (mid) position
+ state = ActuatorState.UP_QUICK; // update cached state
}
+ // -----------------------------------------------------------------------
+ // Query methods
+ // -----------------------------------------------------------------------
+
+ /**
+ * Returns {@code true} if the ramp is in any raised position (i.e. NOT down).
+ * Useful for telemetry and guard conditions that check whether the robot is
+ * ready to shoot.
+ *
+ * @return {@code true} if {@link #state} is {@link ActuatorState#UP_QUICK}
+ * or {@link ActuatorState#UP_INDEXED}; {@code false} if it is
+ * {@link ActuatorState#DOWN}
+ */
public boolean isActivated() {
+ // The actuator is considered "activated" (raised) whenever it is NOT in the DOWN state.
return !(state == ActuatorState.DOWN);
}
+ /**
+ * Returns the current cached {@link ActuatorState} of the ramp.
+ *
+ * @return the last position commanded to the actuator servo
+ */
public ActuatorState getState() {
return state;
}
+ /**
+ * Convenience setter that raises or lowers the ramp based on a boolean flag.
+ * Calls {@link #up()} when {@code true} (which defaults to indexed mode),
+ * and {@link #down()} when {@code false}.
+ *
+ * @param activate {@code true} to raise the ramp; {@code false} to lower it
+ */
public void set(boolean activate) {
- if (activate) up();
- else down();
+ if (activate) up(); // raise ramp (defaults to indexed/highest position)
+ else down(); // lower ramp to intake/transit position
}
+ /**
+ * Returns the estimated time in seconds for the servo to complete its
+ * travel between positions. Callers may use this value in a
+ * {@link com.acmerobotics.roadrunner.SleepAction} to wait for the ramp
+ * to physically reach its target before proceeding.
+ *
+ * @return servo travel wait time in seconds
+ */
public double getWaitTime() {
- return waitTime; // seconds
+ return waitTime; // seconds – used externally to gate action sequences on physical completion
}
}
\ No newline at end of file
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/AntiDefense.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/AntiDefense.java
index 8792d183016d..3b0a1b41387a 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/AntiDefense.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/AntiDefense.java
@@ -7,41 +7,159 @@
import org.firstinspires.ftc.teamcode.auto.roadrunner.miscRR.MecanumDrive;
+/**
+ * AntiDefense provides a pose-hold (position lock) capability for the robot.
+ *
+ * When a defending robot makes contact and tries to push this robot off its
+ * intended position, AntiDefense counteracts the push by computing the error
+ * between the robot's current pose and a saved reference pose, then commanding
+ * the drive motors with a proportional correction signal to drive back toward
+ * that reference.
+ *
+ * Usage:
+ * Road Runner's pose representation already normalises headings, but
+ * this helper is needed after {@code Pose2d.minusExp()} which may return
+ * a heading outside the standard range.
+ *
+ * @param angle heading error in radians
+ * @return equivalent angle in (-π, π]
+ */
private double clampAngle(double angle) {
- angle = angle % (2 * Math.PI);
- if (angle > Math.PI) angle -= 2 * Math.PI;
- if (angle < -Math.PI) angle += 2 * Math.PI;
+ angle = angle % (2 * Math.PI); // reduce to (-2π, 2π)
+ if (angle > Math.PI) angle -= 2 * Math.PI; // fold the [π, 2π) half
+ if (angle < -Math.PI) angle += 2 * Math.PI; // fold the (-2π, -π] half
return angle;
}
+
+ // -----------------------------------------------------------------------
+ // Update loop
+ // -----------------------------------------------------------------------
+
+ /**
+ * Must be called every loop iteration while the lock is active.
+ * Computes the pose error between the reference point and the current
+ * robot position, and if the error exceeds the tolerances commands
+ * proportional correction drive powers.
+ *
+ * If the lock is not active ({@link #referencePoint} is {@code null})
+ * this method returns immediately without issuing any drive command.
+ *
+ * @param currentPos the robot's current estimated field-relative pose,
+ * typically from the Road Runner localizer
+ */
public void update(Pose2d currentPos)
{
+ // Do nothing when the lock is not engaged.
if(referencePoint == null) return;
+
+ // Compute the spatial error: reference – current.
+ // minusExp() performs a pose-relative subtraction that correctly
+ // accounts for heading, giving the error in the robot's local frame.
Pose2d error = referencePoint.minusExp(currentPos);
- //double
+
+ // Extract the translational (XY) and rotational (heading) components.
Vector2d xyError = error.position;
- double theta = clampAngle(error.heading.toDouble());
+ double theta = clampAngle(error.heading.toDouble()); // wrap heading error to (-π, π]
+ // If the robot is already within both tolerance bands, no correction needed.
if(xyError.norm() < xyTolerance && Math.abs(theta) < aTolerance) return;
+
+ // Command a proportional velocity in the robot's local frame:
+ // - XY: position error scaled by kP gives a velocity vector pointing toward the reference.
+ // - Yaw: heading error scaled by kH gives a rotation rate to restore the heading.
drive.setDrivePowers(new PoseVelocity2d(error.position.times(kP), theta * kH));
}
}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Indexer.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Indexer.java
index 694ea4db566f..55de45988d98 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Indexer.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Indexer.java
@@ -10,231 +10,684 @@
import org.firstinspires.ftc.teamcode.subsystems.indexerUtil.ColorSensorSystem;
import org.firstinspires.ftc.teamcode.subsystems.indexerUtil.SlotState;
+/**
+ * Indexer manages the three-slot rotating drum that holds and classifies game
+ * artifacts (game elements) before they are fired by the shooter.
+ *
+ * The drum is a circular magazine with three equally-spaced slots (0, 1, 2)
+ * separated by 120° of rotation. A continuous-rotation servo
+ * ({@link CRServoPositionControl}) drives the drum; an analog encoder tracks its
+ * absolute angle. A single colour sensor ({@link ColorSensorSystem}) is mounted
+ * at a fixed position; as the drum rotates, each slot passes in front of the sensor.
+ *
+ * Each slot is assigned one of four colours:
+ * {@link ArtifactColor#GREEN}, {@link ArtifactColor#PURPLE},
+ * {@link ArtifactColor#EMPTY} (no artifact), or
+ * {@link ArtifactColor#UNKNOWN} (present but hue is ambiguous).
+ * Classification is probabilistic: multiple sensor reads are accumulated in a
+ * {@link org.firstinspires.ftc.teamcode.subsystems.indexerUtil.SlotObservation}
+ * per slot; the winner must exceed a configurable threshold.
+ *
+ * Annotated with {@link Config} so all dashboard-tunable fields are editable
+ * live in the FTC Dashboard.
+ */
@Config
public class Indexer {
- public enum ArtifactColor { GREEN, PURPLE, EMPTY, UNKNOWN }
+ // -----------------------------------------------------------------------
+ // Enums
+ // -----------------------------------------------------------------------
+
+ /**
+ * Represents the colour (or absence) of an artifact stored in a slot.
+ * Used both as the output of the colour classifier and as the stored
+ * state in each {@link SlotState}.
+ */
+ public enum ArtifactColor {
+ /** The artifact is green (one of two colours used in this game). */
+ GREEN,
+ /** The artifact is purple (the other game colour). */
+ PURPLE,
+ /** No artifact is present in this slot. */
+ EMPTY,
+ /** An artifact is detected but its colour cannot be reliably determined
+ * (sensor quality gate failed or hue is outside both defined ranges). */
+ UNKNOWN
+ }
+ /**
+ * Names for each of the three drum positions.
+ * Each state holds its integer {@link #index} (0, 1, 2) and provides
+ * {@link #next()} and {@link #last()} helpers for traversing the ring.
+ */
public enum IndexerState {
- zero(0), one(1), two(2);
+ /** Slot at index 0 (drum home position). */
+ zero(0),
+ /** Slot at index 1 (120° from home). */
+ one(1),
+ /** Slot at index 2 (240° from home). */
+ two(2);
+
+ /** The integer index of this slot (0, 1, or 2). */
public final int index;
+
+ /**
+ * Constructs an IndexerState with the given index.
+ * @param index the position index of this slot
+ */
IndexerState(int index) { this.index = index; }
+
+ /**
+ * Returns the next slot in the drum (wrapping from 2 → 0).
+ * @return the IndexerState immediately clockwise of this one
+ */
public IndexerState next() { return values()[(index + 1) % values().length]; }
+
+ /**
+ * Returns the previous slot in the drum (wrapping from 0 → 2).
+ * @return the IndexerState immediately counter-clockwise of this one
+ */
public IndexerState last() { return values()[(index + values().length - 1) % values().length]; }
}
- // Dashboard control
+ // -----------------------------------------------------------------------
+ // Dashboard overrides
+ // -----------------------------------------------------------------------
+
+ /** When {@code true} (rising edge), advances the drum by one slot.
+ * Set from FTC Dashboard for manual testing without a gamepad. */
public static boolean dashAdvance = false;
+
+ /** Target slot index commanded from the FTC Dashboard.
+ * -1 = disabled; 0/1/2 forces the drum to that specific slot.
+ * Used to manually position the drum during tuning. */
public static int dashTargetSlot = -1; // -1 = disabled; 0/1/2 = slot
- //Things that should be toggled depending on use case (more robust version coming)
- // public boolean ENABLE_AUTO_ADVANCE = true;
+ // -----------------------------------------------------------------------
+ // Auto-advancement policy enum
+ // -----------------------------------------------------------------------
+ /**
+ * Selects how (or whether) the indexer automatically advances to the next
+ * empty slot during intaking.
+ */
public enum AutoAdvancement
{
+ /** Never auto-advance; caller controls drum position entirely. */
DISABLED,
+ /** Advance to the next empty slot every loop iteration while the drum
+ * is aligned and an empty slot exists. */
SEEK_EMPTY,
+ /** Same as SEEK_EMPTY but only triggers when the sensor detects an
+ * artifact (avoids unnecessary drum movement when the intake is idle). */
QUICK_SEEK
}
+ // -----------------------------------------------------------------------
+ // Mode flags (configurable per use case)
+ // -----------------------------------------------------------------------
+
+ /** Current auto-advancement policy for this session.
+ * Should be set in the Hardware or OpMode constructor before use. */
public AutoAdvancement AUTO_ADVANCEMENT_MODE = AutoAdvancement.QUICK_SEEK;
+ /** When {@code true}, enables the full-drum "unknown scan" logic that
+ * rotates to unclassified slots to attempt a colour re-read when the drum
+ * is full but some slots remain UNKNOWN. Disable in autonomous to
+ * save time when the slot colours are already pre-known. */
public boolean ENABLE_FULL_UNKNOWN_SCAN = true;
+
+ /** When {@code true}, the sensor classification pipeline runs every loop
+ * iteration to update slot colours. Setting to {@code false} disables
+ * all colour sensing (useful in autonomous where colours are pre-assigned). */
public boolean SCAN_COLORS = true;
- // Auto-advance detection mode
+ // -----------------------------------------------------------------------
+ // Auto-advance sensitivity (HARD vs SOFT)
+ // -----------------------------------------------------------------------
+
+ /**
+ * Selects whether auto-advancement requires many sensor confirmations (HARD)
+ * or few (SOFT) before accepting that an artifact has entered the slot.
+ * SOFT is faster but may false-trigger; HARD is slower but more robust.
+ */
public enum AutoDetectMode { HARD, SOFT }
+
+ /** Currently active auto-detect mode (dashboard tunable). */
public static AutoDetectMode AUTO_DETECT_MODE = AutoDetectMode.SOFT;
+
+ /** Number of consecutive sensor non-empty reads required in HARD mode
+ * before an artifact is considered fully seated in the current slot. */
public static int HARD_NONEMPTY_HITS_TO_ADVANCE = 8;
+
+ /** Number of consecutive sensor non-empty reads required in SOFT mode
+ * before an artifact is considered fully seated in the current slot. */
public static int SOFT_NONEMPTY_HITS_TO_ADVANCE = 3;
+ // -----------------------------------------------------------------------
+ // Drum angle offsets (degrees)
+ // -----------------------------------------------------------------------
- // Offsets
+ /** Base angle offset added to every slot's computed center angle.
+ * Calibrate so that {@code slot 0} aligns with the colour sensor when
+ * the encoder reads 0°. */
public static double offsetAngle = 92.0;
+
+ /** Additional angle offset applied when the indexer is in autonomous-outtake
+ * mode ({@link #autoOuttaking} == true). Accounts for the difference between
+ * the intake sensor position and the shooter feed position during auto. */
public static double autoOuttakeOffsetAngle = 0.0;
+
+ /** Additional angle offset applied when the indexer is in teleop-outtake
+ * mode ({@link #intaking} == false and {@link #autoOuttaking} == false).
+ * Rotates the drum ~186° so the slot that was facing the intake now faces
+ * the shooter barrel. */
public static double outtakeOffsetAngle = 186.0;
- // Slot spacing for color sensing
+ // -----------------------------------------------------------------------
+ // Slot geometry constants
+ // -----------------------------------------------------------------------
+
+ /** Angular distance (degrees) between consecutive slot centers.
+ * Three slots evenly spaced → 120° apart. */
private static final double SLOT_SPACING_DEG = 120.0;
+
+ /** Maximum angular error (degrees) between the drum's measured position and
+ * a slot's center before observations are attributed to that slot.
+ * A tighter value prevents mis-attributing reads taken while the drum is
+ * still in transit. */
private static final double SLOT_ASSIGN_TOLERANCE = 15.0;
- // Scan timing (kept for compatibility if later used)
+ // -----------------------------------------------------------------------
+ // Scan timing
+ // -----------------------------------------------------------------------
+
+ /** Scaling factor: estimated servo travel time (ms) per degree of rotation.
+ * Computed as {@code scanDelayMs = deltaCW * msPerDegree} and clamped to
+ * [{@link #minWait}, {@link #maxWait}]. Kept for compatibility; may not
+ * currently be used by active code paths. */
private static final double msPerDegree = 0.6;
+
+ /** Minimum scan delay (ms): always wait at least this long after issuing a
+ * moveTo command before reading the sensor to avoid transit-phase reads. */
private static final double minWait = 100;
+
+ /** Maximum scan delay (ms): caps the computed delay even for large angular
+ * moves so the robot does not stall waiting for the sensor. */
private static final double maxWait = 300;
- // Cooldown to avoid spamming moves while full+unknown
+
+ /** Minimum milliseconds between consecutive unknown-slot scan attempts.
+ * Prevents the drum from spinning continuously when it is full of UNKNOWN
+ * slots and none of them can be resolved. */
public static long UNKNOWN_SCAN_COOLDOWN_MS = 250;
- // Thresholds
+ // -----------------------------------------------------------------------
+ // Colour classification thresholds (fractions of total observations)
+ // -----------------------------------------------------------------------
+
+ /** Fraction of readings that must be GREEN for a slot to be declared GREEN.
+ * Lower values make green detection more sensitive (easier to trigger). */
public static double GREEN_THRESHOLD = 0.2;
+
+ /** Fraction of readings that must be PURPLE for a slot to be declared PURPLE. */
public static double PURPLE_THRESHOLD = 0.2;
+
+ /** Fraction of readings that must be EMPTY for a slot to be declared EMPTY.
+ * High threshold (0.9) prevents spurious "empty" declarations when an artifact
+ * is briefly wobbling in the slot. */
public static double EMPTY_THRESHOLD = 0.9;
+
+ /** Fraction of readings that must be UNKNOWN for a slot to be declared UNKNOWN. */
public static double UNKNOWN_THRESHOLD = 0.6;
- // Minimum hits before allowing a color change
+ // -----------------------------------------------------------------------
+ // Observation windowing
+ // -----------------------------------------------------------------------
+
+ /** Minimum number of sensor observations accumulated before a colour decision
+ * is made for a slot. Prevents snap decisions from single noisy reads. */
public static int MIN_HITS_FOR_DECISION = 4;
- // Cap samples to keep responsiveness
+
+ /** Maximum number of observations kept per slot (sliding-window cap).
+ * Older samples are proportionally scaled down by
+ * {@link org.firstinspires.ftc.teamcode.subsystems.indexerUtil.SlotObservation#trimToMax}
+ * when this limit is reached. */
public static int MAX_HITS_TO_KEEP = 20;
+ /** Angular tolerance (degrees) used by {@link #isWithinTargetDegrees} to check
+ * whether the drum has reached its commanded position. Used by auto-advance
+ * logic to confirm the drum is settled before calling {@link #findEmptySlot}. */
public static double ADVANCE_ANGLE_TOLERANCE = 5.0;
- // Telemetry object
+ // -----------------------------------------------------------------------
+ // Telemetry
+ // -----------------------------------------------------------------------
+
+ /** Optional telemetry output for debug. {@code null} means no telemetry output. */
private Telemetry telemetry = null;
+
+ /**
+ * Attaches a telemetry object for debug output in the classification pipeline.
+ * @param t the FTC telemetry instance to write to; or {@code null} to disable
+ */
public void setTelemetry(Telemetry t) { telemetry = t; }
- // Objects
+ // -----------------------------------------------------------------------
+ // Hardware
+ // -----------------------------------------------------------------------
+
+ /** Wrapped colour sensor that classifies artifacts as GREEN, PURPLE, EMPTY, or UNKNOWN. */
private final ColorSensorSystem colorSensor;
+
+ /** Closed-loop position controller for the drum's continuous-rotation servo. */
private final CRServoPositionControl servoControl;
+ // -----------------------------------------------------------------------
// Internal state
+ // -----------------------------------------------------------------------
+
+ /** The currently targeted drum slot (the slot that is (or will be) aligned
+ * with the sensor or shooter). */
private IndexerState state = IndexerState.zero;
+
+ /** {@code true} when the drum is oriented for intaking (sensor at intake opening);
+ * {@code false} when oriented for outtaking (shooter barrel position). */
private boolean intaking = true;
+
+ /** {@code true} when an autonomous outtake sequence is active. Causes the
+ * drum to use {@link #autoOuttakeOffsetAngle} instead of the teleop offset. */
private boolean autoOuttaking = false;
- private boolean loaded = false; // "anything present" (sensor OR stored memory)
- private boolean noEmpty = false; // FULL: true when there are NO stored EMPTY slots
+ /** {@code true} when at least one artifact is present in the drum (either
+ * detected by the sensor right now, or stored as GREEN/PURPLE in a slot's memory).
+ * Used to switch between loaded and unloaded servo gains. */
+ private boolean loaded = false;
+
+ /** {@code true} when all three slot memories are non-EMPTY (GREEN, PURPLE, or UNKNOWN).
+ * Acts as the "drum is full" flag used to stop auto-advance from looking for an empty slot. */
+ private boolean noEmpty = false;
+
+ /**
+ * Returns whether all three slots in memory are occupied (no EMPTY slots stored).
+ * @return {@code true} if the drum is full
+ */
public boolean isFull() { return noEmpty; }
+ // -----------------------------------------------------------------------
// Per-slot state
+ // -----------------------------------------------------------------------
+
+ /** Array of three {@link SlotState} objects, one per drum slot (index 0, 1, 2).
+ * Each holds the stored colour, hit counts, and fill-cycle flags for that slot. */
private final SlotState[] slots = {
- new SlotState(),
- new SlotState(),
- new SlotState()
+ new SlotState(), // slot 0 (index zero)
+ new SlotState(), // slot 1 (index one)
+ new SlotState() // slot 2 (index two)
};
- // Scan scheduling
+ // -----------------------------------------------------------------------
+ // Timers
+ // -----------------------------------------------------------------------
+
+ /** Timer used to compute the estimated scan delay after issuing a moveTo command.
+ * Compared against {@link #scanDelayMs} to know when the drum is likely on-target. */
private final ElapsedTime scanTimer = new ElapsedTime();
+
+ /** Estimated time (ms) for the drum to travel to the most recently commanded slot. */
private double scanDelayMs;
- // Full+unknown scan cooldown
+ /** Cooldown timer for the unknown-slot scan logic. Prevents the drum from
+ * spinning continuously in a failed scan loop. */
private final ElapsedTime unknownScanTimer = new ElapsedTime();
- // Dashboard edge detection
+ // -----------------------------------------------------------------------
+ // Dashboard edge-detection state
+ // -----------------------------------------------------------------------
+
+ /** Previous value of {@link #dashAdvance} used to detect rising edges
+ * (false→true transitions) rather than reacting every loop while it is held true. */
private boolean lastDashAdvance = false;
+
+ /** Previous value of {@link #dashTargetSlot} used to detect changes so
+ * moveTo is only called once per slot selection, not every loop. */
private int lastDashTargetSlot = -1;
+ // -----------------------------------------------------------------------
+ // Constructor
+ // -----------------------------------------------------------------------
+
+ /**
+ * Creates an Indexer instance. Retrieves the servo and encoder from the
+ * hardware map, constructs the control and sensor subsystems, and resets timers.
+ *
+ * @param hardwareMap the robot's hardware map used to resolve:
+ * When {@code forceRecommend} is {@code true}, the target angle is always
+ * sent to the servo controller even if {@code newState == state}. This is
+ * needed when the angle offset has changed (e.g., switching intake/outtake mode)
+ * and the servo needs to move to a new physical position even if the logical
+ * slot index hasn't changed.
+ *
+ * @param newState the slot to rotate to
+ * @param forceRecommend if {@code true}, re-sends the angle even if already at target
+ */
public void moveTo(IndexerState newState, boolean forceRecommend) {
- if (!forceRecommend && newState == state) return;
+ if (!forceRecommend && newState == state) return; // already there and no force; nothing to do
- double targetAngle = getSlotCenterAngle(newState);
+ double targetAngle = getSlotCenterAngle(newState); // compute the physical angle for this slot
+ // Estimate the clockwise travel distance to compute a scan delay.
double currentWrapped = getMeasuredAngle();
double deltaCW = targetAngle - currentWrapped;
- if (deltaCW < 0) deltaCW += 360.0;
+ if (deltaCW < 0) deltaCW += 360.0; // wrap negative delta to positive clockwise arc
+ // Estimate travel time and clamp to safe bounds so sensors are not read mid-transit.
scanDelayMs = clamp(deltaCW * msPerDegree, minWait, maxWait);
- scanTimer.reset();
+ scanTimer.reset(); // start the travel-time countdown
- servoControl.clearOpenLoop();
- servoControl.moveToAngle(targetAngle);
- state = newState;
+ servoControl.clearOpenLoop(); // ensure closed-loop mode is active (not blasting)
+ servoControl.moveToAngle(targetAngle); // issue the position command to the servo controller
+ state = newState; // update logical state immediately
}
- //sortedquickspin api
+ // -----------------------------------------------------------------------
+ // Quickspin API
+ // -----------------------------------------------------------------------
+
+ /**
+ * Prepares the drum for a "quickspin" timed outtake by rotating to the
+ * correct starting slot so that artifacts exit in the desired colour order.
+ *
+ * Prerequisites:
+ * Processing order:
+ * The loaded flag is {@code true} when either:
+ * The "protect known" guard prevents a good GREEN or PURPLE classification
+ * from being overwritten by a transient UNKNOWN or EMPTY read (e.g., if the
+ * artifact wiggles slightly and the sensor briefly loses it).
+ *
+ * @param currentSlot the slot that was closest to the sensor on this cycle
+ * (used to gate telemetry output to only the active slot)
+ */
private void updateSlotClassification(IndexerState currentSlot) {
- double currentAngle = getMeasuredAngle();
+ double currentAngle = getMeasuredAngle(); // current drum angle in [0, 360)
+ // Check every slot: multiple slots could theoretically be within tolerance
+ // (shouldn't happen with 120° spacing and a 15° tolerance, but we check all for robustness).
for (IndexerState s : IndexerState.values()) {
double err = angleError(currentAngle, getSlotCenterAngle(s));
- if (err > SLOT_ASSIGN_TOLERANCE) continue; // not over sensor
+ if (err > SLOT_ASSIGN_TOLERANCE) continue; // slot is not under the sensor; skip
SlotState slot = slot(s);
+ // Read the sensor: presence check determines whether to classify or mark EMPTY.
boolean hasArtifact = colorSensor.hasArtifact();
ArtifactColor instantColor = hasArtifact
- ? colorSensor.classifyColorOnly()
- : ArtifactColor.EMPTY;
+ ? colorSensor.classifyColorOnly() // artifact present: determine its colour
+ : ArtifactColor.EMPTY; // nothing there; record EMPTY
- // If transitioning from empty->nonempty (sensor), reset observation window for faster settle
+ // If this slot was previously empty and now sees something, reset observations
+ // so old EMPTY counts don't dilute the new non-empty classification.
if (slot.wasEmpty && hasArtifact) {
- slot.obs.reset();
+ slot.obs.reset(); // wipe the old observation window for a clean re-read
}
- slot.obs.record(instantColor);
- slot.obs.trimToMax(MAX_HITS_TO_KEEP);
+ slot.obs.record(instantColor); // add this sample to the observation window
+ slot.obs.trimToMax(MAX_HITS_TO_KEEP); // cap the window size to prevent stale data buildup
- int total = slot.obs.totalHits();
+ int total = slot.obs.totalHits(); // total samples in the current observation window
if (total >= MIN_HITS_FOR_DECISION) {
+ // Enough data: try to resolve a colour from the accumulated observations.
ArtifactColor candidate = slot.obs.resolveWithThreshold(
GREEN_THRESHOLD,
PURPLE_THRESHOLD,
@@ -379,18 +925,21 @@ private void updateSlotClassification(IndexerState currentSlot) {
ArtifactColor currentColor = slot.color;
+ // "Protect known" guard: if the slot is already classified as GREEN or PURPLE,
+ // don't let a transient UNKNOWN/EMPTY candidate overwrite it.
boolean protectKnown =
(candidate == ArtifactColor.UNKNOWN || candidate == ArtifactColor.EMPTY) &&
(currentColor == ArtifactColor.GREEN || currentColor == ArtifactColor.PURPLE);
if (!protectKnown && candidate != currentColor) {
- slot.color = candidate;
+ slot.color = candidate; // update the stored colour to the new classification
}
}
- // Update memory ONLY
+ // Update the empty-transition latch based solely on the sensor (not the stored colour).
slot.wasEmpty = !hasArtifact;
+ // Output detailed telemetry for the currently active slot only.
if (telemetry != null && s == currentSlot) {
int totalHitsTelemetry = slot.obs.totalHits();
telemetry.addData("Loaded", loaded);
@@ -398,19 +947,29 @@ private void updateSlotClassification(IndexerState currentSlot) {
telemetry.addData("Slot " + s + " hit %",
String.format(
"G: %.0f%%, P: %.0f%%, E: %.0f%%, U: %.0f%%",
- totalHitsTelemetry > 0 ? slot.obs.greenHits * 100.0 / totalHitsTelemetry : 0,
+ totalHitsTelemetry > 0 ? slot.obs.greenHits * 100.0 / totalHitsTelemetry : 0,
totalHitsTelemetry > 0 ? slot.obs.purpleHits * 100.0 / totalHitsTelemetry : 0,
- totalHitsTelemetry > 0 ? slot.obs.emptyHits * 100.0 / totalHitsTelemetry : 0,
- totalHitsTelemetry > 0 ? slot.obs.unknownHits * 100.0 / totalHitsTelemetry : 0
+ totalHitsTelemetry > 0 ? slot.obs.emptyHits * 100.0 / totalHitsTelemetry : 0,
+ totalHitsTelemetry > 0 ? slot.obs.unknownHits* 100.0 / totalHitsTelemetry : 0
));
- colorSensor.addTelemetry(telemetry);
+ colorSensor.addTelemetry(telemetry); // add full sensor HSV breakdown
}
}
}
- // debug
+ // -----------------------------------------------------------------------
+ // Debug / telemetry helpers
+ // -----------------------------------------------------------------------
+
+ /**
+ * Returns the slot whose physical center angle is closest to the drum's
+ * current measured angle. Used during classification to determine which
+ * slot is currently under the sensor.
+ *
+ * @return the nearest {@link IndexerState} to the current drum position
+ */
public IndexerState debugClosestSlot() {
- double current = getMeasuredAngle();
+ double current = getMeasuredAngle(); // current drum angle
IndexerState best = null;
double bestErr = Double.MAX_VALUE;
@@ -418,26 +977,53 @@ public IndexerState debugClosestSlot() {
for (IndexerState s : IndexerState.values()) {
double err = angleError(current, getSlotCenterAngle(s));
if (err < bestErr) {
- bestErr = err;
+ bestErr = err; // found a closer slot
best = s;
}
}
- return best;
+ return best; // the slot with the smallest angular error to current position
}
+ /**
+ * Returns the angular error (degrees) between the drum's current position and
+ * the closest slot's center. Useful in telemetry to assess positional accuracy.
+ *
+ * @return absolute angular error in [0, 180°]
+ */
public double debugClosestSlotErrorDeg() {
- IndexerState s = debugClosestSlot();
- return angleError(getMeasuredAngle(), getSlotCenterAngle(s));
+ IndexerState s = debugClosestSlot(); // find the closest slot first
+ return angleError(getMeasuredAngle(), getSlotCenterAngle(s)); // compute error to that slot
}
+ /**
+ * Returns the angular error (degrees) between the drum's current position and
+ * the center angle of the specified slot.
+ *
+ * @param s the slot to measure against
+ * @return absolute angular error in [0, 180°]
+ */
public double debugSlotErrorDeg(IndexerState s) {
return angleError(getMeasuredAngle(), getSlotCenterAngle(s));
}
+ /**
+ * Returns {@code true} if the given slot is currently within
+ * {@link #SLOT_ASSIGN_TOLERANCE} of the sensor – i.e., the slot is "over" it.
+ *
+ * @param s the slot to test
+ * @return {@code true} if the slot is aligned with the colour sensor
+ */
public boolean debugSlotIsOverSensor(IndexerState s) {
return debugSlotErrorDeg(s) <= SLOT_ASSIGN_TOLERANCE;
}
+ /**
+ * Returns a human-readable string describing the drum's current alignment status.
+ * Used for telemetry dashboards during debugging.
+ *
+ * @return description such as {@code "Aligned with zero"} or
+ * {@code "Between slots (err=14.2 degrees)"}
+ */
public String debugAssignmentReason() {
IndexerState s = debugClosestSlot();
double err = debugClosestSlotErrorDeg();
@@ -448,6 +1034,13 @@ public String debugAssignmentReason() {
return "Aligned with " + s;
}
+ /**
+ * Returns the angular error of the second-closest slot (degrees).
+ * Used to verify that the closest-slot algorithm is not confused by two slots
+ * being nearly equidistant (which would indicate the drum is exactly between slots).
+ *
+ * @return angular error of the second-nearest slot to the current position
+ */
public double debugSecondClosestSlotErrorDeg() {
double current = getMeasuredAngle();
double best = Double.MAX_VALUE;
@@ -456,127 +1049,232 @@ public double debugSecondClosestSlotErrorDeg() {
for (IndexerState s : IndexerState.values()) {
double err = angleError(current, getSlotCenterAngle(s));
if (err < best) {
- second = best;
+ second = best; // demote the previous best to second
best = err;
} else if (err < second) {
- second = err;
+ second = err; // new second-best
}
}
return second;
}
+ /**
+ * Returns the raw encoder voltage from the drum's analog encoder.
+ * @return encoder voltage in volts
+ */
public double getVoltage() { return servoControl.getVoltage(); }
+
+ /**
+ * Returns the expected encoder voltage that corresponds to the current target angle.
+ * @return target voltage in volts
+ */
public double getTargetVoltage() { return servoControl.getTargetVoltage(); }
- /* =========================
- UTIL
- ========================= */
+ // -----------------------------------------------------------------------
+ // Private utility methods
+ // -----------------------------------------------------------------------
+ /**
+ * Returns the number of consecutive non-empty sensor hits required before
+ * auto-advancing, based on the current {@link #AUTO_DETECT_MODE}.
+ *
+ * @return hit threshold for the current mode
+ */
private int requiredNonEmptyHitsToAdvance() {
return (AUTO_DETECT_MODE == AutoDetectMode.HARD)
- ? HARD_NONEMPTY_HITS_TO_ADVANCE
- : SOFT_NONEMPTY_HITS_TO_ADVANCE;
+ ? HARD_NONEMPTY_HITS_TO_ADVANCE // conservative: 8 hits
+ : SOFT_NONEMPTY_HITS_TO_ADVANCE; // responsive: 3 hits
}
+ /**
+ * Recomputes the {@link #noEmpty} flag by scanning all slot memories.
+ * {@code noEmpty} is {@code true} if and only if no slot stores EMPTY.
+ */
private void recomputeNoEmpty() {
boolean anyEmpty = false;
for (SlotState slot : slots) {
if (slot.color == ArtifactColor.EMPTY) {
- anyEmpty = true;
+ anyEmpty = true; // found an empty slot; short-circuit
break;
}
}
- noEmpty = !anyEmpty;
+ noEmpty = !anyEmpty; // full = no empty slots remain
}
+ /**
+ * Returns {@code true} when every slot has been classified as either
+ * GREEN or PURPLE (i.e., no UNKNOWN or EMPTY slots remain).
+ * Used to skip colour scanning once all colours are known.
+ *
+ * @return {@code true} if all slots have a definitive colour
+ */
private boolean allClassified() {
//checks if all are assigned a color (no unknown or empty)
for (SlotState slot : slots) {
if (slot.color == ArtifactColor.UNKNOWN || slot.color == ArtifactColor.EMPTY) {
- return false;
+ return false; // at least one slot is unresolved
}
}
- return true;
+ return true; // every slot has been assigned GREEN or PURPLE
}
+ /**
+ * Searches the drum for the next slot with the given stored colour, starting
+ * from {@code start} and iterating forward (by {@code next()}) up to 3 times.
+ *
+ * @param start the starting position (search begins at start.next())
+ * @param color the colour to search for
+ * @return the first matching {@link IndexerState}, or {@code null} if none found
+ */
private IndexerState findNextSlotWithStoredColor(IndexerState start, ArtifactColor color) {
IndexerState s = start;
for (int i = 0; i < 3; i++) {
- s = s.next();
- if (slot(s).color == color) return s;
+ s = s.next(); // advance to the next slot in the ring
+ if (slot(s).color == color) return s; // found a matching slot
}
- return null;
+ return null; // no slot with the desired colour found after checking all three
}
+ /**
+ * Finds the slot with the highest score for the desired colour using
+ * {@link #scoreSlotForTarget}. Returns the {@link IndexerState} with the
+ * best match, or {@code null} if no slot scores above 0.
+ *
+ * @param desired the colour to search for
+ * @return the best-matching slot, or {@code null} if all slots are EMPTY
+ */
public IndexerState findBestSlotForColor(ArtifactColor desired) {
IndexerState best = null;
int bestScore = -1;
for (IndexerState s : IndexerState.values()) {
- ArtifactColor slotColor = slot(s).color; // use live slot color
- int score = scoreSlotForTarget(desired, slotColor);
+ ArtifactColor slotColor = slot(s).color; // read the stored colour for this slot
+ int score = scoreSlotForTarget(desired, slotColor); // score this slot
if (score > bestScore) {
- bestScore = score;
+ bestScore = score; // new best
best = s;
}
}
- // If all slots are EMPTY or useless, don't move
+ // If all slots scored 0 (all empty) there is nothing useful to fire.
if (bestScore <= 0) return null;
return best;
}
+ /**
+ * Returns the first slot (searching forward from {@link #state}.last())
+ * whose stored colour is {@link ArtifactColor#EMPTY}.
+ *
+ * @return the nearest empty slot, or {@code null} if none exist
+ */
public IndexerState findEmptySlot()
{
+ // Start from state.last() so we search all three slots including the current one.
return findNextSlotWithStoredColor(state.last(), ArtifactColor.EMPTY);
}
+ /**
+ * Returns an empty slot, optionally excluding the current slot.
+ *
+ * @param includeCurrent if {@code true}, the current slot may be returned;
+ * if {@code false}, only the other two slots are checked
+ * @return the nearest qualifying empty slot, or {@code null} if none exist
+ */
public IndexerState findEmptySlot(boolean includeCurrent)
{
if(includeCurrent)
- return findEmptySlot();
+ return findEmptySlot(); // search from state.last() (includes current)
else
- return findNextSlotWithStoredColor(state.next(), ArtifactColor.EMPTY);
+ return findNextSlotWithStoredColor(state.next(), ArtifactColor.EMPTY); // skip current; search forward
}
+ /**
+ * Returns a priority score indicating how well a slot matches the desired colour.
+ * Higher scores are more desirable for outtaking.
+ *
+ * Scoring:
+ * During a forward-spin blast the drum fires slots in the order:
+ * {@code (x+2)%3 → x → (x+1)%3}, so we verify that the stored colours
+ * at those positions match {@code desired[0]}, {@code desired[1]}, and
+ * {@code desired[2]} respectively.
+ *
+ * @param x the candidate starting slot index (0, 1, or 2)
+ * @param desired the 3-element desired firing order
+ * @return {@code true} if starting at slot {@code x} produces the desired order
+ */
//firing order:(x + 2) % 3, x, (x + 1) % 3
private boolean matchesQuickspin(int x, ArtifactColor[] desired) {
- return slots[(x + 2) % 3].color == desired[0]
- && slots[x].color == desired[1]
- && slots[(x + 1) % 3].color == desired[2];
+ return slots[(x + 2) % 3].color == desired[0] // first artifact to fire
+ && slots[x].color == desired[1] // second artifact to fire
+ && slots[(x + 1) % 3].color == desired[2]; // third artifact to fire
}
+ // -----------------------------------------------------------------------
+ // Public convenience check
+ // -----------------------------------------------------------------------
+
+ /**
+ * Returns {@code true} if the colour sensor detects an artifact and
+ * the drum is within 15° of the current target slot.
+ *
+ * This combined check is used by the feedback-based intake action in
+ * {@link org.firstinspires.ftc.teamcode.auto.utils.BotActions} to determine
+ * when an artifact has been successfully collected: the indexer must be aligned
+ * (drum settled) and the sensor must confirm presence.
+ *
+ * @return {@code true} if an artifact is both detected and the drum is aligned
+ */
public boolean artifactPresentAndAligned() {
- boolean hasArtifact = colorSensor.hasArtifact();
- boolean aligned = isWithinTargetDegrees(15.0);
- return hasArtifact && aligned;
+ boolean hasArtifact = colorSensor.hasArtifact(); // sensor presence check
+ boolean aligned = isWithinTargetDegrees(15.0); // drum position check
+ return hasArtifact && aligned; // both conditions must be true simultaneously
}
}
\ No newline at end of file
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Intake.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Intake.java
index 7ddde54e3c09..4ffe95febb50 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Intake.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Intake.java
@@ -4,40 +4,139 @@
import com.arcrobotics.ftclib.hardware.motors.MotorEx;
import com.qualcomm.robotcore.hardware.HardwareMap;
+/**
+ * Intake manages the robot's ground intake roller motor.
+ *
+ * The intake roller sweeps game elements (artifacts) from the field floor
+ * into the indexer drum. Positive "intaking" direction is configured so that
+ * setting power to {@link #INTAKING_POWER} (a negative value) spins the motor
+ * in the direction that pulls artifacts in. Reversing the sign ejects artifacts
+ * back onto the field, which is useful for clearing jams.
+ *
+ * A {@link #SLOW_MULTIPLIER} is provided so that, when the indexer is
+ * actively aligning a freshly-collected artifact, the intake can be slowed
+ * down to prevent forcing a second artifact in before the first has seated.
+ *
+ * Annotated with {@link Config} so {@code SLOW_MULTIPLIER} and
+ * {@code INTAKING_POWER} can be tuned live via FTC Dashboard.
+ */
@Config
public class Intake {
+
+ // -----------------------------------------------------------------------
+ // Dashboard-tunable power constants
+ // -----------------------------------------------------------------------
+
+ /** Fraction (0–1) applied to {@link #INTAKING_POWER} when running at reduced
+ * speed. A value of 0.5 means 50 % of full intake power. Used to slow the
+ * roller while the indexer repositions so a second artifact doesn't overrun
+ * a freshly-collected one. */
public static double SLOW_MULTIPLIER = 0.5;
+
+ /** Raw motor power applied during normal full-speed intaking. Negative because
+ * the motor's forward direction physically ejects artifacts; reversing it pulls
+ * them into the robot. */
public static double INTAKING_POWER = -1;
+
+ // -----------------------------------------------------------------------
+ // Hardware
+ // -----------------------------------------------------------------------
+
+ /** FTCLib extended motor wrapper for the intake roller, mapped to the
+ * hardware config name {@code "intake"}. */
private MotorEx intakeMotor;
+ // -----------------------------------------------------------------------
+ // Constructor
+ // -----------------------------------------------------------------------
+
+ /**
+ * Creates an Intake instance and looks up the motor in the hardware map.
+ *
+ * @param hardwareMap the robot's hardware map used to resolve the motor
+ * registered under the configuration name {@code "intake"}
+ */
public Intake(HardwareMap hardwareMap)
{
+ // Retrieve the intake roller motor from the hardware map using its
+ // config name. MotorEx wraps DcMotorEx with additional helper methods.
intakeMotor = new MotorEx(hardwareMap, "intake");
}
+
+ // -----------------------------------------------------------------------
+ // Control methods
+ // -----------------------------------------------------------------------
+
+ /**
+ * Stops the intake motor immediately by cutting power to zero.
+ * Should be called whenever the intake is no longer needed to prevent
+ * unnecessary wear and to avoid accidentally ingesting extra artifacts.
+ */
public void stop()
{
- intakeMotor.stopMotor();
+ intakeMotor.stopMotor(); // set motor power to 0 and coast to a stop
}
+
+ /**
+ * Runs the intake at full speed in the intaking direction ({@link #INTAKING_POWER}).
+ * Use this during active field collection when the robot drives over artifacts.
+ */
public void run()
{
- intakeMotor.set(INTAKING_POWER);
+ intakeMotor.set(INTAKING_POWER); // full-speed intake (pulls artifacts inward)
}
+
+ /**
+ * Runs the intake at reduced speed ({@link #INTAKING_POWER} ×
+ * {@link #SLOW_MULTIPLIER}). Useful as a "passive hold" mode while the
+ * indexer positions an artifact that was just collected, preventing a second
+ * artifact from ramming in before the indexer is ready.
+ */
public void runSlow() {
- intakeMotor.set(INTAKING_POWER * SLOW_MULTIPLIER);
+ intakeMotor.set(INTAKING_POWER * SLOW_MULTIPLIER); // half-speed intake
}
+
+ /**
+ * Runs the intake in reverse at full speed, ejecting artifacts back onto
+ * the field. Useful for clearing jams or returning incorrect pieces.
+ */
public void runBackwards()
{
+ // Negating INTAKING_POWER flips the motor direction to "eject"
intakeMotor.set(-INTAKING_POWER);
}
+
+ /**
+ * Runs the intake in reverse at reduced speed ({@code -INTAKING_POWER ×
+ * SLOW_MULTIPLIER}). Provides a gentler eject for careful jam-clearing
+ * without flinging artifacts too far.
+ */
public void runBackwardsSlow() {
- intakeMotor.set(-INTAKING_POWER * SLOW_MULTIPLIER);
+ intakeMotor.set(-INTAKING_POWER * SLOW_MULTIPLIER); // slow reverse eject
}
+
+ /**
+ * Sets the intake motor to an arbitrary power level.
+ * Useful for fine-tuning or for custom autonomous sequences that need a
+ * specific power value not covered by the preset methods above.
+ *
+ * @param newPower motor power in the range [-1.0, 1.0]; positive pushes
+ * artifacts out, negative pulls them in (given the motor
+ * convention set in {@link #INTAKING_POWER})
+ */
public void setPower(double newPower)
{
- intakeMotor.set(newPower);
+ intakeMotor.set(newPower); // directly write the requested power to the motor
}
+
+ /**
+ * Returns the current power being applied to the intake motor.
+ * Used in telemetry to verify that the intake is running at the expected level.
+ *
+ * @return current motor power in the range [-1.0, 1.0]
+ */
public double getPower()
{
- return intakeMotor.get();
+ return intakeMotor.get(); // read back the last commanded power from the motor wrapper
}
}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Movement.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Movement.java
index 550684380ba6..e30f09c02e4a 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Movement.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Movement.java
@@ -12,51 +12,137 @@
import org.firstinspires.ftc.teamcode.auto.roadrunner.miscRR.MecanumDrive;
/**
- * Represents the drivetrain.
+ * Movement controls the four-wheel mecanum drivetrain.
+ *
+ * A mecanum drivetrain uses wheels with rollers set at 45° to the axle, which
+ * allows independent X, Y, and rotational motion. This class provides both a
+ * robot-centric (standard) and a field-centric teleop tick that translates
+ * driver stick inputs into per-wheel power values.
+ *
+ * Motors are wired and configured as follows:
+ * Translates three driver inputs (forward/axial, strafe/lateral, rotate/yaw)
+ * into individual wheel powers using the standard mecanum kinematics matrix.
+ * A "denominator" normalisation ensures at least one wheel always reaches
+ * full power when the driver pushes to the stick limit.
+ *
+ * An additional {@code turnCorrection} term from the AprilTag aimer is
+ * blended into the yaw component to keep the robot pointed at the goal
+ * while the driver manoeuvres.
+ *
+ * @param leftStickX horizontal axis of the left stick (-1 = strafe left, +1 = strafe right)
+ * @param leftStickY vertical axis of the left stick (-1 = drive back, +1 = drive forward)
+ * @param rightStickX horizontal axis of the right stick(-1 = turn left, +1 = turn right)
+ * @param turnCorrection additional rotation correction from vision aiming (can be 0 to disable)
+ */
public void teleopTick(double leftStickX, double leftStickY, double rightStickX, double turnCorrection){
- double axial = leftStickY * STRAFE_MULTIPLIER;
- double lateral = leftStickX * STRAFE_MULTIPLIER;
- double yaw = rightStickX * ROTATION_MULTIPLIER + turnCorrection;
+ // Scale the axial (forward) and lateral (strafe) components by the multiplier.
+ double axial = leftStickY * STRAFE_MULTIPLIER; // forward/backward
+ double lateral = leftStickX * STRAFE_MULTIPLIER; // left/right strafe
+ // Blend the driver's rotation request with the auto-aim correction.
+ double yaw = rightStickX * ROTATION_MULTIPLIER + turnCorrection;
- double leftFrontPower = axial + lateral + yaw;
- double rightFrontPower = axial - lateral - yaw;
- double leftBackPower = axial - lateral + yaw;
- double rightBackPower = axial + lateral - yaw;
+ // Mecanum kinematics: each wheel's power = sum/difference of axial, lateral, yaw.
+ double leftFrontPower = axial + lateral + yaw; // front-left: all three add
+ double rightFrontPower = axial - lateral - yaw; // front-right: lateral & yaw subtract
+ double leftBackPower = axial - lateral + yaw; // back-left: lateral subtracts, yaw adds
+ double rightBackPower = axial + lateral - yaw; // back-right: yaw subtracts
- // For smoother joystick movement
+ // Normalise: find the largest absolute wheel power. If it exceeds 1.0
+ // we divide all powers by it to keep them in [-1, 1] while preserving
+ // their ratios (smooth joystick "feel").
double denominator = Math.max(1.0, Math.abs(axial) + Math.abs(lateral) + Math.abs(yaw));
leftFrontPower /= denominator;
@@ -64,66 +150,116 @@ public void teleopTick(double leftStickX, double leftStickY, double rightStickX,
leftBackPower /= denominator;
rightBackPower /= denominator;
+ // Write the computed powers to the physical motors.
leftFront.setPower(leftFrontPower);
rightFront.setPower(rightFrontPower);
leftBack.setPower(leftBackPower);
rightBack.setPower(rightBackPower);
}
+ /**
+ * Field-centric mecanum drive tick called every loop iteration during teleop.
+ *
+ * NOTE: Field-centric mode is currently not fully functional with the
+ * AprilTag aiming system. The heading source (Road Runner localizer)
+ * must have been initialised/re-zeroed before calling this method.
+ *
+ * In field-centric mode the driver's forward direction always corresponds
+ * to the physical field "north" regardless of the robot's heading. This is
+ * achieved by rotating the stick input vector by the negative of the robot's
+ * current heading angle.
+ *
+ * @param leftStickX horizontal axis of the left stick (field-relative strafe)
+ * @param leftStickY vertical axis of the left stick (field-relative forward)
+ * @param rightStickX horizontal axis of the right stick (rotation)
+ * @param turnCorrection additional rotation correction from vision aiming
+ * @param start when {@code true} resets the localizer pose to (0, 0, 0);
+ * typically mapped to the "Start" button on an Xbox-style
+ * controller to re-zero field orientation on demand
+ */
// NOTE DOESN'T WORK WITH APRILTAG RN
public void teleopTickFieldCentric(double leftStickX, double leftStickY, double rightStickX, double turnCorrection, boolean start){
- double axial = leftStickY * STRAFE_MULTIPLIER;
- double lateral = leftStickX * STRAFE_MULTIPLIER;
- double yaw = rightStickX * ROTATION_MULTIPLIER + turnCorrection;
+ double axial = leftStickY * STRAFE_MULTIPLIER; // field-relative forward
+ double lateral = leftStickX * STRAFE_MULTIPLIER; // field-relative strafe
+ double yaw = rightStickX * ROTATION_MULTIPLIER + turnCorrection;
+ // If the driver requests a heading reset, zero the localizer pose so that
+ // the current robot orientation becomes the new field "forward".
// This button choice was made so that it is hard to hit on accident,
// it can be freely changed based on preference.
// The equivalent button is start on Xbox-style controllers.
if (start) {
- drive.localizer.setPose(new Pose2d(0, 0, 0));
+ drive.localizer.setPose(new Pose2d(0, 0, 0)); // re-zero field origin at current robot pose
}
- double botHeading = drive.localizer.getPose().heading.log();//imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.RADIANS);
+ // Read the robot's heading angle (in radians) from the Road Runner localizer.
+ double botHeading = drive.localizer.getPose().heading.log(); // imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.RADIANS);
- // Rotate the movement direction counter to the bot's rotation
+ // Rotate the driver's input vector counter to the robot's heading so that
+ // "forward" on the stick always means "field north" regardless of robot orientation.
double rotX = lateral * Math.cos(-botHeading) - axial * Math.sin(-botHeading);
double rotY = lateral * Math.sin(-botHeading) + axial * Math.cos(-botHeading);
+ // Compensate for imperfect strafing (mecanum wheels tend to slip laterally;
+ // scaling by 1.1 empirically corrects for this offset on this robot).
rotX = rotX * 1.1; // Counteract imperfect strafing
+ // Normalise: largest absolute component (or 1 if all are ≤ 1) to keep
+ // powers within [-1, 1] while maintaining their ratio.
// Denominator is the largest motor power (absolute value) or 1
// This ensures all the powers maintain the same ratio,
// but only if at least one is out of the range [-1, 1]
double denominator = Math.max(Math.abs(rotY) + Math.abs(rotX) + Math.abs(yaw), 1);
- double frontLeftPower = (rotY + rotX + yaw) / denominator;
- double backLeftPower = (rotY - rotX + yaw) / denominator;
+ double frontLeftPower = (rotY + rotX + yaw) / denominator;
+ double backLeftPower = (rotY - rotX + yaw) / denominator;
double frontRightPower = (rotY - rotX - yaw) / denominator;
- double backRightPower = (rotY + rotX - yaw) / denominator;
+ double backRightPower = (rotY + rotX - yaw) / denominator;
+ // Write field-centric powers to each wheel.
leftFront.setPower(frontLeftPower);
leftBack.setPower(backLeftPower);
rightFront.setPower(frontRightPower);
rightBack.setPower(backRightPower);
}
+ // -----------------------------------------------------------------------
+ // Motor accessors (used by autonomous and diagnostics)
+ // -----------------------------------------------------------------------
+
+ /** @return the left-front drive motor hardware object */
public DcMotor getLeftFront() {
return leftFront;
}
+ /** @return the left-rear drive motor hardware object */
public DcMotor getLeftBack() {
return leftBack;
}
+ /** @return the right-front drive motor hardware object */
public DcMotor getRightFront() {
return rightFront;
}
+ /** @return the right-rear drive motor hardware object */
public DcMotor getRightBack() {
return rightBack;
}
+ // -----------------------------------------------------------------------
+ // Pose helper
+ // -----------------------------------------------------------------------
+
+ /**
+ * Updates the Road Runner localizer's stored pose estimate.
+ * Useful after a known field event (e.g. the robot presses against a wall)
+ * that allows a hard reset of the position estimate without performing a
+ * full relocalization scan.
+ *
+ * @param newPose the new absolute pose to assign to the localizer
+ */
public void setPose(Pose2d newPose) {
- drive.localizer.setPose(newPose);
+ drive.localizer.setPose(newPose); // propagate the new pose into the Road Runner localizer
}
}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Outtake.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Outtake.java
index 7b9415abcb29..8e5420d18336 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Outtake.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Outtake.java
@@ -8,44 +8,149 @@
import com.qualcomm.robotcore.hardware.HardwareMap;
/**
- * POWER mode → set(x) sets power 0–1
- * RPM mode → set(x) sets RPM target and uses PIDF control
+ * Outtake manages the dual-flywheel shooter that launches game artifacts toward
+ * the scoring goal.
+ *
+ * Two operating modes are supported:
+ * The shooter consists of two motors wired in opposite polarities (one inverted,
+ * one not) so they spin in the same physical direction while facing each other.
+ *
+ * A pre-computed range-to-RPM look-up table ({@link #REGRESSION_DATA}) along
+ * with cubic polynomial ({@link #cubicRegressionRPM}) and linear-interpolation
+ * helpers let the robot calculate the exact shooter speed needed for a measured
+ * target distance.
+ *
+ * Annotated with {@link Config} so all gains and tunables are editable live
+ * in FTC Dashboard.
*/
-
@Config
public class Outtake {
+ // -----------------------------------------------------------------------
+ // Mode enum
+ // -----------------------------------------------------------------------
+
+ /**
+ * Selects open-loop (direct power) vs closed-loop (PIDF RPM) control.
+ */
public enum Mode {
+ /** Open-loop: {@link #set(double)} writes power 0–1 directly to the motors. */
POWER,
+ /** Closed-loop: {@link #set(double)} sets a target RPM tracked by PIDF each cycle. */
RPM
}
+ // -----------------------------------------------------------------------
+ // Hardware
+ // -----------------------------------------------------------------------
+
+ /** Primary flywheel motor, mapped as {@code "outtake"}. Inverted so both
+ * flywheels spin in the same physical direction. */
private final MotorEx shooter;
+
+ /** Secondary flywheel motor, mapped as {@code "outtake-2"}. Not inverted;
+ * combined with the primary motor its shaft faces the opposite way so both
+ * contact surfaces move in the same direction against the artifact. */
private final MotorEx shooter2;
- // FTCLib PID controller (P, I, D only)
+ // -----------------------------------------------------------------------
+ // Control
+ // -----------------------------------------------------------------------
+
+ /** FTCLib PID controller used in RPM mode. Only P, I, D terms are managed
+ * here; the feedforward term {@link #f} is computed separately and added
+ * to the PID output. */
private final PIDController controller;
- // Dashboard-tunable gains
+ // -----------------------------------------------------------------------
+ // Dashboard-tunable PIDF gains
+ // -----------------------------------------------------------------------
+
+ /** Proportional gain for the RPM PID loop. Increase to react faster to
+ * RPM error; too high causes oscillation. */
public static double p = 0.000567;
+
+ /** Integral gain for the RPM PID loop. Accumulates steady-state error over
+ * time. Currently 0 because feedforward alone eliminates most steady-state
+ * offset. */
public static double i = 0.0;
+
+ /** Derivative gain for the RPM PID loop. Damps rapid RPM changes.
+ * Currently 0; may be tuned if oscillation is observed. */
public static double d = 0.0;
+
+ /** Feedforward gain – approximately 1 / maxRPM, then hand-tuned.
+ * Multiplied by {@link #targetRPM} to give the approximate power needed
+ * to sustain that speed before the P term corrects any residual error. */
public static double f = 0.0002; // 1 / maxrpm and then tuned
- // Mode + state
+ // -----------------------------------------------------------------------
+ // State
+ // -----------------------------------------------------------------------
+
+ /** Current operating mode ({@link Mode#POWER} or {@link Mode#RPM}). */
public Mode mode;
+
+ /** Last motor power written to the flywheels (0–1 in POWER mode;
+ * computed PIDF output in RPM mode). */
private double motorPower = 0.0;
+
+ /** Target shooter speed in RPM (used only in {@link Mode#RPM}).
+ * Defaults to 2800 RPM, which is used when no AprilTag range is available. */
public static double targetRPM = 2800.0; // without seeing any tags
+
+ /** Most recent measured flywheel speed in RPM, updated every call to
+ * {@link #periodic()}. */
private double currentRPM = 0.0;
+ /** Encoder ticks per motor output shaft revolution for the flywheel motors.
+ * Used to convert raw encoder velocity (ticks/s) to RPM. */
private final double TPR = 28.0; // encoder ticks per rotation
+ // -----------------------------------------------------------------------
+ // Spin-up timing
+ // -----------------------------------------------------------------------
+
+ /** Minimum time in milliseconds that the RPM must stay within tolerance
+ * before {@link #inRange(double)} reports {@code true}. Prevents
+ * false "ready" signals from a single momentary on-target reading. */
public static double spinupInRangeMinTime = 200; // ms
+
+ /** Hard timeout in milliseconds for the spin-up phase. If the flywheel
+ * has not reached the target within this window {@link #inRange(double)}
+ * returns {@code true} anyway so the robot doesn't stall indefinitely. */
public static double spinupMaxTime = 2750; // ms
+
+ /** Timestamp (ms) when the RPM first entered the tolerance band, or -1
+ * if it is currently outside tolerance or a new target was just set. */
private long inRangeStartTime = -1;
+
+ /** Timestamp (ms) when the current spin-up sequence began, or -1 if no
+ * spin-up is currently in progress. */
private long spinupStartTime = -1;
+
+ /** Minimum target RPM for the intake-pass-through scenario. Any target
+ * below this value is considered "stopped" for regression purposes. */
public static double INTAKE_MIN_RPM = 3500.0;
+ // -----------------------------------------------------------------------
+ // Range-to-RPM look-up table (full resolution)
+ // -----------------------------------------------------------------------
+
+ /**
+ * Empirically measured data pairs of {range (inches), optimal shooter RPM}.
+ * Collected by shooting from each measured distance and recording the RPM
+ * that produced the best scoring rate. Used by the linear-interpolation
+ * regression helpers.
+ */
private static final double[][] REGRESSION_DATA = {
{46.4, 3400},
{48.3, 3450},
@@ -78,6 +183,16 @@ public enum Mode {
{116.6, 4700}
};
+ // -----------------------------------------------------------------------
+ // Reduced look-up table (sparse key points only)
+ // -----------------------------------------------------------------------
+
+ /**
+ * A sparse subset of {@link #REGRESSION_DATA} containing only key boundary
+ * points. Used by {@link #linearInterpolationRegressionReducedRPM(double)}
+ * to provide a faster (but less accurate) interpolation when full resolution
+ * is unnecessary.
+ */
private static final double[][] REGRESSION_DATA_REDUCED = {
{46.4, 3400},
{61.7, 3660},
@@ -86,139 +201,308 @@ public enum Mode {
{116.6, 4700}
};
+ // -----------------------------------------------------------------------
+ // Constructor
+ // -----------------------------------------------------------------------
+
+ /**
+ * Creates an Outtake instance, initialises both flywheel motors, and
+ * sets up the PID controller with the current gain values.
+ *
+ * @param hardwareMap the robot hardware map used to resolve motor config names
+ * @param mode initial operating mode ({@link Mode#POWER} or {@link Mode#RPM})
+ */
public Outtake(HardwareMap hardwareMap, Mode mode) {
+ // Retrieve and configure the primary flywheel motor.
+ // Inverted = true so that this motor spins in the correct physical direction.
shooter = new MotorEx(hardwareMap, "outtake");
shooter.setInverted(true);
+
+ // Retrieve and configure the secondary flywheel motor.
+ // Not inverted – when combined with the primary (facing opposite),
+ // both contact surfaces push the artifact in the same direction.
shooter2 = new MotorEx(hardwareMap, "outtake-2");
shooter2.setInverted(false);
+ this.mode = mode; // store the requested operating mode
- this.mode = mode;
+ // Initialise the PID controller with the dashboard-tunable gains.
controller = new PIDController(p, i, d);
}
+ // -----------------------------------------------------------------------
+ // Public API
+ // -----------------------------------------------------------------------
+
+ /**
+ * Stops both flywheel motors immediately and resets internal state.
+ * Sets {@link #motorPower} and {@link #targetRPM} to 0 so that
+ * subsequent PIDF calls do not fight to restart the flywheels.
+ */
public void stop() {
- shooter.stopMotor();
- shooter2.stopMotor();
- motorPower = 0.0;
- targetRPM = 0.0;
+ shooter.stopMotor(); // cut power to primary flywheel
+ shooter2.stopMotor(); // cut power to secondary flywheel
+ motorPower = 0.0; // clear the cached power so it isn't re-applied
+ targetRPM = 0.0; // clear the RPM target so PIDF stays off
}
- /** Unified setter */
+ /**
+ * Unified setter for both operating modes.
+ *
+ * "Ready" means either:
+ * After returning {@code true} all timing state is reset so the next
+ * call to {@link #set(double)} starts a fresh spin-up sequence.
+ *
+ * @param tolerance allowable RPM deviation from {@link #targetRPM} to be
+ * considered "in range" (e.g. 50 RPM or 100 RPM)
+ * @return {@code true} if the shooter is ready to fire; {@code false} if
+ * still spinning up
+ */
public boolean inRange(double tolerance) {
long currentTime = System.currentTimeMillis();
+
+ // Initialise the spin-up start timestamp on the first call after a new
+ // target was set (indicated by spinupStartTime == -1).
if (spinupStartTime == -1)
{
spinupStartTime = currentTime;
}
+ // Check whether the measured RPM is within the specified tolerance band.
boolean withinTolerance = Math.abs(currentRPM - targetRPM) <= tolerance;
if (withinTolerance)
{
+ // Start the sustained-in-range timer if this is the first in-range sample.
if (inRangeStartTime == -1)
inRangeStartTime = currentTime;
}
else
{
- inRangeStartTime = -1;
+ // RPM left the tolerance band; reset the sustained timer.
+ inRangeStartTime = -1;
}
+ // Has the RPM been continuously in range for the minimum required duration?
boolean inRangeLongEnough = inRangeStartTime >= 0 && (currentTime - inRangeStartTime) >= spinupInRangeMinTime;
+
+ // Has the overall spin-up exceeded the maximum allowed time (safety valve)?
boolean spunPastMaxTime = (currentTime - spinupStartTime) >= spinupMaxTime;
if (inRangeLongEnough || spunPastMaxTime)
{
+ // Reset timing state so the next spin-up begins clean.
spinupStartTime = -1;
inRangeStartTime = -1;
- return true;
+ return true; // shooter is ready (or timed out – fire anyway)
}
- return false;
+ return false; // still spinning up; caller should keep waiting
}
}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Turret.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Turret.java
index 25f5f96f4ed4..26c4a972bc23 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Turret.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Turret.java
@@ -4,25 +4,106 @@
import com.qualcomm.robotcore.hardware.DcMotor;
import com.qualcomm.robotcore.hardware.HardwareMap;
+/**
+ * Turret models a rotational (panning) platform driven by a single DC motor.
+ *
+ * The turret can be used to aim a sensor or shooter by rotating it to a
+ * desired angle. Angle calculation uses a ticks-per-degree conversion factor
+ * ({@link #TICKS_TO_DEGREES}) and a zero-position offset ({@link #ZERO_OFFSET})
+ * that compensate for the motor's encoder starting point and any mechanical
+ * offset between "encoder zero" and the physical zero of the turret.
+ *
+ * The current angle is always wrapped into the [0, 360) range to avoid
+ * ambiguity about "which revolution" the turret is on.
+ *
+ * Annotated with {@link Config} so {@link #TICKS_TO_DEGREES} and
+ * {@link #ZERO_OFFSET} can be calibrated live in FTC Dashboard.
+ */
@Config
public class Turret {
+
+ // -----------------------------------------------------------------------
+ // Dashboard-tunable calibration constants
+ // -----------------------------------------------------------------------
+
+ /** Conversion factor from raw encoder ticks to degrees of turret rotation.
+ * Set to 0 by default; must be measured and configured for the specific
+ * gear ratio and encoder used on this robot. */
public static double TICKS_TO_DEGREES = 0;
+
+ /** Angular offset (degrees) added to the converted tick value to align
+ * the encoder's zero position with the desired physical zero of the turret.
+ * A positive offset rotates the "zero" clockwise. */
public static double ZERO_OFFSET = 0;
+
+ // -----------------------------------------------------------------------
+ // Hardware
+ // -----------------------------------------------------------------------
+
+ /** The DC motor that drives the turret rotation mechanism.
+ * Mapped to hardware config name {@code "TurretMotor"}. */
private final DcMotor motor;
+ // -----------------------------------------------------------------------
+ // Constructor
+ // -----------------------------------------------------------------------
+
+ /**
+ * Creates a Turret instance, retrieves the motor from the hardware map,
+ * and configures it to BRAKE when power is zero (so the turret holds its
+ * position without drifting when no correction is applied).
+ *
+ * @param hardwareMap the robot's hardware map used to look up the motor
+ * registered under the config name {@code "TurretMotor"}
+ */
public Turret(HardwareMap hardwareMap) {
+ // Retrieve the turret motor from the hardware map.
motor = hardwareMap.get(DcMotor.class, "TurretMotor");
+
+ // BRAKE mode holds the turret in place when power = 0, preventing wind-up.
motor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
- // May be needed?
+
+ // Motor direction reversal may be needed depending on how the motor is mounted;
+ // uncomment the line below and adjust if the turret spins the wrong way.
// motor.setDirection(DcMotorSimple.Direction.REVERSE);
}
+ // -----------------------------------------------------------------------
+ // Public API
+ // -----------------------------------------------------------------------
+
+ /**
+ * Returns the current turret angle in degrees, wrapped to [0, 360).
+ *
+ * The raw encoder tick count is first multiplied by {@link #TICKS_TO_DEGREES}
+ * to convert to degrees, then {@link #ZERO_OFFSET} is added to correct for
+ * any mechanical misalignment. Finally the result is wrapped with a
+ * positive-modulo operation so that the angle is always in [0, 360).
+ *
+ * Note: Java's {@code %} operator returns a negative result for
+ * negative operands (e.g., {@code -2 % 5 == -2}), so the double-modulo
+ * pattern {@code ((x % 360) + 360) % 360} is used to guarantee a positive
+ * result.
+ *
+ * @return current turret angle in degrees, range [0, 360)
+ */
public double getCurrentAngle() {
+ // Convert encoder ticks to degrees and apply the zero-point offset.
double curAngle = motor.getCurrentPosition() * TICKS_TO_DEGREES + ZERO_OFFSET;
// In Java -2 % 5 = -2, not 3
+ // Wrap into [0, 360): add 360 first then take mod again to handle negative values.
return ((curAngle % 360) + 360) % 360;
}
+
+ /**
+ * Sets the raw power applied to the turret motor.
+ * A positive value rotates in one direction; a negative value reverses it.
+ * Callers are responsible for implementing closed-loop angle control on
+ * top of this method.
+ *
+ * @param power motor power in the range [-1.0, 1.0]
+ */
public void setPower(double power) {
- motor.setPower(power);
+ motor.setPower(power); // directly apply power to the turret motor
}
}
\ No newline at end of file
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/indexerUtil/BetterCRControl.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/indexerUtil/BetterCRControl.java
index cc2a08680f16..65cacde042f8 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/indexerUtil/BetterCRControl.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/indexerUtil/BetterCRControl.java
@@ -1,80 +1,212 @@
package org.firstinspires.ftc.teamcode.subsystems.indexerUtil;
+
import com.acmerobotics.dashboard.config.Config;
import com.qualcomm.robotcore.hardware.CRServo;
import com.qualcomm.robotcore.hardware.AnalogInput;
import com.qualcomm.robotcore.util.ElapsedTime;
+/**
+ * BetterCRControl is a PID position controller for a continuous-rotation servo
+ * equipped with an analog (potentiometer-style) encoder.
+ *
+ * Unlike a standard servo that accepts absolute position commands,
+ * a continuous-rotation (CR) servo must be driven by open-loop power signals.
+ * This class closes the position loop manually:
+ * Note: This class is an older/alternative implementation.
+ * {@link CRServoPositionControl} is currently used in production.
+ *
+ * Annotated with {@link Config} so all gains and thresholds can be tuned
+ * live from the FTC Dashboard.
+ */
@Config
public class BetterCRControl {
+
+ // -----------------------------------------------------------------------
+ // Hardware
+ // -----------------------------------------------------------------------
+
+ /** The continuous-rotation servo being controlled. */
private final CRServo crServo;
+
+ /** The analog encoder attached to the servo shaft.
+ * Provides a 0–3.2 V signal proportional to the current angle. */
private final AnalogInput encoder;
+ // -----------------------------------------------------------------------
+ // Dashboard-tunable PID gains
+ // -----------------------------------------------------------------------
+
+ /** Proportional gain. Scales the error (degrees) directly into power.
+ * Higher values give faster response but increase overshoot risk. */
public static double kp = 0.45;
+
+ /** Integral gain. Accumulates error over time to eliminate steady-state
+ * offset. Currently 0; enable only if there is noticeable steady-state
+ * positional drift. */
public static double ki = 0.0;
+
+ /** Derivative gain. Damps rapid changes in error to reduce overshoot.
+ * A value of 0.04 provides light damping. */
public static double kd = 0.04;
+ // -----------------------------------------------------------------------
+ // Dashboard-tunable thresholds
+ // -----------------------------------------------------------------------
+
+ /** Angular error (degrees) below which the servo is considered "on target".
+ * Within this band a small hold power is applied instead of running
+ * the full PID loop, preventing high-frequency chatter. */
public static double deadband = 3; // degrees
+
+ /** Minimum absolute power applied when the error exceeds the deadband.
+ * Overcomes static friction (stiction) in the servo gearbox. */
public static double minPower = 0.08;
+
+ /** Power applied while within the deadband to resist external disturbances
+ * (e.g., field elements pressing against the indexer). Applied in the
+ * direction of the last non-zero error to act as a light restoring force. */
public static double holdPower = 0.05;
+ // -----------------------------------------------------------------------
+ // Internal PID state
+ // -----------------------------------------------------------------------
+
+ /** Accumulated error × dt (integral term numerator). */
private double integral = 0.0;
+
+ /** Error from the previous loop iteration; used to compute the derivative. */
private double lastError = 0.0;
+
+ /** Smoothed / filtered voltage reading from the encoder.
+ * Uses an exponential moving average to reduce noise. */
private double filteredVoltage = 0;
+ /** Elapsed-time timer used to measure the dt between consecutive
+ * {@link #moveToAngle(double)} calls for the integral and derivative terms. */
private ElapsedTime timer = new ElapsedTime();
+ // -----------------------------------------------------------------------
+ // Constructor
+ // -----------------------------------------------------------------------
+
+ /**
+ * Creates a BetterCRControl instance and resets the delta-time timer.
+ *
+ * @param servo the continuous-rotation servo to control
+ * @param encoder the analog encoder attached to the servo output shaft
+ */
public BetterCRControl(CRServo servo, AnalogInput encoder) {
- this.crServo = servo;
- this.encoder = encoder;
- timer.reset();
+ this.crServo = servo; // store servo reference
+ this.encoder = encoder; // store encoder reference
+ timer.reset(); // start the dt timer from zero
}
+ // -----------------------------------------------------------------------
+ // Internal helpers
+ // -----------------------------------------------------------------------
+
+ /**
+ * Returns a smoothed encoder voltage using a fixed-coefficient exponential
+ * moving average (EMA): {@code new = 0.6 × old + 0.4 × raw}.
+ * This reduces high-frequency ADC noise without introducing significant
+ * latency into the position feedback.
+ *
+ * @return filtered encoder voltage in volts
+ */
private double getFilteredVoltage() {
+ // Blend the previous filtered value (60 %) with the new raw reading (40 %).
filteredVoltage = 0.6 * filteredVoltage + 0.4 * encoder.getVoltage();
return filteredVoltage;
}
+ /**
+ * Converts the analog encoder voltage to a shaft angle in degrees.
+ *
+ * The encoder produces a 0–3.2 V signal over one full 360° revolution,
+ * so {@code angle = (voltage / 3.2) × 360}.
+ *
+ * @param v encoder voltage in volts (expected range: 0–3.2)
+ * @return corresponding shaft angle in degrees (0–360)
+ */
private double voltageToAngle(double v) {
- return (v / 3.2) * 360.0;
+ return (v / 3.2) * 360.0; // linear mapping: 3.2 V → 360°, 0 V → 0°
}
+ /**
+ * Wraps an angle into the range (-180, 180] so that the shortest-path error
+ * direction is always chosen.
+ *
+ * @param angle raw angle or error in degrees
+ * @return equivalent angle in (-180, 180]
+ */
private double wrapDegrees(double angle) {
- angle %= 360;
- if (angle > 180) angle -= 360;
- if (angle < -180) angle += 360;
+ angle %= 360; // reduce to (-360, 360)
+ if (angle > 180) angle -= 360; // fold [180, 360) to [-180, 0)
+ if (angle < -180) angle += 360; // fold (-360, -180) to (0, 180]
return angle;
}
+ // -----------------------------------------------------------------------
+ // Public control method
+ // -----------------------------------------------------------------------
+
+ /**
+ * Commands the servo to move toward the given target angle, using a PID
+ * controller to compute the required power.
+ *
+ * Call this method every loop iteration while position control is active.
+ * The servo will move toward {@code target} and settle when the error falls
+ * within the {@link #deadband}.
+ *
+ * @param target desired shaft angle in degrees (wrapped internally to [-180, 180])
+ */
public void moveToAngle(double target) {
+ // Read current angle from the (filtered) encoder.
double current = voltageToAngle(getFilteredVoltage());
+
+ // Compute the shortest-path error from current to target.
double error = wrapDegrees(target - current);
- // Hold state behavior
+ // ---- Deadband / hold phase ----
if (Math.abs(error) < deadband) {
+ // Close enough: apply a small hold power in the direction of the last
+ // recorded error to resist backdrive forces.
crServo.setPower(holdPower * Math.signum(lastError));
- lastError = error;
- return;
+ lastError = error; // update for next iteration
+ return; // skip PID computation; we're within tolerance
}
- double dt = timer.seconds();
- timer.reset();
- if (dt < 0.0001) dt = 0.0001;
+ // ---- Full PID phase ----
+ double dt = timer.seconds(); // elapsed time since the last call (seconds)
+ timer.reset(); // restart the timer for the next iteration
+ if (dt < 0.0001) dt = 0.0001; // guard against near-zero dt (first call or very fast loop)
+ // Accumulate error for the integral term and clamp to prevent wind-up.
integral += error * dt;
- integral = Math.max(-2, Math.min(2, integral));
+ integral = Math.max(-2, Math.min(2, integral)); // clamp integral to [-2, 2]
+ // Derivative: rate of change of error since the previous call.
double derivative = (error - lastError) / dt;
+ // Combine PID terms.
double output = kp * error + ki * integral + kd * derivative;
- // Minimum power to break stiction
+ // Enforce a minimum power to overcome static friction (stiction).
if (Math.abs(output) < minPower)
- output = minPower * Math.signum(output);
+ output = minPower * Math.signum(output); // preserve direction but boost magnitude
+ // Clamp final output to valid servo power range [-1, 1].
output = Math.max(-1.0, Math.min(1.0, output));
- crServo.setPower(output);
+ crServo.setPower(output); // apply computed power to the servo
- lastError = error;
+ lastError = error; // save current error for next derivative calculation
}
}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/indexerUtil/CRServoPositionControl.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/indexerUtil/CRServoPositionControl.java
index 17de1780cf56..7a85b740d16c 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/indexerUtil/CRServoPositionControl.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/indexerUtil/CRServoPositionControl.java
@@ -4,74 +4,206 @@
import com.qualcomm.robotcore.hardware.CRServo;
import com.qualcomm.robotcore.hardware.AnalogInput;
+/**
+ * CRServoPositionControl implements closed-loop angular position control for a
+ * continuous-rotation (CR) servo that has an analog (potentiometer-style) absolute
+ * encoder attached to its output shaft.
+ *
+ * A CR servo does not natively accept position commands; it only responds to
+ * a −1 to +1 power signal. This class wraps the servo with:
+ * Annotated with {@link Config} so all dashboard fields are editable live
+ * in the FTC Dashboard.
+ */
@Config
public class CRServoPositionControl {
- // general constants
+ // -----------------------------------------------------------------------
+ // General hardware / encoder constants
+ // -----------------------------------------------------------------------
+
+ /** Maximum voltage output of the analog encoder (volts).
+ * Corresponds to one full 360° revolution. Calibrated to 3.26 V. */
public static double maxVoltage = 3.26;
+
+ /** Degrees in one full encoder revolution; always 360° for a rotary encoder. */
public static double degreesPerRev = 360.0;
- // gains
+ // -----------------------------------------------------------------------
+ // Active (runtime) PD gains
+ // -----------------------------------------------------------------------
+
+ /** Proportional gain: error (degrees) × kP = base output power.
+ * Higher values close the loop faster but risk oscillation. */
public static double kP = 0.004;
+
+ /** Integral gain. Currently 0; enable only if steady-state offset persists
+ * under load. */
public static double kI = 0.0;
+
+ /** Derivative gain: angular velocity (deg/s) × kD is subtracted from the
+ * output to damp overshoot. Currently 0. */
public static double kD = 0.0;
+
+ /** Static friction (stiction) compensation: minimum power magnitude applied
+ * when the controller output would otherwise be too small to overcome
+ * gearbox friction and start moving the servo. */
public static double kS = 0.07;
+ // -----------------------------------------------------------------------
+ // Power and stiffness limits
+ // -----------------------------------------------------------------------
+
+ /** Maximum absolute power that will be commanded to the servo.
+ * Clamps the controller output to prevent mechanical damage. */
public static double maxPower = 1.0;
- public static double stiffnessGain = 1.0; //1.0 is normal behavior
- // deadbands
+ /** Scales the proportional error term. 1.0 = normal behaviour.
+ * Can be increased for a stiffer position hold, e.g., when the indexer
+ * drum is under heavy load from the weight of three artifacts. */
+ public static double stiffnessGain = 1.0; // 1.0 is normal behavior
+
+ // -----------------------------------------------------------------------
+ // Deadband
+ // -----------------------------------------------------------------------
+
+ /** Angular deadband in degrees: if |error| < deadbandDeg the servo is
+ * turned off (power = 0). Prevents constant small oscillations while
+ * the indexer is "close enough" to its target. */
public static double deadbandDeg = 6.0;
+
+ /** Rotation-direction preference for {@link #moveToAngle(double)}.
+ * {@code true} = always move clockwise (positive angle increase);
+ * {@code false} = always move counter-clockwise. */
public static boolean rotateClockwise = true;
- // Preset gains
+ // -----------------------------------------------------------------------
+ // Preset gain sets – switched automatically based on load state
+ // -----------------------------------------------------------------------
+
+ // Unloaded preset (no artifacts in drum) – lighter gains suffice.
public static double unloaded_kP = 0.004;
public static double unloaded_kI = 0.0;
public static double unloaded_kD = 0.0;
public static double unloaded_kS = 0.08;
+ // Loaded preset (one or more artifacts in drum) – stronger integral and hold.
public static double loaded_kP = 0.004;
public static double loaded_kI = 0.00009;
public static double loaded_kD = 0.0;
public static double loaded_kS = 0.08;
+ // -----------------------------------------------------------------------
+ // Internal state
+ // -----------------------------------------------------------------------
+
+ /** Whether the drum currently contains at least one artifact.
+ * Used to select between loaded and unloaded gain presets. */
private boolean loaded = false;
- private boolean manualOverride = false; // when true, update() does nothing (open-loop control active)
- // hardware
+ /** When {@code true} the {@link #update()} method is bypassed and the servo
+ * is running on a raw power command from {@link #setOpenLoopPower(double)}.
+ * Used during the "full blast" indexer spin. */
+ private boolean manualOverride = false;
+
+ // -----------------------------------------------------------------------
+ // Hardware
+ // -----------------------------------------------------------------------
+
+ /** The continuous-rotation servo hardware object. */
private final CRServo servo;
+
+ /** The analog encoder reporting the servo's absolute shaft position. */
private final AnalogInput encoder;
- // state
+ // -----------------------------------------------------------------------
+ // Angle tracking state
+ // -----------------------------------------------------------------------
+
+ /** Last wrapped (0–360°) encoder angle, used to detect and accumulate
+ * full-revolution wrap-arounds for continuous angle tracking. */
private double lastWrappedDeg;
+
+ /** Continuously-accumulated shaft angle (may exceed 360° or be negative).
+ * Monotonically increasing/decreasing across multiple revolutions so the
+ * PD target can be set without ambiguity. */
private double continuousDeg;
+
+ /** The target angle (continuous units, not wrapped) that the controller is
+ * driving toward. */
private double targetDeg;
+ // -----------------------------------------------------------------------
+ // Derivative calculation state
+ // -----------------------------------------------------------------------
+
+ /** Shaft angle at the time of the last derivative calculation (degrees). */
private double lastAngleDeg = 0.0;
+
+ /** Timestamp (nanoseconds) of the last derivative calculation.
+ * Used to compute dt in seconds for the velocity calculation. */
private long lastTimeNs = 0;
+ // -----------------------------------------------------------------------
+ // Constructor
+ // -----------------------------------------------------------------------
+
+ /**
+ * Creates a CRServoPositionControl instance. Reads the initial encoder angle
+ * and uses it as the starting point for both the continuous-angle tracker and
+ * the initial target, so the servo holds its power-on position immediately.
+ *
+ * @param servo the CR servo hardware object
+ * @param encoder the analog encoder attached to the servo output shaft
+ */
public CRServoPositionControl(CRServo servo, AnalogInput encoder) {
- this.servo = servo;
+ this.servo = servo;
this.encoder = encoder;
- double initial = getWrappedAngle();
- lastWrappedDeg = initial;
- continuousDeg = initial;
- targetDeg = initial;
+ // Bootstrap the continuous-angle tracker from the current raw encoder reading.
+ double initial = getWrappedAngle(); // read actual shaft angle at startup
+ lastWrappedDeg = initial; // store as the "previous" wrapped angle (no delta yet)
+ continuousDeg = initial; // initialise the continuous tracker at the current position
+ targetDeg = initial; // hold the current position until explicitly commanded
- lastAngleDeg = continuousDeg;
- lastTimeNs = System.nanoTime();
+ lastAngleDeg = continuousDeg; // seed derivative state with current position
+ lastTimeNs = System.nanoTime(); // start the derivative timestamp
}
- //mroe balls is more gains
+ // -----------------------------------------------------------------------
+ // Load state & gain switching
+ // -----------------------------------------------------------------------
+
+ /**
+ * Notifies the controller whether the indexer drum is loaded with artifacts.
+ * Automatically switches between the loaded and unloaded PD gain presets.
+ *
+ * @param hasBalls {@code true} if one or more artifacts are currently in
+ * the drum; {@code false} when the drum is empty
+ */
+ // more balls is more gains
public void setLoaded(boolean hasBalls) {
this.loaded = hasBalls;
if (hasBalls) {
+ // Loaded: apply heavier gains to hold against artifact weight.
kP = loaded_kP;
kI = loaded_kI;
kD = loaded_kD;
kS = loaded_kS;
} else {
+ // Unloaded: revert to lighter gains for snappier, lower-current motion.
kP = unloaded_kP;
kI = unloaded_kI;
kD = unloaded_kD;
@@ -79,113 +211,254 @@ public void setLoaded(boolean hasBalls) {
}
}
+ /**
+ * Returns whether the drum is currently in the "loaded" state.
+ *
+ * @return {@code true} if loaded gains are active
+ */
public boolean isLoaded() { return loaded; }
+ // -----------------------------------------------------------------------
+ // Open-loop override
+ // -----------------------------------------------------------------------
+
+ /**
+ * Switches to open-loop mode and sets a fixed raw power on the servo.
+ * The {@link #update()} method will be a no-op until {@link #clearOpenLoop()}
+ * is called. Used for the "full blast" indexer dump phase.
+ *
+ * @param power desired servo power in [-1.0, 1.0]
+ */
public void setOpenLoopPower(double power) {
- manualOverride = true;
- servo.setPower(clamp(power, -1.0, 1.0));
+ manualOverride = true; // disable closed-loop update
+ servo.setPower(clamp(power, -1.0, 1.0)); // write clamped power directly to hardware
}
+ /**
+ * Returns to closed-loop position control mode.
+ * Must be called after {@link #setOpenLoopPower(double)} to re-enable
+ * the PD controller in subsequent {@link #update()} calls.
+ */
public void clearOpenLoop() {
- manualOverride = false;
+ manualOverride = false; // re-enable closed-loop update
}
+ // -----------------------------------------------------------------------
+ // Main control loop
+ // -----------------------------------------------------------------------
+
+ /**
+ * Runs one iteration of the position control loop. Must be called every
+ * robot loop iteration while position control is active.
+ *
+ * If open-loop override is active ({@link #manualOverride} == true),
+ * this method is a no-op and the servo continues at the last power set by
+ * {@link #setOpenLoopPower(double)}.
+ *
+ * Otherwise:
+ * The wrapped angle is converted to a continuous-space target by choosing
+ * the delta that matches the rotation direction preference and adding it to
+ * the current continuous position.
+ *
+ * @param wrappedAngleDeg target angle in degrees, in the range [0, 360)
+ */
public void moveToAngle(double wrappedAngleDeg) {
- updateContinuousAngle();
+ updateContinuousAngle(); // ensure continuousDeg is current before computing the delta
+ // Current position in the wrapped [0, 360) space.
double currentWrapped = mod(continuousDeg, 360.0);
+ // Shortest-path delta in [-180, 180].
double delta = wrappedAngleDeg - currentWrapped;
if (delta > 180) delta -= 360;
if (delta < -180) delta += 360;
- // rotation direction
- if (rotateClockwise && delta < 0) delta += 360;
- if (!rotateClockwise && delta > 0) delta -= 360;
+ // Override delta to enforce the preferred rotation direction.
+ if ( rotateClockwise && delta < 0) delta += 360; // force clockwise (positive direction)
+ if (!rotateClockwise && delta > 0) delta -= 360; // force counter-clockwise
- targetDeg = continuousDeg + delta;
+ targetDeg = continuousDeg + delta; // set the continuous-space target
}
+ /**
+ * Moves the target by a relative amount from the current target angle.
+ *
+ * @param deltaDeg number of degrees to advance (positive = forward in
+ * the current rotation direction)
+ */
public void moveBy(double deltaDeg) {
- targetDeg += deltaDeg;
+ targetDeg += deltaDeg; // simply offset the existing target
}
+ /**
+ * Resets the continuous angle tracker and target to the current raw encoder
+ * reading, and stops the servo. Call this after manually repositioning the
+ * servo or recovering from a fault.
+ */
public void reset() {
- double wrapped = getWrappedAngle();
- lastWrappedDeg = wrapped;
- continuousDeg = wrapped;
- targetDeg = wrapped;
- servo.setPower(0);
+ double wrapped = getWrappedAngle(); // read the encoder's current physical position
+ lastWrappedDeg = wrapped; // seed the wrap-tracker
+ continuousDeg = wrapped; // reset the continuous accumulator
+ targetDeg = wrapped; // hold the current position (no motion)
+ servo.setPower(0); // cut power immediately
}
- //bencoder
+ // -----------------------------------------------------------------------
+ // Internal angle tracking
+ // -----------------------------------------------------------------------
+
+ /**
+ * Updates the continuous-angle tracker by comparing the current wrapped
+ * encoder angle with the last recorded wrapped angle and accumulating any
+ * delta (including wrap-arounds) into {@link #continuousDeg}.
+ *
+ * Wrap-arounds are detected when the raw delta exceeds ±180°; in that
+ * case 360° is added or subtracted to recover the true (small) motion delta.
+ */
+ // bencoder
private void updateContinuousAngle() {
- double wrapped = getWrappedAngle();
- double delta = wrapped - lastWrappedDeg;
+ double wrapped = getWrappedAngle(); // current raw 0–360° reading
+ double delta = wrapped - lastWrappedDeg; // naïve delta (may include wrap error)
- // unwrap
- if (delta > 180) delta -= 360;
- if (delta < -180) delta += 360;
+ // Unwrap: if the sensor jumped by > 180° it crossed the 0/360 boundary.
+ if (delta > 180) delta -= 360; // e.g. 359° → 5°: delta was +354, true delta is −6
+ if (delta < -180) delta += 360; // e.g. 5° → 359°: delta was −354, true delta is +6
- continuousDeg += delta;
- lastWrappedDeg = wrapped;
+ continuousDeg += delta; // accumulate the true rotation delta
+ lastWrappedDeg = wrapped; // save for the next call
}
+ /**
+ * Reads the raw encoder voltage, clamps it to [0, maxVoltage], and converts
+ * it to a wrapped shaft angle in [0, 360°].
+ *
+ * @return current shaft angle in degrees, range [0, 360)
+ */
private double getWrappedAngle() {
- double v = clamp(encoder.getVoltage(), 0.0, maxVoltage);
- return (v / maxVoltage) * degreesPerRev;
+ double v = clamp(encoder.getVoltage(), 0.0, maxVoltage); // clamp ADC reading
+ return (v / maxVoltage) * degreesPerRev; // linear voltage → degrees
}
- /* ================= UTIL ================= */
+ // -----------------------------------------------------------------------
+ // Utility
+ // -----------------------------------------------------------------------
+
+ /** Clamps {@code v} to the range [min, max]. */
private double clamp(double v, double min, double max) { return Math.max(min, Math.min(max, v)); }
+
+ /**
+ * Positive modulo (always returns a value in [0, m)).
+ * Unlike Java's {@code %} which can return negative values, this helper
+ * ensures the result is non-negative, matching mathematical modulo behaviour.
+ */
private double mod(double v, double m) {
double r = v % m;
- return (r < 0) ? r + m : r;
+ return (r < 0) ? r + m : r; // shift negative remainder into [0, m)
}
- /* ================= DEBUG ================= */
+ // -----------------------------------------------------------------------
+ // Debug / telemetry accessors
+ // -----------------------------------------------------------------------
+
+ /**
+ * Returns the current continuous (unwrapped) shaft angle in degrees.
+ * Used for telemetry and diagnostic logging.
+ *
+ * @return continuously-tracked shaft angle (may exceed 360° or be negative)
+ */
public double getCurrentAngle() { return continuousDeg; }
+
+ /**
+ * Returns the current continuous-space target angle in degrees.
+ *
+ * @return target angle in continuous space
+ */
public double getTargetAngle() { return targetDeg; }
+
+ /**
+ * Returns the target angle converted back to an encoder voltage equivalent.
+ * Useful for telemetry to verify that the target position maps to a valid
+ * encoder reading.
+ *
+ * @return expected encoder voltage for the target angle (0–maxVoltage)
+ */
public double getTargetVoltage() {
+ // Wrap the continuous target back into [0, 360°].
double wrappedDeg = targetDeg % degreesPerRev;
- if (wrappedDeg < 0) wrappedDeg += degreesPerRev;
+ if (wrappedDeg < 0) wrappedDeg += degreesPerRev; // ensure positive result
+
+ // Convert degrees to voltage using the inverse of getWrappedAngle().
return (wrappedDeg / degreesPerRev) * maxVoltage;
}
+
+ /**
+ * Returns the raw encoder voltage from the analog input.
+ * Used in telemetry to compare against the expected target voltage.
+ *
+ * @return live encoder voltage in volts
+ */
public double getVoltage() { return encoder.getVoltage(); }
}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/indexerUtil/ColorSensorSystem.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/indexerUtil/ColorSensorSystem.java
index 9031f2a3107c..768db54ba3d2 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/indexerUtil/ColorSensorSystem.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/indexerUtil/ColorSensorSystem.java
@@ -7,151 +7,333 @@
import org.firstinspires.ftc.robotcore.external.Telemetry;
import org.firstinspires.ftc.teamcode.subsystems.Indexer;
+/**
+ * ColorSensorSystem wraps the robot's normalized colour sensor and provides
+ * methods to detect whether an artifact (game element) is present and, if so,
+ * classify its colour as GREEN, PURPLE, EMPTY, or UNKNOWN.
+ *
+ * Classification is performed in HSV (Hue, Saturation, Value) colour space
+ * rather than raw RGB, because HSV separates the chromatic content (hue) from
+ * the lighting level (value), making it more robust to ambient light variation.
+ *
+ * Detection pipeline for a single sample:
+ * Annotated with {@link Config} so all thresholds and gain values can be
+ * tuned live from the FTC Dashboard.
+ */
@Config
public class ColorSensorSystem {
- // Sensor gain to be tuned
+ // -----------------------------------------------------------------------
+ // Sensor configuration
+ // -----------------------------------------------------------------------
+
+ /** Analog gain applied to the sensor's LED/photodiode pair.
+ * Higher gain increases sensitivity in dim environments but can saturate
+ * in bright light; value should be calibrated for the competition field. */
public static float SENSOR_GAIN = 20.0f;
- // Minimum alpha (light) required to consider the slot full
+ // -----------------------------------------------------------------------
+ // Presence detection threshold
+ // -----------------------------------------------------------------------
+
+ /** Minimum normalized alpha (light intensity) required to conclude that
+ * an artifact is physically close to the sensor. Alpha is a proxy for
+ * how much light is reflected back; a value ≥ 0.8 indicates the sensor
+ * is very close to a surface (artifact present). */
public static float PRESENCE_ALPHA_THRESHOLD = 0.8f;
- // HSV classification
- // Hue is in degrees
+ // -----------------------------------------------------------------------
+ // Green hue range (degrees in HSV)
+ // -----------------------------------------------------------------------
+
+ /** Minimum hue angle (°) for the GREEN classification region. */
public static float GREEN_H_MIN = 90f;
+
+ /** Maximum hue angle (°) for the GREEN classification region. */
public static float GREEN_H_MAX = 165f;
+ // -----------------------------------------------------------------------
+ // Purple hue range (degrees in HSV)
+ // -----------------------------------------------------------------------
+
+ /** Minimum hue angle (°) for the PURPLE classification region. */
public static float PURPLE_H_MIN = 200f;
+
+ /** Maximum hue angle (°) for the PURPLE classification region. */
public static float PURPLE_H_MAX = 250f;
- // Gate out low quality color (gray/too dark)
+ // -----------------------------------------------------------------------
+ // Quality gates (reject low-quality colour reads)
+ // -----------------------------------------------------------------------
+
+ /** Minimum saturation required for a colour read to be trusted.
+ * Very low saturation = grey-ish colour that does not map reliably to
+ * any specific hue range. */
public static float MIN_SATURATION = 0.25f;
+
+ /** Minimum value (brightness) required for a colour read to be trusted.
+ * Very dark readings produce unreliable hue values. */
public static float MIN_VALUE = 0.10f;
- // hardware
+ // -----------------------------------------------------------------------
+ // Hardware
+ // -----------------------------------------------------------------------
+
+ /** The normalized colour sensor hardware object, mapped to the config
+ * name {@code "color"}. */
private final NormalizedColorSensor color;
- // constructor
+ // -----------------------------------------------------------------------
+ // Constructor
+ // -----------------------------------------------------------------------
+
+ /**
+ * Initialises the colour sensor system, retrieves the sensor from the
+ * hardware map, and sets its analog gain.
+ *
+ * @param hardwareMap the robot's hardware map, used to look up the sensor
+ * registered under the name {@code "color"}
+ */
public ColorSensorSystem(HardwareMap hardwareMap) {
- color = hardwareMap.get(NormalizedColorSensor.class, "color");
- color.setGain(SENSOR_GAIN);
+ color = hardwareMap.get(NormalizedColorSensor.class, "color"); // fetch sensor by config name
+ color.setGain(SENSOR_GAIN); // apply the configured sensor gain
}
- // api
+ // -----------------------------------------------------------------------
+ // Public API
+ // -----------------------------------------------------------------------
+
+ /**
+ * Returns {@code true} if the sensor detects that an artifact is present
+ * (i.e., {@code alpha >= PRESENCE_ALPHA_THRESHOLD}).
+ *
+ * Alpha is used as a proximity indicator because a highly reflective
+ * surface at close range returns much more light than an empty slot.
+ *
+ * @return {@code true} if an artifact is close to the sensor
+ */
public boolean hasArtifact() {
- NormalizedRGBA rgba = color.getNormalizedColors();
- return rgba.alpha >= PRESENCE_ALPHA_THRESHOLD;
+ NormalizedRGBA rgba = color.getNormalizedColors(); // single sensor read
+ return rgba.alpha >= PRESENCE_ALPHA_THRESHOLD; // presence check by reflected light level
}
+ /**
+ * Classifies the artifact colour (or EMPTY) based on a single sensor sample.
+ * Returns EMPTY if no artifact is detected (alpha below threshold);
+ * otherwise delegates to the HSV classifier.
+ *
+ * @return classified {@link Indexer.ArtifactColor}
+ */
public Indexer.ArtifactColor classify() {
- NormalizedRGBA rgba = color.getNormalizedColors();
+ NormalizedRGBA rgba = color.getNormalizedColors(); // read RGBA from hardware
if (rgba.alpha < PRESENCE_ALPHA_THRESHOLD) {
- return Indexer.ArtifactColor.EMPTY;
+ return Indexer.ArtifactColor.EMPTY; // not close enough to an artifact; slot is empty
}
- return classifyColorOnlyFromRGBA(rgba);
+ return classifyColorOnlyFromRGBA(rgba); // presence confirmed: classify the colour
}
+ /**
+ * Classifies the colour assuming an artifact is present.
+ * Ignores the alpha/presence check and only evaluates the chromatic content.
+ * Use this when the caller has already confirmed presence by other means.
+ *
+ * @return classified {@link Indexer.ArtifactColor} (UNKNOWN if colour is ambiguous)
+ */
public Indexer.ArtifactColor classifyColorOnly() {
- NormalizedRGBA rgba = color.getNormalizedColors();
- return classifyColorOnlyFromRGBA(rgba);
+ NormalizedRGBA rgba = color.getNormalizedColors(); // read RGBA from hardware
+ return classifyColorOnlyFromRGBA(rgba); // classify without checking alpha
}
+ /**
+ * Adds a detailed colour sensor readout to the telemetry display.
+ * Includes raw RGBA values, converted HSV values, detected artifact colour,
+ * and the presence flag. Used during tuning sessions.
+ *
+ * @param telemetry the FTC telemetry object to write data to
+ */
public void addTelemetry(Telemetry telemetry) {
- NormalizedRGBA rgba = color.getNormalizedColors();
- float[] hsv = rgbToHsv(rgba.red, rgba.green, rgba.blue);
+ NormalizedRGBA rgba = color.getNormalizedColors(); // read RGBA once
+ float[] hsv = rgbToHsv(rgba.red, rgba.green, rgba.blue); // convert to HSV for display
telemetry.addLine("===== COLOR SENSOR (HSV) =====");
- telemetry.addData("Detected Artifact", classify());
- telemetry.addData("Has Artifact", hasArtifact());
+ telemetry.addData("Detected Artifact", classify()); // full classification result
+ telemetry.addData("Has Artifact", hasArtifact()); // presence flag
+ // Raw RGBA components (normalized 0–1 by the SDK)
telemetry.addData("R", "%.4f", rgba.red);
telemetry.addData("G", "%.4f", rgba.green);
telemetry.addData("B", "%.4f", rgba.blue);
telemetry.addData("Alpha", "%.4f", rgba.alpha);
- telemetry.addData("H", "%.1f°", hsv[0]);
- telemetry.addData("S", "%.4f", hsv[1]);
- telemetry.addData("V", "%.4f", hsv[2]);
+ // HSV components derived from the RGB reading
+ telemetry.addData("H", "%.1f°", hsv[0]); // hue in degrees [0, 360)
+ telemetry.addData("S", "%.4f", hsv[1]); // saturation [0, 1]
+ telemetry.addData("V", "%.4f", hsv[2]); // value/brightness [0, 1]
}
+ /**
+ * Returns the raw normalized alpha (light intensity) from the sensor.
+ * Values close to 1.0 indicate a very nearby surface; values near 0
+ * indicate an empty/far slot.
+ *
+ * @return normalized alpha in the range [0, 1]
+ */
public float getAlpha() {
- return color.getNormalizedColors().alpha;
+ return color.getNormalizedColors().alpha; // single read, return alpha only
}
+ /**
+ * Returns the raw normalized RGB values as a three-element float array.
+ *
+ * @return {@code float[]{ red, green, blue }} each in [0, 1]
+ */
public float[] getRGB() {
NormalizedRGBA rgba = color.getNormalizedColors();
return new float[]{ rgba.red, rgba.green, rgba.blue };
}
- //its so peak
+ /**
+ * Returns the HSV representation of the current sensor reading.
+ *
+ * @return {@code float[]{ hue°, saturation, value }} where hue ∈ [0, 360),
+ * saturation ∈ [0, 1], value ∈ [0, 1]
+ */
+ // its so peak
public float[] getHSV() {
- NormalizedRGBA rgba = color.getNormalizedColors();
- return rgbToHsv(rgba.red, rgba.green, rgba.blue);
+ NormalizedRGBA rgba = color.getNormalizedColors(); // read RGB
+ return rgbToHsv(rgba.red, rgba.green, rgba.blue); // convert and return
}
- //helprs
-
+ // -----------------------------------------------------------------------
+ // Internal classification helpers
+ // -----------------------------------------------------------------------
+
+ /**
+ * Internal method: classifies colour from an already-read RGBA sample.
+ * Assumes presence has already been confirmed (alpha check done externally).
+ *
+ * Steps:
+ * The conversion follows the standard algorithm:
+ * Each call to {@link #record(Indexer.ArtifactColor)} increments the
+ * counter for the observed colour. After enough observations have accumulated
+ * ({@code totalHits() >= MIN_HITS_FOR_DECISION} in the Indexer), the slot's
+ * colour can be resolved with confidence via
+ * {@link #resolveWithThreshold(double, double, double, double)}.
+ *
+ * The {@link #trimToMax(int)} method scales all counters proportionally
+ * when the total exceeds a cap, keeping the computation lightweight and
+ * preventing old data from dominating indefinitely (sliding-window effect).
+ */
public class SlotObservation {
+
+ // -----------------------------------------------------------------------
+ // Hit counters (one per colour category)
+ // -----------------------------------------------------------------------
+
+ /** Number of times a GREEN artifact was detected while observing this slot. */
public int greenHits = 0;
+
+ /** Number of times a PURPLE artifact was detected while observing this slot. */
public int purpleHits = 0;
+
+ /** Number of times the slot appeared EMPTY while being observed. */
public int emptyHits = 0;
+
+ /** Number of times the colour sensor returned UNKNOWN (artifact present but
+ * ambiguous hue – often an artifact in transit or a bad sensor read). */
public int unknownHits = 0;
- public void reset() { greenHits = purpleHits = emptyHits = unknownHits = 0; }
+ // -----------------------------------------------------------------------
+ // Reset
+ // -----------------------------------------------------------------------
+
+ /**
+ * Resets all hit counters to zero.
+ * Called when a slot's stored colour is re-initialised or when the indexer
+ * moves away from the slot and fresh observations need to start clean.
+ */
+ public void reset() {
+ greenHits = purpleHits = emptyHits = unknownHits = 0; // zero every counter in one assignment chain
+ }
+ // -----------------------------------------------------------------------
+ // Recording
+ // -----------------------------------------------------------------------
+
+ /**
+ * Records a single colour observation for this slot.
+ * Increments the counter that corresponds to the supplied colour.
+ *
+ * @param c the colour classification from the colour sensor for this cycle
+ */
public void record(Indexer.ArtifactColor c) {
switch (c) {
case GREEN:
- greenHits++;
+ greenHits++; // saw a green artifact; accumulate green evidence
break;
case PURPLE:
- purpleHits++;
+ purpleHits++; // saw a purple artifact; accumulate purple evidence
break;
case EMPTY:
- emptyHits++;
+ emptyHits++; // slot appeared empty; accumulate empty evidence
break;
case UNKNOWN:
- unknownHits++;
+ unknownHits++; // ambiguous read; accumulate unknown evidence
break;
}
}
- public int totalHits() { return greenHits + purpleHits + emptyHits + unknownHits; }
+ // -----------------------------------------------------------------------
+ // Aggregation helpers
+ // -----------------------------------------------------------------------
+
+ /**
+ * Returns the total number of observations recorded so far.
+ * Used to check whether enough data has been gathered before making
+ * a colour decision.
+ *
+ * @return sum of all four hit counters
+ */
+ public int totalHits() {
+ return greenHits + purpleHits + emptyHits + unknownHits;
+ }
+ /**
+ * Scales down all counters proportionally so that the total does not
+ * exceed {@code maxTotal}. This implements a simple sliding-window
+ * effect: very old observations are gradually aged out while still
+ * influencing the overall ratio.
+ *
+ * If the current total is already within the limit this method is a no-op.
+ *
+ * @param maxTotal the maximum number of total observations to retain
+ */
public void trimToMax(int maxTotal) {
int total = totalHits();
- if (total <= maxTotal) return;
+ if (total <= maxTotal) return; // already within the cap; nothing to do
+
+ // Compute the scale factor needed to bring the total down to maxTotal.
double scale = maxTotal / (double) total;
- greenHits = (int) Math.round(greenHits * scale);
+
+ // Round each counter to the nearest integer after scaling.
+ greenHits = (int) Math.round(greenHits * scale);
purpleHits = (int) Math.round(purpleHits * scale);
- emptyHits = (int) Math.round(emptyHits * scale);
- unknownHits = (int) Math.round(unknownHits * scale);
+ emptyHits = (int) Math.round(emptyHits * scale);
+ unknownHits= (int) Math.round(unknownHits* scale);
}
+ // -----------------------------------------------------------------------
+ // Colour decision
+ // -----------------------------------------------------------------------
+
+ /**
+ * Resolves the most likely slot colour from the accumulated observations
+ * using per-colour fraction thresholds.
+ *
+ * For each colour, if {@code hits / totalHits >= threshold} the method
+ * returns that colour immediately (first match wins, so ordering matters:
+ * GREEN is checked first, then PURPLE, EMPTY, UNKNOWN). If no threshold
+ * is exceeded the method falls back to {@link Indexer.ArtifactColor#UNKNOWN}.
+ *
+ * Returns {@link Indexer.ArtifactColor#EMPTY} when no observations exist
+ * yet (to default to "slot is free").
+ *
+ * @param greenThresh fraction of total hits that must be GREEN to declare GREEN
+ * @param purpleThresh fraction of total hits that must be PURPLE to declare PURPLE
+ * @param emptyThresh fraction of total hits that must be EMPTY to declare EMPTY
+ * @param unknownThresh fraction of total hits that must be UNKNOWN to declare UNKNOWN
+ * @return the resolved {@link Indexer.ArtifactColor}
+ */
public Indexer.ArtifactColor resolveWithThreshold(double greenThresh, double purpleThresh, double emptyThresh, double unknownThresh) {
int total = totalHits();
- if (total == 0) return Indexer.ArtifactColor.EMPTY;
+ if (total == 0) return Indexer.ArtifactColor.EMPTY; // no data yet; treat as empty by default
- if (greenHits / (double) total >= greenThresh) return Indexer.ArtifactColor.GREEN;
- if (purpleHits / (double) total >= purpleThresh) return Indexer.ArtifactColor.PURPLE;
- if (emptyHits / (double) total >= emptyThresh) return Indexer.ArtifactColor.EMPTY;
- if (unknownHits / (double) total >= unknownThresh) return Indexer.ArtifactColor.UNKNOWN;
+ // Compare each colour's fraction to its threshold (first match wins).
+ if (greenHits / (double) total >= greenThresh) return Indexer.ArtifactColor.GREEN;
+ if (purpleHits / (double) total >= purpleThresh) return Indexer.ArtifactColor.PURPLE;
+ if (emptyHits / (double) total >= emptyThresh) return Indexer.ArtifactColor.EMPTY;
+ if (unknownHits/ (double) total >= unknownThresh) return Indexer.ArtifactColor.UNKNOWN;
return Indexer.ArtifactColor.UNKNOWN; // fallback if nothing crosses threshold
}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/indexerUtil/SlotState.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/indexerUtil/SlotState.java
index 848eb5f465c8..371e0a6bb874 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/indexerUtil/SlotState.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/indexerUtil/SlotState.java
@@ -2,10 +2,61 @@
import org.firstinspires.ftc.teamcode.subsystems.Indexer;
+/**
+ * SlotState bundles all mutable per-slot data for one indexer position.
+ *
+ * The indexer drum has three equally-spaced slots (0, 1, 2). Each slot
+ * independently tracks:
+ * An instance of this class is held in the {@code slots[]} array inside
+ * {@link Indexer}, one entry per physical slot.
+ */
public class SlotState {
+
+ // -----------------------------------------------------------------------
+ // Fields
+ // -----------------------------------------------------------------------
+
+ /** The resolved (or manually assigned) colour currently stored in this slot.
+ * Defaults to UNKNOWN so the indexer knows it has not yet been classified. */
public Indexer.ArtifactColor color = Indexer.ArtifactColor.UNKNOWN;
+
+ /** Accumulated sensor hit counts for this slot.
+ * Used to probabilistically resolve the slot's colour from multiple
+ * sequential sensor reads rather than from a single noisy sample. */
public SlotObservation obs = new SlotObservation();
+
+ /** {@code true} when the slot was EMPTY the last time the colour was
+ * evaluated. Transitions from {@code true} → {@code false} indicate that
+ * a new artifact has entered this slot, triggering fill-cycle logic. */
public boolean wasEmpty = true;
- public int fillingHits = 0; // counts sensorNonEmpty hits during a fill cycle
- public boolean fillCycleActive = false; // latched after stored EMPTY + sensor nonempty
+
+ /** Running count of consecutive sensor-detected "non-empty" readings during
+ * the current fill cycle. Once this reaches the required threshold the
+ * auto-advance logic accepts that the artifact is fully seated and can
+ * move the indexer to the next slot. */
+ public int fillingHits = 0;
+
+ /** {@code true} once the slot has been confirmed to be transitioning from
+ * EMPTY to a filled state (artifact detected after stored EMPTY).
+ * Reset to {@code false} when the fill cycle completes or the slot is
+ * re-initialised. */
+ public boolean fillCycleActive = false;
}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/limelight/Aimer.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/limelight/Aimer.java
index 36bf31fb50c9..b2dae95f5ec3 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/limelight/Aimer.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/limelight/Aimer.java
@@ -7,148 +7,361 @@
import org.firstinspires.ftc.teamcode.auto.roadrunner.miscRR.MecanumDrive;
+/**
+ * Aimer computes the heading-correction power needed to point the robot at
+ * a scoring goal using the Road Runner localizer instead of live camera data.
+ *
+ * Field coordinate convention used by this class:
+ * Workflow:
+ * A PIDF controller is used for the heading loop; all gains and the target
+ * pose can be tuned live in FTC Dashboard because the class is annotated with
+ * {@link Config}.
+ */
@Config
public class Aimer {
- //bottom left of field is (0,0) (red human player), top right (near red goal) is (144,144)
- //bottom right of field is (144,0) (blue human player), top left (near blue goal) is (0,144)
- //the bot is relocalized at the respective human player zone
+ // -----------------------------------------------------------------------
+ // PIDF gains and filter
+ // -----------------------------------------------------------------------
+
+ /** Proportional gain for the heading PID loop.
+ * Higher values produce faster corrections but risk oscillation. */
public static double kP = 0.06;
+
+ /** Integral gain for the heading PID loop. Accumulates bearing error over
+ * time to eliminate steady-state offset. Currently 0. */
public static double kI = 0.0;
+
+ /** Derivative gain for the heading PID loop. Damps rapid bearing changes.
+ * Currently 0; a low-pass filter on the derivative is also applied. */
public static double kD = 0.0;
+
+ /** Feedforward gain: a small constant power applied in the direction of the
+ * bearing error to overcome static friction in the drive motors. */
public static double kF = 0.0;
- public static double filter = 0.867; // smoothing factor (1 = no filtering, 0 = very heavy smoothing)
+
+ /** Derivative smoothing coefficient (0–1).
+ * 1.0 = no smoothing (raw derivative); 0.0 = maximum smoothing (holds last value).
+ * The exponential moving average is: {@code d = (1 - filter)*prevD + filter*rawD}. */
+ public static double filter = 0.867;
+
+ /** Maximum absolute integral value (anti-windup clamp).
+ * Prevents the integral from accumulating large values when the robot is far off-aim. */
public static double maxIntegral = 1.0;
- public static double deadband = 1;
+
+ /** Bearing deadband (degrees). When |error| < deadband no correction is applied
+ * and the integral is reset to prevent chattering when the robot is aimed well. */
+ public static double deadband = 1; // degrees
+
+ // -----------------------------------------------------------------------
+ // PID internal state
+ // -----------------------------------------------------------------------
+
+ /** Accumulated bearing error integral (degrees × seconds). */
private double integral = 0;
+
+ /** Smoothed derivative of the bearing error from the last call. */
private double lastDerivative = 0.0;
+
+ /** Bearing error on the previous iteration (degrees), used to compute the derivative. */
private double lastError = 0;
+
+ /** System time (ms) of the last {@link #calculateTurnPowerFromBearing(double)} call.
+ * Used to compute dt for the I and D terms. */
private long lastTimestamp = 0;
+
+ // -----------------------------------------------------------------------
+ // Dependencies
+ // -----------------------------------------------------------------------
+
+ /** Road Runner mecanum drive; provides the localizer pose estimate. */
private final MecanumDrive drive;
- //private final InertiaAutoAim inertiaAutoAim;
+
+ // -----------------------------------------------------------------------
+ // Target configuration
+ // -----------------------------------------------------------------------
+
+ /** Field-relative pose of the target AprilTag (scoring goal).
+ * Updated by {@link #setRedTarget()} or {@link #setBlueTarget()}.
+ * Default heading of 90° means the tag faces the field's +Y direction. */
public static Pose2d tagPose = new Pose2d(0, 132, Math.toRadians(90));
+
+ /** Height of the robot's camera/sensor above the floor (inches). */
public static double cameraHeight = 11.815; // inches
+
+ /** Height of the goal AprilTag above the floor (inches).
+ * Used to compute the 3-D range from the 2-D horizontal distance. */
public static double goalAprilTagHeight = 29.5; // inches
- public static double goalBack = 4; //how far from the back of the field the aiming point is
- public static double goalOut = 15; //how far from the side border of the field (where drivers stand) the aiming point is
+ /** Horizontal distance (inches) from the back field wall to the aiming point.
+ * Adjusts where on the field the robot tries to aim "at" the goal from. */
+ public static double goalBack = 4; // inches from back wall of the field
+ /** Lateral distance (inches) from the side border (driver station wall) to
+ * the aiming point. Positions the goal target away from the wall. */
+ public static double goalOut = 15; // inches from driver side wall
+
+ // -----------------------------------------------------------------------
+ // Goal selection
+ // -----------------------------------------------------------------------
+
+ /**
+ * Enum representing the two alliance goals on the field.
+ */
public enum Goal {
+ /** Red alliance scoring goal (top-right area of the field). */
RED,
+ /** Blue alliance scoring goal (top-left area of the field). */
BLUE
}
+ /** The currently selected alliance goal. Written by {@link #setRedTarget()}
+ * and {@link #setBlueTarget()}; read by {@link #relocalize()}. */
public static Goal selectedGoal = Goal.RED;
+ // -----------------------------------------------------------------------
+ // Constructor
+ // -----------------------------------------------------------------------
+
+ /**
+ * Creates an Aimer and immediately configures it for the red goal.
+ * Also re-zeros the localizer pose to the robot's assumed starting position
+ * at the human player zone.
+ *
+ * @param mecanumDrive the Road Runner drive used to read the localizer pose
+ */
public Aimer(MecanumDrive mecanumDrive) {
this.drive = mecanumDrive;
- //defaults to red goal
+ // Default to the red goal on construction; can be changed before a match.
setRedTarget();
+ // Re-zero the localizer at the human player starting position.
relocalize();
- //inertiaAutoAim = new InertiaAutoAim();
}
+ // -----------------------------------------------------------------------
+ // Goal selection helpers
+ // -----------------------------------------------------------------------
+
+ /**
+ * Configures the aimer to target the red alliance goal (top-right
+ * corner, approximately (144−goalOut, 144−goalBack) on the field).
+ * Also switches to the red AprilTag pipeline implicitly via the caller.
+ */
public void setRedTarget(){
selectedGoal = Goal.RED;
- tagPose = new Pose2d(144-goalOut,144-goalBack, Math.toRadians(90));
+ // Compute the red goal's field position from the back/side offsets.
+ tagPose = new Pose2d(144-goalOut, 144-goalBack, Math.toRadians(90));
}
+ /**
+ * Configures the aimer to target the blue alliance goal (top-left
+ * corner, approximately (goalOut, 144−goalBack) on the field).
+ */
public void setBlueTarget(){
selectedGoal = Goal.BLUE;
- tagPose = new Pose2d(goalOut,144-goalBack, Math.toRadians(90));
+ // Compute the blue goal's field position from the back/side offsets.
+ tagPose = new Pose2d(goalOut, 144-goalBack, Math.toRadians(90));
}
+ /**
+ * Returns the currently selected alliance goal.
+ *
+ * @return {@link Goal#RED} or {@link Goal#BLUE}
+ */
public Goal getGoal() {
return selectedGoal;
}
+ // -----------------------------------------------------------------------
+ // Localization
+ // -----------------------------------------------------------------------
+
+ /**
+ * Re-zeros the Road Runner localizer at the robot's known starting position
+ * in the human player zone.
+ *
+ * The robot is assumed to be flush against the human player wall,
+ * oriented at 90° (facing the field). A {@code botThick} offset accounts
+ * for the robot's physical half-width so that the localizer's (x,y) refers
+ * to the robot center, not the field boundary.
+ */
public void relocalize(){
+ // Half-width of the robot in inches; used to place the center away from the wall.
int botThick = 9;
- //RED Human player zone
if(selectedGoal == Goal.RED){
+ // Red human player zone: bottom-left corner, heading 90° (facing +Y).
drive.localizer.setPose(new Pose2d(0+botThick, 0+botThick, Math.toRadians(90)));
} else if (selectedGoal == Goal.BLUE){
+ // Blue human player zone: bottom-right corner, heading 90° (facing +Y).
drive.localizer.setPose(new Pose2d(144-botThick, 0+botThick, Math.toRadians(90)));
}
}
+ // -----------------------------------------------------------------------
+ // Main aiming computation
+ // -----------------------------------------------------------------------
+
+ /**
+ * Computes the turn correction power, range, and bearing to the goal using
+ * the current localizer pose estimate.
+ *
+ * Steps:
+ * Used to ensure heading errors are always expressed as the shortest
+ * angular path (avoiding ±360 jumps).
+ *
+ * @param angle raw angle in degrees
+ * @return equivalent angle in (-180, 180]
+ */
private double angleWrapDegrees(double angle) {
+ // Shift by 180, take mod 360, then shift back so result is in (-180, 180].
return (angle + 180) % 360 - 180;
}
+ // -----------------------------------------------------------------------
+ // PID controller
+ // -----------------------------------------------------------------------
+
+ /**
+ * Runs the PIDF heading controller and returns a motor power in [-1, 1].
+ *
+ * If the bearing is {@link Double#NaN} (e.g., tag not visible) all PID
+ * state is reset and 0 is returned. If the bearing is within the
+ * {@link #deadband} the integral is reset and 0 is returned.
+ *
+ * @param bearing current heading error in degrees;
+ * {@link Double#NaN} if aiming data is unavailable
+ * @return turn correction power in [-1.0, 1.0]; positive turns right
+ */
public double calculateTurnPowerFromBearing(double bearing) {
- // If apriltag lost - reset PID state and return no correction
+ // If bearing is NaN (e.g., tag lost), reset all PID state and return no correction.
if (Double.isNaN(bearing)) {
- integral = 0;
- lastError = 0;
- lastDerivative = 0;
- lastTimestamp = 0;
- return 0.0;
+ integral = 0; // clear integral accumulation
+ lastError = 0; // clear previous error
+ lastDerivative = 0; // clear smoothed derivative
+ lastTimestamp = 0; // force dt to be treated as first call on resume
+ return 0.0; // no correction while tag is not visible
}
+ // Wrap bearing to (-180, 180] to ensure shortest-path error.
double error = angleWrapDegrees(bearing);
+
if (Math.abs(error) < deadband) {
+ // Within the deadband: on-target enough; reset integral and return 0.
integral = 0;
- lastError = error;
- lastDerivative = 0;
- return 0;
+ lastError = error; // keep last error so derivative is smooth on next entry
+ lastDerivative = 0; // reset derivative since we're stationary in the band
+ return 0; // no correction needed
}
- // Time diff in seconds
+ // ---- Compute dt ----
long currentTime = System.currentTimeMillis();
double deltaTime;
-
if (lastTimestamp == 0) {
- deltaTime = 0.01; // assume 10 ms on first loop
+ deltaTime = 0.01; // assume 10 ms on the very first call (no previous timestamp)
} else {
- deltaTime = (currentTime - lastTimestamp) / 1000.0;
+ deltaTime = (currentTime - lastTimestamp) / 1000.0; // convert ms to seconds
}
- lastTimestamp = currentTime;
+ lastTimestamp = currentTime; // store for next iteration
- // Don't consider > 100 ms or < 5ms
- deltaTime = Math.max(Math.min(deltaTime, 0.1), .005);
+ // Clamp dt to [5 ms, 100 ms] to avoid numerical issues from very short or
+ // very long gaps between control loop iterations.
+ deltaTime = Math.max(Math.min(deltaTime, 0.1), 0.005);
- // Integral accumulation
- integral += error * deltaTime;
+ // ---- Integral ----
+ integral += error * deltaTime; // accumulate error × time
+ // Anti-windup: clamp so the integral can't grow unboundedly.
integral = Math.max(-maxIntegral, Math.min(maxIntegral, integral));
- // Derivative with smoothing
- double rawDerivative = (error - lastError) / deltaTime;
+ // ---- Derivative with low-pass filter ----
+ double rawDerivative = (error - lastError) / deltaTime; // rate of change of error
+ // Exponential moving average: blends previous smoothed derivative with raw one.
double derivative = (1 - filter) * lastDerivative + filter * rawDerivative;
- lastDerivative = derivative;
- lastError = error;
+ lastDerivative = derivative; // save for next iteration
+ lastError = error; // save current error for next iteration
- // Gets proper direction
+ // ---- Feedforward ----
+ // A small constant in the direction of the error to overcome stiction.
double feedforward = Math.signum(error) * kF;
- // output
+ // ---- Combined PIDF output ----
double power = kP * error + kI * integral + kD * derivative + feedforward;
+ // Clamp output to valid motor power range.
return Math.max(-1, Math.min(1, power));
}
}
\ No newline at end of file
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/limelight/AprilTag.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/limelight/AprilTag.java
index 3b0124cfc29c..9bcc849396e4 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/limelight/AprilTag.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/limelight/AprilTag.java
@@ -8,97 +8,312 @@
import java.util.List;
+/**
+ * AprilTag wraps the Limelight 3A vision co-processor to detect and measure
+ * fiducial AprilTag markers on the FTC field.
+ *
+ * Two types of AprilTag scans are supported:
+ * Range is computed from the tag's elevation angle using a geometric formula
+ * based on the camera height and the known tag height above the floor.
+ *
+ * Alliance-specific notes:
+ * The obelisk tag ID encodes the scoring motif pattern for the current
+ * round (IDs 21, 22, or 23). Reads the first fiducial in the result list
+ * and returns immediately; only one obelisk tag is expected to be visible.
+ */
public void scanObeliskTag() {
- setPipeline(2);
+ setPipeline(2); // switch to pipeline 2 which is configured for obelisk tags
+ // Get all fiducial detections from the latest frame.
List Formula:
+ * If no tag is found in the current frame, {@link #bearing},
+ * {@link #elevation}, and {@link #range} are set to {@link Double#NaN}
+ * so callers can detect a "tag lost" condition. {@link #id} is reset to
+ * {@code -1}.
+ *
+ * If multiple tags are visible the last one in the list "wins" (all are
+ * iterated); in practice only one goal tag should be in frame.
+ */
public void scanGoalTag() {
- id = -1;
- /* So that if you scan and theres no tag range stays, (bearing should be reset in the loops)*/
- bearing = Double.NaN;
- elevation = Double.NaN;
- range = Double.NaN;
+ id = -1; // reset to no-detection state
+ /* So that if you scan and there's no tag, range stays NaN (bearing should be reset in loops) */
+ bearing = Double.NaN; // no valid bearing until a tag is found
+ elevation = Double.NaN; // no valid elevation until a tag is found
+ range = Double.NaN; // no valid range until a tag is found
- // If camera is facing to the right of the center of the cam (if it needs to move to the left) the bearing is positive.
+ // Get all fiducial detections from the latest frame.
+ // If camera is facing to the right of center the bearing is positive.
List Pipeline assignments:
+ * Problem: the robot is driving while it fires. A ball fired straight ahead
+ * will be displaced sideways by the robot's lateral velocity during the ball's
+ * flight time. To compensate, the aimer should lead the target by the amount
+ * the ball will drift.
+ *
+ * Algorithm (simplified):
+ * Note: This class is currently not used in the main codebase
+ * (the reference in {@link Aimer} is commented out) but is available for
+ * future inertia-compensation work.
+ */
public class InertiaAutoAim {
- //Given: Magnitude and direction of Velocity, and range and elevation
- //Get: needed yaw change
- private final static double[] ROBOT_POS = {0,0,0};
- // double[] --> {x,y,z} (y is vertical)
- public InertiaAutoAim() {
+ // -----------------------------------------------------------------------
+ // Constants
+ // -----------------------------------------------------------------------
+
+ /**
+ * Robot position in the local reference frame – always (0, 0, 0) because
+ * all calculations are robot-relative. The layout is {x, y, z} where y
+ * is the vertical axis.
+ */
+ private final static double[] ROBOT_POS = {0, 0, 0};
+ // double[] --> {x, y, z} (y is vertical)
+ // -----------------------------------------------------------------------
+ // Constructor
+ // -----------------------------------------------------------------------
+
+ /**
+ * Creates an InertiaAutoAim instance. No parameters are needed because
+ * all state is passed per-call via {@link #getYawDegrees}.
+ */
+ public InertiaAutoAim() {
+ // No member state to initialise; all inputs are provided per-call.
}
+ // -----------------------------------------------------------------------
+ // Correction computation
+ // -----------------------------------------------------------------------
+
+ /**
+ * Computes the yaw angle correction (in degrees) needed to compensate for
+ * the robot's translational velocity while firing.
+ *
+ * Coordinate convention: the robot is at (0, 0, 0). X points to the
+ * robot's right, Y points upward, and Z points forward (in the direction
+ * the robot is heading).
+ *
+ * @param robotVel robot velocity vector as {@code {vx, vy, vz}} in
+ * inches per second (robot frame; y is vertical)
+ * @param ballSpeed muzzle speed of the ball in inches per second
+ * @param robotYawRad current robot heading in radians (field frame);
+ * used to rotate the goal position into robot frame
+ * @param goalDistance straight-line distance from robot to goal in inches
+ * @param goalElevation height of the goal above the robot in inches (vertical component)
+ * @return yaw correction in degrees; positive = need to aim further left
+ * (counter-clockwise) to compensate for rightward robot motion
+ */
public double getYawDegrees(double[] robotVel, double ballSpeed, double robotYawRad, double goalDistance, double goalElevation) {
+ // Compute the horizontal (ground-plane) distance from robot to goal,
+ // stripping the vertical elevation component out of the total slant range.
double baseDistance = Math.sqrt(goalDistance * goalDistance - goalElevation * goalElevation);
- double[] goalPos = {baseDistance * Math.sin(robotYawRad),goalElevation,baseDistance * Math.cos(robotYawRad)};
- // goal pos is relative to the robot pos
- double time = (goalDistance / ballSpeed);
- // get the needed yaw change
- double dx = goalPos[0] - (ROBOT_POS[0] + robotVel[0] * time);
- double dz = goalPos[2] - (ROBOT_POS[2] + robotVel[2] * time);
+ // Represent the goal's position in the robot's local 3-D frame.
+ // X = right of robot = baseDistance * sin(heading) (lateral field component)
+ // Y = elevation = goalElevation (vertical)
+ // Z = forward of robot = baseDistance * cos(heading) (forward field component)
+ double[] goalPos = {
+ baseDistance * Math.sin(robotYawRad), // horizontal offset to the side
+ goalElevation, // vertical offset to the goal
+ baseDistance * Math.cos(robotYawRad) // horizontal offset forward
+ };
+ // goalPos is relative to the robot position (which is ROBOT_POS = {0,0,0})
+
+ // Estimate ball flight time: assume constant speed and no air resistance.
+ double time = (goalDistance / ballSpeed); // seconds for the ball to reach the goal
+
+ // Compute where the aim point must be to hit the goal given that the robot
+ // (launch origin) will have drifted by robotVel * time during the ball's flight.
+ // dx and dz are the lead-compensated horizontal aim deltas.
+ double dx = goalPos[0] - (ROBOT_POS[0] + robotVel[0] * time); // X: right of robot
+ double dz = goalPos[2] - (ROBOT_POS[2] + robotVel[2] * time); // Z: forward of robot
+
+ // atan2 gives the compensated heading needed to reach the corrected aim point.
+ // Subtracting robotYawRad converts it from field heading to a yaw correction delta.
double yaw = Math.atan2(dz, dx) - robotYawRad;
- return Math.toDegrees(yaw);
+ return Math.toDegrees(yaw); // convert radians to degrees for the caller
}
}
\ No newline at end of file
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/outtakeUtil/RpmController.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/outtakeUtil/RpmController.java
index dc028deba156..4cce43777cfa 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/outtakeUtil/RpmController.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/outtakeUtil/RpmController.java
@@ -4,38 +4,107 @@
import com.acmerobotics.dashboard.config.Config;
+/**
+ * RpmController is a feedforward + proportional (FP) controller used to drive
+ * the shooter flywheel motors to a target RPM.
+ *
+ * The control law is:
+ * The feedforward component ({@code kS} + {@code kV}) pre-loads the motor
+ * with approximately the right power so that the proportional term ({@code Kp})
+ * only needs to correct small residual errors. This reduces overshoot relative
+ * to pure P-only control while still reaching the setpoint quickly.
+ *
+ * Annotated with {@link Config} so gains can be tuned live in FTC Dashboard
+ * (note: the fields are {@code final} and will not actually be editable at
+ * runtime unless changed to {@code public static}).
+ */
@Config
public class RpmController {
- // Gains
+ // -----------------------------------------------------------------------
+ // Gains (set once at construction via the constructor)
+ // -----------------------------------------------------------------------
+
+ /**
+ * Proportional gain: scales the RPM error (setpoint − measured) into a
+ * motor power correction. Larger values close the error faster but may
+ * cause overshoot or oscillation if too high.
+ */
public final double Kp;
- public final double kS; //static
- public final double kV; //velocity proportional
+ /**
+ * Static friction feedforward gain: a small constant power added in the
+ * direction of the setpoint to overcome motor and gearbox stiction before
+ * the proportional term fully engages. Ensures the flywheel starts moving
+ * even at very low RPM targets.
+ */
+ public final double kS; // static (stiction) feedforward
+
+ /**
+ * Velocity feedforward gain: scales the setpoint RPM directly into a base
+ * motor power estimate. The product {@code kV * setpointRPM} approximates
+ * the power needed to sustain the target speed, leaving only small errors
+ * for the P term to handle.
+ */
+ public final double kV; // velocity proportional feedforward
+
+ // -----------------------------------------------------------------------
+ // Constructor
+ // -----------------------------------------------------------------------
+
+ /**
+ * Creates an RpmController with the given FP gains.
+ *
+ * @param Kp proportional gain (RPM error → motor power)
+ * @param kS static-friction feedforward gain
+ * @param kV velocity feedforward gain (RPM → motor power)
+ */
public RpmController(double Kp, double kS, double kV) {
- this.Kp = Kp;
- this.kS = kS;
- this.kV = kV;
+ this.Kp = Kp; // store proportional gain
+ this.kS = kS; // store static feedforward gain
+ this.kV = kV; // store velocity feedforward gain
}
+ // -----------------------------------------------------------------------
+ // Control update
+ // -----------------------------------------------------------------------
+
/**
- * Update FF + P controller and return motor power
+ * Computes the motor power needed to drive the flywheel from
+ * {@code measuredRPM} toward {@code setpointRPM} using feedforward + P
+ * control.
+ *
+ * Algorithm:
+ * Road Runner {@link Action}s are designed for use in autonomous sequences,
+ * but they can also be used during teleop to perform multi-step background
+ * operations (e.g., raise the actuator, wait for RPM, fire, lower actuator)
+ * without blocking the main teleop loop.
+ *
+ * Usage:
+ * Only one action runs at a time. Starting a new action via
+ * {@link #start(Action)} while one is running will replace the current action.
+ */
public class ActionHost {
+
+ // -----------------------------------------------------------------------
+ // State
+ // -----------------------------------------------------------------------
+
+ /**
+ * The currently executing {@link Action}, or {@code null} if no action is
+ * active. Made {@code protected} so subclasses can inspect it if needed.
+ */
protected Action current;
+ // -----------------------------------------------------------------------
+ // Control methods
+ // -----------------------------------------------------------------------
+
+ /**
+ * Starts executing the given {@link Action}.
+ * If an action is already running it is discarded and replaced by the new
+ * one immediately.
+ *
+ * @param action the Road Runner Action to execute; must not be {@code null}
+ */
public void start(Action action) {
- current = action;
+ current = action; // replace any in-progress action with the new one
}
+ /**
+ * Cancels the currently running action.
+ * After this call {@link #isRunning()} returns {@code false} and
+ * {@link #update()} becomes a no-op until {@link #start(Action)} is called
+ * again.
+ */
public void abort() {
- current = null;
+ current = null; // discard the in-progress action; any hardware it was driving keeps its last power
}
+ /**
+ * Returns whether an action is currently executing.
+ * Use this in the teleop loop to guard button presses that would conflict
+ * with a running action sequence.
+ *
+ * @return {@code true} if an action is in progress; {@code false} otherwise
+ */
public boolean isRunning() {
- return current != null;
+ return current != null; // non-null current ↔ action running
}
+ // -----------------------------------------------------------------------
+ // Loop update
+ // -----------------------------------------------------------------------
+
+ /**
+ * Advances the current action by one loop iteration.
+ * Must be called every robot loop tick (typically inside
+ * {@code BotPeriodics.handlePeriodics()}).
+ *
+ * Each call:
+ * It extends {@link BotPeriodics} (which owns the hardware and runs all
+ * per-loop subsystem updates) and adds a finite-state machine (FSM) for
+ * managing the five phases of a teleop match:
+ *
+ * Annotated with {@link Config} so tuning constants are visible in Dashboard.
+ */
@Config
public class Bot extends BotPeriodics {
- // haptics & lights
+
+ // -----------------------------------------------------------------------
+ // Haptics / lights state
+ // -----------------------------------------------------------------------
+
+ /** Latched to {@code true} once the "drum full" rumble has been sent to
+ * prevent repeated rumbles every loop tick after the drum fills. */
private boolean rumbledAlready = false;
+
+ // -----------------------------------------------------------------------
+ // Finite-state machine
+ // -----------------------------------------------------------------------
+
+ /**
+ * The five high-level states of the teleop FSM.
+ */
public enum FSM {
+ /** Pre-match: drivers select which scoring motif (color pattern) to use. */
MotifSelection,
+ /** Collecting artifacts via the intake/indexer assembly. */
Intake,
+ /** Full-drum timed dump: all three artifacts fired in one blast. */
QuickOuttake,
+ /** Sorted single-ball outtake: fire one artifact at a time by color. */
SortOuttake,
+ /** End-game phase (climbing, parking, or special actions). */
Endgame
}
+ /** Current FSM state. Drives the {@code switch} in {@link #teleopTick()}. */
public FSM state;
- public static double NON_INDEX_SPIN_TIME = 3; //seconds of full-power indexer blast
+ // -----------------------------------------------------------------------
+ // Tuning constants (Dashboard-editable)
+ // -----------------------------------------------------------------------
+
+ /** Duration (seconds) of the full-power "non-indexed dump" spin in
+ * {@link #actionNonIndexedDump()}. Long enough to clear all three slots
+ * but short enough not to waste time after the drum is empty. */
+ public static double NON_INDEX_SPIN_TIME = 3; // seconds
+
+ /** Raw power applied to the indexer drum during the non-indexed dump blast.
+ * Low enough to feed balls smoothly without jamming. */
public static double FULL_BLAST_POWER = 0.25;
- public static double QUICKSPIN_OUTTAKE_RPM_SCALE = 0.93; // 1.12
- public Indexer.ArtifactColor[] motif;
+ /** Scale factor applied to the base shooter RPM during the quick-spin outtake.
+ * Values < 1.0 reduce RPM slightly to trade accuracy for faster spin-up. */
+ public static double QUICKSPIN_OUTTAKE_RPM_SCALE = 0.93; // proportion of full target RPM
- private Indexer.ArtifactColor[] PPG = new Indexer.ArtifactColor[]{Indexer.ArtifactColor.PURPLE, Indexer.ArtifactColor.PURPLE, Indexer.ArtifactColor.GREEN};
- private Indexer.ArtifactColor[] PGP = new Indexer.ArtifactColor[]{Indexer.ArtifactColor.PURPLE, Indexer.ArtifactColor.GREEN, Indexer.ArtifactColor.PURPLE};
- private Indexer.ArtifactColor[] GPP = new Indexer.ArtifactColor[]{Indexer.ArtifactColor.GREEN, Indexer.ArtifactColor.PURPLE, Indexer.ArtifactColor.PURPLE};
+ // -----------------------------------------------------------------------
+ // Motif definitions
+ // -----------------------------------------------------------------------
+ /** The motif (desired color order of artifacts) currently selected by the driver.
+ * Determines which slot the indexer presents first during outtake. */
+ public Indexer.ArtifactColor[] motif;
+
+ /** Purple–Purple–Green motif: two purple balls then one green. */
+ private Indexer.ArtifactColor[] PPG = new Indexer.ArtifactColor[]{
+ Indexer.ArtifactColor.PURPLE,
+ Indexer.ArtifactColor.PURPLE,
+ Indexer.ArtifactColor.GREEN
+ };
+
+ /** Purple–Green–Purple motif: purple, green, purple interleaved. */
+ private Indexer.ArtifactColor[] PGP = new Indexer.ArtifactColor[]{
+ Indexer.ArtifactColor.PURPLE,
+ Indexer.ArtifactColor.GREEN,
+ Indexer.ArtifactColor.PURPLE
+ };
+
+ /** Green–Purple–Purple motif: green first, then two purple. */
+ private Indexer.ArtifactColor[] GPP = new Indexer.ArtifactColor[]{
+ Indexer.ArtifactColor.GREEN,
+ Indexer.ArtifactColor.PURPLE,
+ Indexer.ArtifactColor.PURPLE
+ };
+
+ // -----------------------------------------------------------------------
+ // Constructor
+ // -----------------------------------------------------------------------
+
+ /**
+ * Constructs a Bot, passing all hardware references to the parent class
+ * and initialising the FSM in the {@link FSM#MotifSelection} state.
+ *
+ * @param hardwareMap the robot's hardware map
+ * @param tele FTC telemetry
+ * @param mecanumDrive pre-built Road Runner drive
+ * @param gamepad1 Driver 1 raw gamepad
+ * @param gamepad2 Driver 2 (operator) raw gamepad
+ * @param twoMovement {@code true} to allow gamepad 2 to also control driving
+ */
public Bot(HardwareMap hardwareMap, Telemetry tele, MecanumDrive mecanumDrive, Gamepad gamepad1, Gamepad gamepad2, boolean twoMovement) {
super(hardwareMap, tele, mecanumDrive, gamepad1, gamepad2, twoMovement);
- state = FSM.MotifSelection;
+ state = FSM.MotifSelection; // always start in motif-selection so drivers confirm the motif
}
+ // -----------------------------------------------------------------------
+ // OpMode lifecycle
+ // -----------------------------------------------------------------------
+
+ /**
+ * Called once during the {@code init()} phase, before the start button is
+ * pressed. Resets the indexer color table, enables auto intake advancement,
+ * and stops the shooter.
+ */
public void teleopInit() {
- indexer.initializeColors(Indexer.ArtifactColor.EMPTY);
- indexer.setIntaking(true);
- state = FSM.MotifSelection;
- outtake.stop();
+ indexer.initializeColors(Indexer.ArtifactColor.EMPTY); // mark every slot as empty at start
+ indexer.setIntaking(true); // enable the indexer's auto-advance-on-intake logic
+ state = FSM.MotifSelection; // reset FSM to motif-selection in case of re-init
+ outtake.stop(); // ensure shooter is off before the match begins
}
+ /**
+ * Called once immediately after the start button is pressed.
+ * Lowers the actuator to the "down / intake" position and moves the
+ * indexer to its zero (home) slot so intake can begin immediately.
+ */
public void teleopStart(){
- actuator.down();
- indexer.moveTo(Indexer.IndexerState.zero);
+ actuator.down(); // bring the actuator to the loading position
+ indexer.moveTo(Indexer.IndexerState.zero); // rotate drum so slot 0 faces the intake port
}
+ // -----------------------------------------------------------------------
+ // Main loop tick
+ // -----------------------------------------------------------------------
+
+ /**
+ * Called every loop iteration from the OpMode's while loop.
+ * Runs all periodic subsystem updates first, then delegates to the
+ * appropriate FSM state handler.
+ */
public void teleopTick()
{
- handlePeriodics();
+ handlePeriodics(); // run all subsystem periodic updates (inherited from BotPeriodics)
+
+ // Route to the correct state handler based on the current FSM state.
switch (state) {
case MotifSelection:
+ // Wait for the driver to press a face button choosing the motif.
if (g2.wasJustPressed(GamepadKeys.Button.X)){
- motif = PPG;
- indexer.prepareQuickspin(motif);
- state = FSM.Intake;
+ motif = PPG; // select PPG motif
+ indexer.prepareQuickspin(motif); // rotate drum to the first motif slot
+ state = FSM.Intake; // transition to intake phase
}
if (g2.wasJustPressed(GamepadKeys.Button.Y)){
- motif = PGP;
+ motif = PGP; // select PGP motif
indexer.prepareQuickspin(motif);
state = FSM.Intake;
}
if (g2.wasJustPressed(GamepadKeys.Button.B)){
- motif = GPP;
+ motif = GPP; // select GPP motif
indexer.prepareQuickspin(motif);
state = FSM.Intake;
}
break;
case Intake:
- handleIntakeState();
+ handleIntakeState(); // handle intake controls and state transitions
break;
case QuickOuttake:
- handleQuickOuttakeState();
+ handleQuickOuttakeState(); // handle quick-dump controls
break;
case SortOuttake:
- handleSortOuttakeState();
+ handleSortOuttakeState(); // handle sorted single-ball firing controls
break;
case Endgame:
- handleEndgameState();
+ handleEndgameState(); // handle endgame controls
break;
}
}
+ // -----------------------------------------------------------------------
+ // FSM state handlers
+ // -----------------------------------------------------------------------
+
+ /**
+ * Handles driver inputs and logic while in the {@link FSM#Intake} state.
+ *
+ * Key bindings (gamepad 2):
+ * Also handles the "drum full" rumble notification to both gamepads.
+ */
// MAINLINE HANDLERS
private void handleIntakeState() {
double leftTrigger = g2.getTrigger(GamepadKeys.Trigger.LEFT_TRIGGER);
- //double rightTrigger = g2.getTrigger(GamepadKeys.Trigger.RIGHT_TRIGGER);
- // Press-and-hold right bumper to spin up shooter while in Intake
+ // Right bumper held: begin spinning up the shooter and transition to quick outtake.
if (!actionHost.isRunning()) {
if (g2.gamepad.right_bumper) {
- rangeRequested = true;
- state = FSM.QuickOuttake;
- applyPreSpinRPM();
+ rangeRequested = true; // start requesting range / turn-correction from aimer
+ state = FSM.QuickOuttake; // switch FSM to quick-outtake mode
+ applyPreSpinRPM(); // command shooter to pre-spin target RPM
} else {
- outtake.stop();
- rangeRequested = false;
+ outtake.stop(); // no bumper held: ensure shooter is off
+ rangeRequested = false; // stop requesting range updates
}
}
-
+ // Left trigger: manually run the intake in addition to any continuous intake.
if (leftTrigger > TeleopConstants.Gamepad.TRIGGER_DEADZONE) intake.run();
else intake.stop();
- //if (rightTrigger > TRIGGER_DEADZONE) intake.runBackwards();
- //else intake.stop();
-
- if (g2.wasJustPressed(GamepadKeys.Button.DPAD_RIGHT)) indexer.moveTo(indexer.getState().next());
+ // DPAD_RIGHT: manually step the indexer to the next slot.
+ if (g2.wasJustPressed(GamepadKeys.Button.DPAD_RIGHT))
+ indexer.moveTo(indexer.getState().next());
+ // X: debug override – force all three slots to the PPG colour assignment.
if(g2.wasJustPressed(GamepadKeys.Button.X))
- indexer.initializeColors(Indexer.ArtifactColor.PURPLE, Indexer.ArtifactColor.PURPLE, Indexer.ArtifactColor.GREEN);
+ indexer.initializeColors(
+ Indexer.ArtifactColor.PURPLE,
+ Indexer.ArtifactColor.PURPLE,
+ Indexer.ArtifactColor.GREEN
+ );
+
+ // A: jump to QuickOuttake without pre-spinning.
if (g2.wasJustPressed(GamepadKeys.Button.A))
state = FSM.QuickOuttake;
+
+ // B: jump to SortOuttake (disable intake auto-advance so drum doesn't rotate during aiming).
if (g2.wasJustPressed(GamepadKeys.Button.B)){
state = FSM.SortOuttake;
- indexer.setIntaking(false);
- indexer.moveTo(indexer.getState());
+ indexer.setIntaking(false); // stop indexer auto-advance
+ indexer.moveTo(indexer.getState()); // hold current slot
}
+
+ // DPAD_UP: re-prepare the quick-spin ordering for the current motif.
if(g2.wasJustPressed(GamepadKeys.Button.DPAD_UP))
- indexer.prepareQuickspin(new Indexer.ArtifactColor[]{Indexer.ArtifactColor.GREEN, Indexer.ArtifactColor.PURPLE,Indexer.ArtifactColor.PURPLE});
+ indexer.prepareQuickspin(new Indexer.ArtifactColor[]{
+ Indexer.ArtifactColor.GREEN,
+ Indexer.ArtifactColor.PURPLE,
+ Indexer.ArtifactColor.PURPLE
+ });
+
+ // Y: jump to endgame mode.
if (g2.wasJustPressed(GamepadKeys.Button.Y)) state = FSM.Endgame;
+
+ // Second DPAD_UP check prepares using the driver-selected motif.
if(g2.wasJustPressed(GamepadKeys.Button.DPAD_UP))
indexer.prepareQuickspin(motif);
- if(indexer.isFull() && !rumbledAlready && !g1.gamepad.isRumbling() && !g2.gamepad.isRumbling()){
- g1.gamepad.rumbleBlips(TeleopConstants.Gamepad.FULL_WARNING_RUMBLES);
- g2.gamepad.rumbleBlips(TeleopConstants.Gamepad.FULL_WARNING_RUMBLES);
- rumbledAlready = true;
+ // Drum-full notification: rumble both gamepads once when the drum fills.
+ if(indexer.isFull() && !rumbledAlready
+ && !g1.gamepad.isRumbling() && !g2.gamepad.isRumbling()){
+ g1.gamepad.rumbleBlips(TeleopConstants.Gamepad.FULL_WARNING_RUMBLES); // alert driver 1
+ g2.gamepad.rumbleBlips(TeleopConstants.Gamepad.FULL_WARNING_RUMBLES); // alert driver 2
+ rumbledAlready = true; // latch so we only rumble once per fill cycle
}
}
+ /**
+ * Overrides the parent's alliance selection handler to add gamepad LED
+ * color feedback when an alliance is selected.
+ *
+ * Key bindings (gamepad 1):
+ * Key bindings (gamepad 2):
+ * Key bindings (gamepad 2):
+ * Key bindings (gamepad 2):
+ * This is the quick-outtake path: all three artifacts are fired at once
+ * without sorting by colour. The sequence:
+ * Finds the best slot containing a GREEN artifact, rotates the indexer
+ * to that slot, spins up the shooter, and fires when in range.
+ * If no green artifact is loaded, returns a no-op action.
+ *
+ * Sequence:
+ * Finds the best slot containing a PURPLE artifact, rotates the indexer
+ * to that slot, spins up the shooter, and fires when in range.
+ * If no purple artifact is loaded, returns a no-op action.
+ *
+ * Sequence is the same as {@link #actionFireGreen()} but targets PURPLE.
+ *
+ * @return the purple-shot {@link Action}, or a no-op if no purple slot exists
+ */
private Action actionFirePurple() {
+ // Find the indexer slot that best matches a PURPLE artifact.
final Indexer.IndexerState slot =
indexer.findBestSlotForColor(Indexer.ArtifactColor.PURPLE);
if (slot == null) {
+ // No purple artifact loaded: return an empty no-op action.
return new InstantAction(() -> {
});
}
- final double rpm = getTargetRPM();
+ final double rpm = getTargetRPM(); // capture current RPM target
return new SequentialAction(
- new InstantAction(actuator::down),
- new InstantAction(() -> indexer.moveTo(slot, true)),
+ new InstantAction(actuator::down), // 1. ensure actuator is lowered
+ new InstantAction(() -> indexer.moveTo(slot, true)), // 2. rotate to purple slot
- new InstantAction(() -> outtake.set(rpm)),
- // wait for shooter RPM to be within 100 instead of fixed spinup
+ new InstantAction(() -> outtake.set(rpm)), // 3. spin up shooter
+ // 4. wait for shooter RPM to be within 100 of target
new Action() {
@Override
public boolean run(TelemetryPacket p) {
- return !outtake.inRange(100.0);
+ return !outtake.inRange(100.0); // returns true while NOT in range (keep waiting)
}
},
- new InstantAction(actuator::upIndexed),
- new SleepAction(1),
+ new InstantAction(actuator::upIndexed), // 5. raise actuator to fire position
+ new SleepAction(1), // 6. allow 1 s for artifact to exit
+ // 7. mark the fired slot as empty
new InstantAction(() ->
indexer.assignSlotColor(slot, Indexer.ArtifactColor.EMPTY)
),
- new InstantAction(outtake::stop),
- new InstantAction(actuator::down)
+ new InstantAction(outtake::stop), // 8. stop shooter
+ new InstantAction(actuator::down) // 9. lower actuator
);
}
}
\ No newline at end of file
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/BotPeriodics.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/BotPeriodics.java
index 8ce26899e589..6e8df30dfa45 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/BotPeriodics.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/BotPeriodics.java
@@ -16,195 +16,413 @@
import org.firstinspires.ftc.teamcode.subsystems.Movement;
import org.firstinspires.ftc.teamcode.subsystems.Outtake;
+/**
+ * BotPeriodics is the base class for all teleop bot controllers.
+ *
+ * It holds every hardware subsystem as a protected field and implements the
+ * per-loop "periodic" handlers that must run every iteration regardless of the
+ * current game state:
+ * Subclasses (e.g., {@link Bot}) override {@link #handleAllianceSelection()}
+ * to add extra LED/haptics logic, and add game-state FSM code on top.
+ *
+ * Annotated with {@link Config} so {@link #targetRPM} and any future static
+ * fields are editable via FTC Dashboard.
+ */
@Config
public class BotPeriodics {
+
+ // -----------------------------------------------------------------------
+ // Subsystems (protected so subclasses can access them directly)
+ // -----------------------------------------------------------------------
+
+ /** Conveyor / intake roller subsystem. */
protected final Intake intake;
+
+ /** Three-slot indexer drum subsystem. */
protected final Indexer indexer;
+
+ /** Actuator (launch ramp or ball-gate) subsystem. */
protected final Actuator actuator;
+
+ /** Flywheel shooter (outtake) subsystem. */
protected final Outtake outtake;
+
+ /** Mecanum drivetrain subsystem. */
protected final Movement movement;
+
+ /** Limelight AprilTag scanner subsystem. */
protected final AprilTag aprilTag;
+
+ /** Localization-based heading aimer. */
protected final Aimer aimer;
+
+ /** Road Runner MecanumDrive used for pose estimation and field-centric heading. */
protected final MecanumDrive drive;
+
+ // -----------------------------------------------------------------------
+ // Gamepad wrappers (FTCLib – provide edge-detection on button presses)
+ // -----------------------------------------------------------------------
+
+ /** Driver 1 gamepad with edge-detection helpers. */
protected final GamepadEx g1;
+
+ /** Driver 2 (operator) gamepad with edge-detection helpers. */
protected final GamepadEx g2;
+
+ /** FTC telemetry object for writing Driver Station / Dashboard output. */
protected final Telemetry telemetry;
+
+ // -----------------------------------------------------------------------
+ // Aiming / vision state
+ // -----------------------------------------------------------------------
+
+ /** Bearing turn-correction power from the last AprilTag/aimer update.
+ * Blended into the driver's rotation input to keep the robot aimed at the goal. */
protected double bearingTurnCorrection = 0;
+
+ /** Background Road Runner Action executor (one action at a time during teleop). */
protected ActionHost actionHost;
- // camera vision
+
+ /** Whether field-centric drive mode is currently active. */
protected boolean fieldCentric = false;
+
+ /** When {@code true} the aimer runs every {@link #AIM_UPDATE_INTERVAL_MS} ms
+ * to maintain a continuous lock on the goal. The robot's heading correction
+ * is applied to the drive command as long as this flag is set. */
protected boolean continuousAprilTagLock = false;
+
+ /** System time (ms) of the last aimer update tick. Used to rate-limit calls to
+ * {@link Aimer#calculateLocalizedTurnPower()} to {@link #AIM_UPDATE_INTERVAL_MS}. */
protected long lastAimUpdate = 0;
+
+ /** The turn-correction value from the previous aimer update. Held between
+ * updates so the motor command is not zero between update periods. */
protected double lastTurnCorrection = 0.0;
+
+ /** Current turn correction applied to the drive command this loop iteration. */
protected double turnCorrection = 0.0;
+
+ /** Human-readable alliance colour selected by the drivers ("Red" or "Blue").
+ * Shown in telemetry so the team can confirm the correct alliance is configured. */
protected String colorGoalSelected = "";
+
+ /** Whether the driver has requested an RPM spin-up/range calculation this tick. */
protected boolean rangeRequested = false;
- protected double[] targetData = {0,0,0};
+ /** Most recent aimer output: {@code {turnPower, range, bearing}}.
+ * Index 0 = turn power, Index 1 = range (inches), Index 2 = bearing (degrees). */
+ protected double[] targetData = {0, 0, 0};
+
+ /** When {@code true} the intake runs at slow speed continuously in the background
+ * (passive intake). Toggled by the DPAD_DOWN button on gamepad 2. */
protected boolean continuousIntake = true;
+ // -----------------------------------------------------------------------
+ // Shooter RPM configuration
+ // -----------------------------------------------------------------------
+
+ /**
+ * Target RPM for the shooter flywheel. Updated dynamically from the range
+ * regression when aiming is active; can also be set manually via Dashboard.
+ */
public static double targetRPM = 3800;
+
+ /**
+ * Minimum interval (ms) between aimer recalculations.
+ * At 20 ms = 50 Hz: fast enough for smooth correction without overloading
+ * the control loop with expensive trigonometry every single tick.
+ */
protected static final long AIM_UPDATE_INTERVAL_MS = 20;
+ // -----------------------------------------------------------------------
+ // Drive mode
+ // -----------------------------------------------------------------------
+
+ /**
+ * {@code true} = use gamepad 2 for drive inputs; {@code false} = gamepad 1.
+ * Allows a "two-movement-mode" where the second driver also controls driving
+ * (e.g., for practice or single-player competitions).
+ */
protected boolean twoMovementMode;
+ // -----------------------------------------------------------------------
+ // Constructor
+ // -----------------------------------------------------------------------
+
+ /**
+ * Constructs a BotPeriodics and initialises every hardware subsystem.
+ *
+ * @param hardwareMap the robot's hardware map used to locate all devices
+ * @param tele FTC telemetry object; output goes to Driver Station and Dashboard
+ * @param mecanumDrive pre-constructed Road Runner drive for pose estimation
+ * @param gamepad1 the Driver 1 raw gamepad
+ * @param gamepad2 the Driver 2 (operator) raw gamepad
+ * @param useMovement when {@code true} gamepad 2 also controls driving
+ */
public BotPeriodics(HardwareMap hardwareMap, Telemetry tele, MecanumDrive mecanumDrive, Gamepad gamepad1, Gamepad gamepad2, boolean useMovement) {
- intake = new Intake(hardwareMap);
- indexer = new Indexer(hardwareMap);
+ // Construct all hardware subsystems from the hardware map.
+ intake = new Intake(hardwareMap);
+ indexer = new Indexer(hardwareMap);
actuator = new Actuator(hardwareMap);
- outtake = new Outtake(hardwareMap, Outtake.Mode.RPM);
- drive = mecanumDrive;
+ outtake = new Outtake(hardwareMap, Outtake.Mode.RPM); // use RPM-mode for closed-loop control
+ drive = mecanumDrive;
movement = new Movement(hardwareMap, drive);
aprilTag = new AprilTag(hardwareMap, tele);
- aimer = new Aimer(drive);
+ aimer = new Aimer(drive);
+
+ // Wrap raw gamepads with FTCLib GamepadEx to get edge-detection helpers.
g1 = new GamepadEx(gamepad1);
g2 = new GamepadEx(gamepad2);
- actionHost = new ActionHost();
+
+ actionHost = new ActionHost(); // background action executor
telemetry = tele;
- twoMovementMode = useMovement;
+ twoMovementMode = useMovement; // remember whether gamepad 2 also drives
}
-
+
+ // -----------------------------------------------------------------------
+ // Main periodic handler
+ // -----------------------------------------------------------------------
+
+ /**
+ * Runs all per-loop periodic tasks. Must be called at the top of every
+ * teleop iteration (before the subclass's state-machine switch).
+ *
+ * Order of operations:
+ * Subclasses may override this to add LED/haptic feedback on selection.
+ */
protected void handleAllianceSelection() {
if (g1.wasJustPressed(GamepadKeys.Button.BACK)) {
- aprilTag.setPipeline(0);
- aimer.setBlueTarget();
- colorGoalSelected = "Blue";
+ aprilTag.setPipeline(0); // pipeline 0 = blue goal AprilTag
+ aimer.setBlueTarget(); // aim toward the blue alliance goal
+ colorGoalSelected = "Blue"; // update telemetry label
}
if (g1.wasJustPressed(GamepadKeys.Button.START)) {
- aprilTag.setPipeline(1);
- aimer.setRedTarget();
- colorGoalSelected = "Red";
+ aprilTag.setPipeline(1); // pipeline 1 = red goal AprilTag
+ aimer.setRedTarget(); // aim toward the red alliance goal
+ colorGoalSelected = "Red"; // update telemetry label
}
}
+ /**
+ * Reads stick inputs from the active driver gamepad and forwards them to
+ * the drive subsystem, applying the current turn correction from the aimer.
+ *
+ * If {@link #twoMovementMode} is {@code true} the drive inputs come from
+ * gamepad 2 (operator) instead of gamepad 1 (driver). This is useful for
+ * single-player practice sessions.
+ */
protected void handleMovement() {
+ // Read joystick axes from the appropriate gamepad.
+ double lx = g1.getLeftX(); // driver 1 left stick X (strafe)
+ double ly = g1.getLeftY(); // driver 1 left stick Y (forward)
+ double rx = g1.getRightX(); // driver 1 right stick X (rotate)
- double lx = g1.getLeftX();
- double ly = g1.getLeftY();
- double rx = g1.getRightX();
if(twoMovementMode){
- lx = g2.getLeftX();
- ly = g2.getLeftY();
- rx = g2.getRightX();
+ // Override with gamepad 2 inputs in two-movement mode.
+ lx = g2.getLeftX();
+ ly = g2.getLeftY();
+ rx = g2.getRightX();
}
+
+ // Send to the drive subsystem in the appropriate mode.
if (fieldCentric) movement.teleopTickFieldCentric(lx, ly, rx, turnCorrection, true);
- else movement.teleopTick(lx, ly, rx, turnCorrection);
+ else movement.teleopTick(lx, ly, rx, turnCorrection); // robot-centric mode
}
+ /**
+ * Handles buttons that toggle the continuous AprilTag heading lock and
+ * trigger a localizer re-zero.
+ *
+ * When the lock is disabled {@link #turnCorrection} is set to 0 so the
+ * robot doesn't keep spinning based on a stale correction value.
+ */
protected void handleAprilTagLock() {
-
- // Toggle continuous lock
+ // A: enable continuous heading lock toward the goal.
if (g1.wasJustPressed(GamepadKeys.Button.A)) {
continuousAprilTagLock = true;
- g1.gamepad.rumbleBlips(2);
+ g1.gamepad.rumbleBlips(2); // 2 blips = lock engaged
}
+ // B: disable continuous heading lock.
if (g1.wasJustPressed(GamepadKeys.Button.B)) {
continuousAprilTagLock = false;
- g1.gamepad.rumbleBlips(1);
+ g1.gamepad.rumbleBlips(1); // 1 blip = lock released
}
+ // X: re-zero the localizer at the current alliance's human player zone.
if (g1.wasJustPressed(GamepadKeys.Button.X)) {
+ // Always relocalize regardless of which alliance is selected.
if (colorGoalSelected.equals("Blue"))
- aimer.relocalize();
+ aimer.relocalize(); // re-zero for blue human player zone
else if (colorGoalSelected.equals("Red"))
- aimer.relocalize();
+ aimer.relocalize(); // re-zero for red human player zone
else {
- aimer.relocalize();
+ aimer.relocalize(); // default relocalize even if no alliance was explicitly set
}
}
+ // If lock is off, clear any lingering turn correction from the last active period.
if (!continuousAprilTagLock){
- turnCorrection = 0;
+ turnCorrection = 0; // no heading correction when lock is disabled
}
}
+ // -----------------------------------------------------------------------
+ // Utility
+ // -----------------------------------------------------------------------
+
+ /**
+ * Returns the current target RPM for the shooter flywheel.
+ * Subclasses use this to set the outtake RPM; it is exposed here so the
+ * Dashboard can override it and so range-based RPM updates propagate.
+ *
+ * @return the current shooter target RPM
+ */
protected double getTargetRPM() {
- return targetRPM;
+ return targetRPM; // static field: shared across all instances and editable via Dashboard
}
}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/MainTeleop.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/MainTeleop.java
index 53376b8be969..11802bf98c5d 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/MainTeleop.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/MainTeleop.java
@@ -8,19 +8,60 @@
import org.firstinspires.ftc.teamcode.auto.roadrunner.miscRR.MecanumDrive;
+/**
+ * MainTeleop is the two-driver teleop OpMode for competition matches.
+ *
+ * This OpMode wires together the {@link Bot} controller with the FTC
+ * runtime in a simple three-phase pattern:
+ * Telemetry is multiplexed to both the Driver Station and FTC Dashboard via
+ * {@link MultipleTelemetry} so live data is visible on both displays during the
+ * match.
+ *
+ * Drive mode is set to robot-centric single-driver
+ * ({@code twoMovement = false}): only gamepad 1 controls driving.
+ * For a two-movement (operator-also-drives) version see {@link OneDriverTeleop}.
+ *
+ * Registered as {@code "MainTeleOp"} in the {@code "AA_main"} group so it
+ * appears at the top of the OpMode list on the Driver Station.
+ */
@TeleOp(name = "MainTeleOp", group = "AA_main")
public class MainTeleop extends LinearOpMode {
+ /**
+ * Entry point called by the FTC runtime.
+ * Initialises hardware, waits for the start signal, then loops.
+ *
+ * @throws InterruptedException if the OpMode is interrupted while waiting
+ */
@Override
public void runOpMode() throws InterruptedException {
+ // Mirror telemetry to FTC Dashboard so tuning values are visible on a laptop/PC.
telemetry = new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry());
+
+ // Construct the Road Runner MecanumDrive at the starting pose (0, 0, 0°).
+ // The localizer will be re-zeroed at the human player zone via the X button.
MecanumDrive drive = new MecanumDrive(hardwareMap, new Pose2d(0, 0, Math.toRadians(0)));
- Bot bot = new Bot(hardwareMap, telemetry, drive, gamepad1, gamepad2,false);
- bot.teleopInit();
- waitForStart();
- bot.teleopStart();
+
+ // Build the Bot controller. false = gamepad 1 drives (standard two-driver mode).
+ Bot bot = new Bot(hardwareMap, telemetry, drive, gamepad1, gamepad2, false);
+ bot.teleopInit(); // reset subsystems during init phase
+
+ waitForStart(); // block here until the driver presses the PLAY button
+
+ bot.teleopStart(); // position hardware at match-start positions
+
+ // Main teleop loop: run until the OpMode is stopped.
while (opModeIsActive() && !isStopRequested()) {
- bot.teleopTick();
+ bot.teleopTick(); // run one complete control loop iteration
}
}
}
\ No newline at end of file
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/OneDriverTeleop.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/OneDriverTeleop.java
index a97e4661d643..cb3f5727f166 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/OneDriverTeleop.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/OneDriverTeleop.java
@@ -8,19 +8,54 @@
import org.firstinspires.ftc.teamcode.auto.roadrunner.miscRR.MecanumDrive;
+/**
+ * OneDriverTeleop is an alternative teleop OpMode intended for single-driver
+ * practice sessions or situations where only one gamepad is available.
+ *
+ * The structure mirrors {@link MainTeleop} exactly; the only difference is
+ * the OpMode name registered with the FTC runtime ({@code "OneDriver"} instead
+ * of {@code "MainTeleOp"}). Both OpModes pass {@code twoMovement = false} to
+ * {@link Bot}, meaning only gamepad 1 controls driving.
+ *
+ * This OpMode also appears in the {@code "AA_main"} group on the Driver
+ * Station for quick access.
+ *
+ * Lifecycle:
+ * Grouping constants here (rather than scattering magic numbers through
+ * the code) makes it easy to:
+ * All fields are {@code public static} (not actually {@code final} at
+ * runtime) so that FTC Dashboard can write back updated values. The
+ * {@code final} modifier on the inner class is just syntactic sugar; the
+ * fields themselves are mutable through the Dashboard.
+ */
@Config
public final class TeleopConstants
{
- //These variables use final notation but aren't actually final since they are editable in the dashboard
+ // Note: fields use final notation syntactically but are NOT truly final
+ // because FTC Dashboard can edit them at runtime through reflection.
+
+ /**
+ * Constants that govern gamepad feedback (rumble, LEDs) and input
+ * filtering applied to both driver gamepads during teleop.
+ */
public static final class Gamepad {
- public static int GAMEPAD_LIGHT_COLOR_DURATION = 500;
+
+ /**
+ * Duration (milliseconds) for which the gamepad LED colour is held
+ * after an alliance-selection button press. After this time the LED
+ * reverts to its default colour.
+ */
+ public static int GAMEPAD_LIGHT_COLOR_DURATION = 500; // ms
+
+ /**
+ * Number of rumble "blips" sent to both gamepads when the indexer
+ * drum is completely full. Three short blips alert both drivers
+ * without being overly distracting.
+ */
public static int FULL_WARNING_RUMBLES = 3;
- public static double TRIGGER_DEADZONE = 0.05;
+
+ /**
+ * Minimum trigger axis value that counts as a deliberate button press.
+ * Values below this threshold are treated as 0. Prevents unintended
+ * intake activation from small amounts of resting-finger pressure on
+ * the trigger buttons.
+ */
+ public static double TRIGGER_DEADZONE = 0.05; // trigger value below this is ignored
}
//
}
+ *
+ *
+ * @return the quick-dump {@link Action} sequence
+ */
public Action actionQuickOuttake() {
return new SequentialAction(
- new InstantAction(actuator::upQuick),// lower up position for quick dump
- new SleepAction(.35),// for actuator
- new InstantAction(() -> indexer.setIndexerPower(FULL_BLAST_POWER)),// full blast
- new SleepAction(NON_INDEX_SPIN_TIME),
- new InstantAction(indexer::stopIndexerPower),
- new InstantAction(() -> indexer.setAutoOuttaking(false)),
- new InstantAction(() -> indexer.moveTo(Indexer.IndexerState.one)), // This means, if you don't move the indexer, the next intaken will enter slot 0
- new InstantAction(outtake::stop),
- new InstantAction(actuator::down),
- new InstantAction(indexer::initializeColors)
+ new InstantAction(actuator::upQuick), // 1. raise actuator (lower "up" position for quick dump)
+ new SleepAction(.35), // 2. wait for actuator to reach position (~350 ms)
+ new InstantAction(() -> indexer.setIndexerPower(FULL_BLAST_POWER)),// 3. blast drum at full power
+ new SleepAction(NON_INDEX_SPIN_TIME), // 4. hold blast for configured duration
+ new InstantAction(indexer::stopIndexerPower), // 5. stop the drum
+ new InstantAction(() -> indexer.setAutoOuttaking(false)), // 6. disable auto-outtake mode
+ new InstantAction(() -> indexer.moveTo(Indexer.IndexerState.one)), // 7. home to slot 1 (next intake enters slot 0)
+ new InstantAction(outtake::stop), // 8. stop shooter
+ new InstantAction(actuator::down), // 9. lower actuator back to intake position
+ new InstantAction(indexer::initializeColors) // 10. mark all slots EMPTY
);
}
+ // -----------------------------------------------------------------------
+ // Obelisk motif helpers
+ // -----------------------------------------------------------------------
+
+ /**
+ * Returns an instantaneous {@link Action} that rotates the indexer drum to
+ * the correct starting slot for the motif encoded in the obelisk tag.
+ *
+ *
+ *
+ *
+ * @param row field row number (0–3) whose colour pattern is being shot
+ * @param id supplier that returns the current obelisk tag ID at run time
+ * @param startingSlot the indexer slot index where the first intaken artifact landed
+ * @return an {@link InstantAction} that rotates the drum to the correct firing position
+ */
// helpers at the end of the file
public Action rotateToMotifColorBeforeOuttake(int row, IntSupplier id, int startingSlot) {
return new InstantAction(() -> {
- // inside action to read obelisk id when function runs not runtime
+ // Read the obelisk ID at action-run time (not when this method was called)
+ // so that a prior scanObelisk action has time to populate it.
int tagId = id.getAsInt();
if (tagId != 21 && tagId != 22 && tagId != 23) {
+ // Not a valid motif tag – log and bail out without moving the drum.
telemetry.addData("No obelisk Id 21, 22 or 23, id is", tagId);
return;
}
- // set current color configuration
+ // Apply the per-row colour assignments, rotated for the starting slot.
applyCurrentColorsFromRow(row, startingSlot);
- // get desired firing order from obelisk id
+ // Convert the obelisk ID to the desired artifact firing order.
Indexer.ArtifactColor[] desiredOrder = getDesiredShootOrder(tagId);
- // values gets an array of the enums
+ // Search all three indexer slots for the slot whose first two colours
+ // match the desired firing order.
for (Indexer.IndexerState state : Indexer.IndexerState.values()) {
telemetry.addData("Started search for index of proper", "color");
if (matchesOrder(state.index, desiredOrder)) {
telemetry.addData("Rotated To Motif", "Color");
- // Indexer.IndexerState gotoState = Indexer.IndexerState.values()[(state.index - 1) % Indexer.IndexerState.values().length];
+ // Rotate the drum to the matching slot so it fires in the right order.
Indexer.IndexerState gotoState = state;
- indexer.moveTo(gotoState, true);
- return;
+ indexer.moveTo(gotoState, true); // true = block until rotation complete
+ return; // stop after the first matching slot is found
}
}
});
}
+ /**
+ * Returns an {@link Action} that switches the indexer back to intake mode
+ * at slot two. Used to reset indexer state between outtake and intake phases.
+ *
+ * @return a {@link SequentialAction} that disables auto-outtake, enables intake
+ * at slot two, and rotates the drum to slot two
+ */
public Action actionSetSomeShizzle() {
return new SequentialAction(
- new InstantAction(() -> indexer.setAutoOuttaking(false)),
- new InstantAction(() -> indexer.setIntaking(true, Indexer.IndexerState.two)),
- new InstantAction(() -> indexer.moveTo(Indexer.IndexerState.two, true))
+ new InstantAction(() -> indexer.setAutoOuttaking(false)), // disable auto-outtake
+ new InstantAction(() -> indexer.setIntaking(true, Indexer.IndexerState.two)), // enable intake starting at slot 2
+ new InstantAction(() -> indexer.moveTo(Indexer.IndexerState.two, true)) // physically rotate drum to slot 2
);
}
+ // -----------------------------------------------------------------------
+ // Intake command helpers
+ // -----------------------------------------------------------------------
+
+ /**
+ * Returns an instantaneous {@link Action} that runs the intake in slow
+ * reverse (used to push partially-captured artifacts back out during reset).
+ *
+ * @return an {@link InstantAction} that calls {@code intake.runBackwardsSlow()}
+ */
public Action actionSetIntakeReverse() {
- return new InstantAction(intake::runBackwardsSlow);
+ return new InstantAction(intake::runBackwardsSlow); // slow reverse to eject partial artifacts
}
+ /**
+ * Returns an instantaneous {@link Action} that sets the intake to slow
+ * passive speed (keeps the intake alive without aggressive pulling).
+ *
+ * @return an {@link InstantAction} that calls {@code intake.runSlow()}
+ */
public Action actionSetIntakePassive() {
- return new InstantAction(intake::runSlow);
+ return new InstantAction(intake::runSlow); // passive slow intake
}
+ // -----------------------------------------------------------------------
+ // Timed intake trajectory
+ // -----------------------------------------------------------------------
+
+ /**
+ * Returns an {@link Action} that drives the robot to the intake field zone,
+ * runs the intake during the approach, and advances the indexer at
+ * pre-calibrated time offsets to accept up to three artifacts.
+ *
+ *
+ *
+ *
+ *
+ *
+ *
+ * @param id the obelisk AprilTag ID (should be 21, 22, or 23)
+ * @return an array of three {@link Indexer.ArtifactColor} values representing
+ * the desired firing order; empty array if the ID is unrecognised
+ */
private Indexer.ArtifactColor[] getDesiredShootOrder(int id) {
// After the tag ID cases, you want to change the physical rows into shooting that motif
Indexer.ArtifactColor[] desiredShootOrder;
switch (id) {
- case 21: // G -> P -> P - will shoot out in this order
+ case 21: // Tag 21 → G -> P -> P - will shoot out in this order
desiredShootOrder = new Indexer.ArtifactColor[] {Indexer.ArtifactColor.GREEN, Indexer.ArtifactColor.PURPLE, Indexer.ArtifactColor.PURPLE};
break;
- case 22: // P -> G -> P
+ case 22: // Tag 22 → P -> G -> P
desiredShootOrder = new Indexer.ArtifactColor[] {Indexer.ArtifactColor.PURPLE, Indexer.ArtifactColor.GREEN, Indexer.ArtifactColor.PURPLE};
break;
- case 23: // P -> P -> G
+ case 23: // Tag 23 → P -> P -> G
desiredShootOrder = new Indexer.ArtifactColor[] {Indexer.ArtifactColor.PURPLE, Indexer.ArtifactColor.PURPLE, Indexer.ArtifactColor.GREEN};
break;
default:
- desiredShootOrder = new Indexer.ArtifactColor[] {};
+ desiredShootOrder = new Indexer.ArtifactColor[] {}; // unknown ID: empty order
break;
}
- return desiredShootOrder;
+ return desiredShootOrder; // return the color order to fire in
}
}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auto/utils/Hardware.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auto/utils/Hardware.java
index 1a26bd4948ad..769b2a5707e5 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auto/utils/Hardware.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auto/utils/Hardware.java
@@ -10,33 +10,105 @@
import org.firstinspires.ftc.teamcode.subsystems.limelight.AprilTag;
import org.firstinspires.ftc.teamcode.subsystems.limelight.Aimer;
+/**
+ * Hardware is a data class that centralises construction and storage of every
+ * robot subsystem needed during autonomous OpModes.
+ *
+ *
+ *
+ *
+ *
+ * Hardware hw = new Hardware(hardwareMap, telemetry, this, startPose);
+ * hw.actions.initializeAuto(Indexer.IndexerState.zero).run(...);
+ *
+ */
public class Hardware {
+
+ // -----------------------------------------------------------------------
+ // Subsystem fields
+ // -----------------------------------------------------------------------
+
+ /** Conveyor/intake roller subsystem. */
public final Intake intake;
+
+ /** Three-slot indexer drum subsystem. */
public final Indexer indexer;
+
+ /** Flywheel shooter (outtake) subsystem, configured in RPM mode. */
public final Outtake outtake;
+
+ /** Actuator (launch ramp or ball-gate) subsystem. */
public final Actuator actuator;
+
+ /** Limelight AprilTag scanner subsystem. */
public final AprilTag aprilTag;
+
+ /** Localization-based heading aimer. */
public final Aimer aprilAimer;
+
+ /** High-level action factory that builds Road Runner Action sequences for
+ * autonomous routines (intake, outtake, scanning, etc.). */
public final BotActions actions;
+
+ /** Road Runner MecanumDrive used for trajectory execution and pose estimation. */
public final MecanumDrive mecanumDrive;
+ // -----------------------------------------------------------------------
+ // Constructor
+ // -----------------------------------------------------------------------
+
+ /**
+ * Constructs all robot subsystems and applies autonomous-specific configuration.
+ *
+ *
+ *
+ *
+ * @param hardwareMap the robot's hardware map used to retrieve all devices
+ * @param telemetry FTC telemetry for the AprilTag subsystem debug output
+ * @param opMode the running LinearOpMode (passed to BotActions for stop checks)
+ * @param startPose the robot's field starting pose for Road Runner's localizer
+ */
public Hardware(HardwareMap hardwareMap, Telemetry telemetry, LinearOpMode opMode, Pose2d startPose) {
+ // Build Road Runner MecanumDrive at the provided starting pose.
mecanumDrive = new MecanumDrive(
hardwareMap,
- startPose
+ startPose // initialise localizer with match starting position
);
+ // Construct each subsystem from the hardware map.
intake = new Intake(hardwareMap);
indexer = new Indexer(hardwareMap);
- indexer.AUTO_ADVANCEMENT_MODE = Indexer.AutoAdvancement.DISABLED;
- indexer.ENABLE_FULL_UNKNOWN_SCAN = false;
- indexer.SCAN_COLORS = false;
- outtake = new Outtake(hardwareMap, Outtake.Mode.RPM);
+ // Autonomous-specific indexer configuration:
+ indexer.AUTO_ADVANCEMENT_MODE = Indexer.AutoAdvancement.DISABLED; // explicit control
+ indexer.ENABLE_FULL_UNKNOWN_SCAN = false; // skip re-scan (colours known in advance)
+ indexer.SCAN_COLORS = false; // disable live sensor reads to save loop time
+
+ outtake = new Outtake(hardwareMap, Outtake.Mode.RPM); // RPM closed-loop shooter
actuator = new Actuator(hardwareMap);
aprilTag = new AprilTag(hardwareMap, telemetry);
aprilAimer = new Aimer(mecanumDrive);
+ // Build the action factory last so it can reference all subsystems above.
actions = new BotActions(this, telemetry, opMode);
}
}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auto/utils/Paths.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auto/utils/Paths.java
index 0070d3778389..239a25a8818b 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auto/utils/Paths.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auto/utils/Paths.java
@@ -5,8 +5,60 @@
import org.firstinspires.ftc.teamcode.auto.roadrunner.miscRR.MecanumDrive;
+/**
+ * Paths is a utility class that builds Road Runner trajectory sequences for
+ * full autonomous routines.
+ *
+ *
+ *
+ *
+ *
+ *
+ *
+ * @param mecanumDrive the Road Runner drive used to build trajectories
+ * @param actions action factory for inline action injection (currently unused but reserved)
+ * @param startPose field pose at which the path begins
+ * @param shootPos 2-D field position where the robot shoots
+ * @param parkPos 2-D field position of the parking zone
+ * @param obeliskScanPos 2-D field position where the Limelight can see the obelisk tag
+ * @param artifactPositions 3x4 array: index[row][0] = row start, index[row][1..3] = artifact positions
+ * @param intakeHeading robot heading (radians) while traversing the intake rows
+ * @param outtakeHeading robot heading (radians) at the shooting and parking positions
+ * @param obeliskScanHeading robot heading (radians) when scanning the obelisk tag
+ * @return the complete autonomous trajectory as a single {@link Action}
+ */
public static Action buildPath(
MecanumDrive mecanumDrive,
BotActions actions,
@@ -19,52 +71,45 @@ public static Action buildPath(
double outtakeHeading,
double obeliskScanHeading
) {
- //actions.initializeColors(Indexer.ArtifactColor.GREEN, Indexer.ArtifactColor.PURPLE, Indexer.ArtifactColor.PURPLE);
-
+ // Start building the trajectory chain from the provided starting pose.
TrajectoryActionBuilder builder = mecanumDrive.actionBuilder(startPose);
- /*builder.stopAndAdd(
- new RaceAction(
- actions.actionPeriodic(), // keeps updating shooter/indexer
- builder.fresh()
- .build())
-
- );*/
+ // Drive to the obelisk scan position so the Limelight can see the motif tag.
+ // (The scan action is currently commented out while being validated.)
builder.strafeToSplineHeading(obeliskScanPos, obeliskScanHeading)
- /*.stopAndAdd(actions.actionScanObelisk())*/;
+ /*.stopAndAdd(actions.actionScanObelisk())*/; // enable when obelisk scan is ready
+
+ // Drive to the shooting position and execute the shoot-with-lock action.
+ // (Shoot action is commented out; trajectory movement is active.)
builder.strafeToSplineHeading(shootPos, outtakeHeading)
/*.stopAndAdd(actions.actionShootWithLock(actions.aprilTag.getObeliskId(), 2.0, mecanumDrive))*/;
+ // Iterate from the farthest intake row (row 2) back to the nearest (row 0).
for (int row = 2; row >= 0; row--) {
- // Move to the row's starting position for intake
+ // Move to the starting position of this row at the intake heading.
builder.strafeToSplineHeading(
- artifactPositions[row][0],
- intakeHeading
+ artifactPositions[row][0], // first waypoint of the row
+ intakeHeading // heading that points intake toward the artifacts
);
+ // Traverse each artifact column within the row (columns 1, 2, 3).
for (int col = 1; col <= 3; col++) {
- builder/*.stopAndAdd(new ParallelAction(
- //actions.actionIntakeOneCycle(),
- // mecanumDrive.actionBuilder(new Pose2d(artifactPositions[row][0], intakeHeading))
- */.strafeToSplineHeading(
- artifactPositions[row][col],
- intakeHeading
- )
- /*)
- )*/;
- //builder.afterTime(.5, actions.actionIntakeOneCycle());
+ // Strafe to each artifact's field position to collect it.
+ builder.strafeToSplineHeading(
+ artifactPositions[row][col], // x,y of this artifact
+ intakeHeading // keep same intake heading throughout
+ );
}
+ // After collecting all artifacts in this row, drive to shoot.
+ // (Disabled; re-enable when sorted outtake is validated.)
//builder.stopAndAdd(actions.actionShootWithLock(actions.aprilTag.getObeliskId(), 2.0, mecanumDrive));
-
- // already turns off lock-in mode in action
- // builder.stopAndAdd(new InstantAction(() -> continuousAprilTagLock = false));
}
+ // After all intake rows, drive to the parking zone and stop.
return builder
- .strafeToSplineHeading(parkPos, outtakeHeading)
- //.stopAndAdd(actions.actionPark())
- .build();
+ .strafeToSplineHeading(parkPos, outtakeHeading) // drive to park position
+ //.stopAndAdd(actions.actionPark()) // enable when parking action is ready
+ .build(); // finalise and return the trajectory action
}
}
-
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Actuator.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Actuator.java
index 15a7bbd3b754..61b25d52d57d 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Actuator.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Actuator.java
@@ -4,63 +4,197 @@
import com.arcrobotics.ftclib.hardware.SimpleServo;
import com.qualcomm.robotcore.hardware.HardwareMap;
+/**
+ * Actuator controls the physical ramp/deflector servo that lifts game elements
+ * (artifacts) from the indexer into the shooter barrel.
+ *
+ *
+ *
+ *
+ *
+ *
+ *
+ *
+ */
public class AntiDefense {
+ // -----------------------------------------------------------------------
+ // Dashboard-tunable gains and tolerances
+ // -----------------------------------------------------------------------
+
+ /** Proportional gain for X/Y position correction.
+ * Multiplied by the XY error vector to produce a drive velocity.
+ * Increase to push back harder; too high causes oscillation. */
public static double kP = 0.0;
- public static double kH = 0.0; //heading gain
+
+ /** Heading (angular) proportional gain.
+ * Multiplied by the heading error (radians) to produce a yaw correction. */
+ public static double kH = 0.0; // heading gain
+
+ /** Maximum XY distance (inches) from the reference pose before correction
+ * activates. Within this tolerance the robot is considered "on target"
+ * and no drive output is applied. */
public static double xyTolerance = 0.5;
- public static double aTolerance = Math.PI /30;
+
+ /** Maximum heading error (radians) before rotational correction activates.
+ * π/30 ≈ 6° – small enough to keep the robot well-aimed. */
+ public static double aTolerance = Math.PI / 30;
+
+ // -----------------------------------------------------------------------
+ // State
+ // -----------------------------------------------------------------------
+
+ /** The Road Runner MecanumDrive used to command wheel powers. */
MecanumDrive drive;
+
+ /** The pose the robot should return to if it is pushed away.
+ * {@code null} when the lock is inactive. */
private Pose2d referencePoint;
+
+ // -----------------------------------------------------------------------
+ // Constructor
+ // -----------------------------------------------------------------------
+
+ /**
+ * Creates an AntiDefense instance.
+ *
+ * @param telemetry currently unused; reserved for future debug output
+ * @param drive the MecanumDrive used to issue correction velocities
+ */
public AntiDefense(Telemetry telemetry, MecanumDrive drive)
{
- this.drive = drive;
+ this.drive = drive; // store the drive reference for update() to use
}
+
+ // -----------------------------------------------------------------------
+ // Lock control
+ // -----------------------------------------------------------------------
+
+ /**
+ * Activates the pose lock at the given position.
+ * From this point on, {@link #update(Pose2d)} will try to return the robot
+ * to {@code currentBotPos} whenever it is pushed away.
+ *
+ * @param currentBotPos the field-relative pose to hold (usually the robot's
+ * current estimated pose at the moment of locking)
+ */
public void lock(Pose2d currentBotPos)
{
- referencePoint = currentBotPos;
+ referencePoint = currentBotPos; // save this pose as the correction target
}
+
+ /**
+ * Deactivates the pose lock.
+ * After calling this, {@link #update(Pose2d)} is a no-op until
+ * {@link #lock(Pose2d)} is called again.
+ */
public void unlock()
{
- referencePoint = null;
+ referencePoint = null; // null reference means "no lock active"
}
+
+ // -----------------------------------------------------------------------
+ // Helper
+ // -----------------------------------------------------------------------
+
+ /**
+ * Wraps an angle (radians) into the range (-π, π].
+ *
+ * Mechanical Overview
+ * Colour Classification
+ * Modes
+ *
+ *
+ *
+ * Auto-Advancement Policies
+ *
+ *
+ *
+ *
+ *
+ */
public Indexer(HardwareMap hardwareMap) {
- CRServo servo = hardwareMap.get(CRServo.class, "index");
- AnalogInput analog = hardwareMap.get(AnalogInput.class, "indexAnalog");
+ CRServo servo = hardwareMap.get(CRServo.class, "index"); // fetch the drum CR servo
+ AnalogInput analog = hardwareMap.get(AnalogInput.class, "indexAnalog"); // fetch the drum angle encoder
+ // Build the closed-loop servo controller using the servo and its encoder.
servoControl = new CRServoPositionControl(servo, analog);
+
+ // Build the colour sensor wrapper (fetches the sensor internally by name).
colorSensor = new ColorSensorSystem(hardwareMap);
- unknownScanTimer.reset();
+ unknownScanTimer.reset(); // start the cooldown timer so the first scan isn't delayed
}
- // getters
+ // -----------------------------------------------------------------------
+ // Getters
+ // -----------------------------------------------------------------------
+
+ /** Returns the currently targeted drum slot. */
public IndexerState getState() { return state; }
+
+ /** Returns {@code true} if the drum is in intaking mode. */
public boolean isIntaking() { return intaking; }
+
+ /**
+ * Returns the stored colour for the specified slot.
+ * @param s the slot to query
+ * @return the {@link ArtifactColor} stored in that slot's memory
+ */
public ArtifactColor getColorAt(IndexerState s) { return slot(s).color; }
+
+ /** Returns {@code true} if any artifact is detected in or remembered by the drum. */
public boolean isLoaded() { return loaded; }
- /** Initialize all slots to the given color (default EMPTY) and reset observations/flags. */
+ // -----------------------------------------------------------------------
+ // Slot initialisation
+ // -----------------------------------------------------------------------
+
+ /**
+ * Resets all slot memories to {@link ArtifactColor#EMPTY} and clears all
+ * observations and fill-cycle flags. Call before each match or test to
+ * start from a clean state.
+ */
public void initializeColors() { initializeColors(ArtifactColor.EMPTY); }
+ /**
+ * Resets all slot memories to the given colour. Used at the start of an
+ * autonomous routine where all three slots are pre-loaded with a known colour.
+ *
+ * @param initialColor the colour to assign to every slot
+ */
public void initializeColors(ArtifactColor initialColor) {
for (SlotState slot : slots) {
- slot.color = initialColor;
- slot.obs.reset();
- slot.wasEmpty = true;
- slot.fillingHits = 0;
- slot.fillCycleActive = false;
+ slot.color = initialColor; // assign the same color to every slot
+ slot.obs.reset(); // clear any previous sensor observations
+ slot.wasEmpty = true; // reset the transition-detection latch
+ slot.fillingHits = 0; // clear the fill-confirmation counter
+ slot.fillCycleActive = false; // deactivate any in-progress fill cycle
}
- recomputeNoEmpty();
+ recomputeNoEmpty(); // update the full/not-full flag based on new colours
}
+ /**
+ * Sets each slot to a specific colour. Used when the pre-game loading order
+ * is known (e.g., slot 0 = GREEN, slot 1 = PURPLE, slot 2 = PURPLE).
+ *
+ * @param one colour for slot 0
+ * @param two colour for slot 1
+ * @param three colour for slot 2
+ */
public void initializeColors(ArtifactColor one, ArtifactColor two, ArtifactColor three) {
- ArtifactColor[] colors = { one, two, three };
+ ArtifactColor[] colors = { one, two, three }; // pack into array for indexed access
for (int i = 0; i < slots.length; i++) {
SlotState slot = slots[i];
- slot.color = colors[i];
- slot.obs.reset();
- slot.wasEmpty = true;
- slot.fillingHits = 0;
- slot.fillCycleActive = false;
+ slot.color = colors[i]; // assign the per-slot colour
+ slot.obs.reset(); // clear observation history
+ slot.wasEmpty = true; // reset transition latch
+ slot.fillingHits = 0; // clear fill counter
+ slot.fillCycleActive = false; // deactivate fill cycle
}
- recomputeNoEmpty();
+ recomputeNoEmpty(); // update the full flag
}
+ // -----------------------------------------------------------------------
+ // Angle accessor
+ // -----------------------------------------------------------------------
+
+ /**
+ * Returns the drum's current measured angle, wrapped into [0, 360°).
+ * Reads the continuous angle from the servo controller and applies a
+ * positive-modulo wrap to ensure the value is always in range.
+ *
+ * @return current drum angle in degrees [0, 360)
+ */
public double getMeasuredAngle() {
- return mod(servoControl.getCurrentAngle(), 360.0);
+ return mod(servoControl.getCurrentAngle(), 360.0); // wrap continuous angle to [0, 360)
}
- // api
+ // -----------------------------------------------------------------------
+ // Mode setters
+ // -----------------------------------------------------------------------
+
+ /**
+ * Switches between intaking and outtaking orientation.
+ * If the mode actually changes, re-issues the current slot's target angle
+ * using the new offset so the servo adjusts immediately.
+ *
+ * @param isIntaking {@code true} to orient the drum for intaking;
+ * {@code false} for outtaking
+ */
public void setIntaking(boolean isIntaking) {
if (this.intaking != isIntaking) {
this.intaking = isIntaking;
- moveTo(state, true);
+ moveTo(state, true); // force-reissue so the offset change takes effect immediately
}
}
+ /**
+ * Switches intake mode and simultaneously commands the drum to a specific slot.
+ * Used at the start of a match or after a shooting sequence to set both the
+ * mode and the initial slot position in one call.
+ *
+ * @param isIntaking {@code true} for intaking, {@code false} for outtaking
+ * @param moveToState the slot to move to after setting the mode
+ */
public void setIntaking(boolean isIntaking, IndexerState moveToState) {
- this.intaking = isIntaking;
- moveTo(moveToState, true); // forceRecommend true
+ this.intaking = isIntaking; // update mode flag
+ moveTo(moveToState, true); // forceRecommend = true ensures servo is re-commanded
}
+ /**
+ * Enables or disables the autonomous-outtake angle offset.
+ * When {@code true}, slot angles use {@link #autoOuttakeOffsetAngle} instead
+ * of {@link #outtakeOffsetAngle}, which may differ mechanically from the
+ * teleop shooting position. Re-issues the current slot target after switching.
+ *
+ * @param isAutoOuttaking {@code true} to activate autonomous-outtake offset
+ */
public void setAutoOuttaking(boolean isAutoOuttaking) {
this.autoOuttaking = isAutoOuttaking;
- moveTo(state);
+ moveTo(state); // recompute and reissue target with the new offset
}
+ // -----------------------------------------------------------------------
+ // Colour-based movement
+ // -----------------------------------------------------------------------
+
+ /**
+ * Commands the drum to rotate to the best slot for the desired colour.
+ * "Best" is determined by {@link #scoreSlotForTarget}: a slot with an exact
+ * match scores 100; UNKNOWN scores 80; the other non-empty colour scores 60.
+ * EMPTY slots score 0 (skip them; an artifact is needed for outtaking).
+ *
+ * @param desired the target colour to seek
+ * @return {@code true} if a suitable slot was found and the move was issued;
+ * {@code false} if no non-empty slot exists for the desired colour
+ */
public boolean moveToColor(ArtifactColor desired) {
- IndexerState target = findBestSlotForColor(desired);
- if (target == null) return false;
- moveTo(target);
+ IndexerState target = findBestSlotForColor(desired); // find the highest-scoring slot
+ if (target == null) return false; // no usable slot found; abort
+ moveTo(target); // command the drum to rotate
return true;
}
+ // -----------------------------------------------------------------------
+ // Core moveTo methods
+ // -----------------------------------------------------------------------
+
+ /**
+ * Commands the drum to rotate to the given slot.
+ * If the target is already the current state, this is a no-op (no duplicate move).
+ *
+ * @param newState the slot to rotate to
+ */
public void moveTo(IndexerState newState) {
- moveTo(newState, false);
+ moveTo(newState, false); // use default: do not force-reissue if already at target
}
- // Move to a slot and forceRecommend will reissue the target even if it's the current slot.
+ /**
+ * Commands the drum to rotate to the given slot, with optional force-reissue.
+ *
+ *
+ *
+ *
+ * @param desiredOrder the 3-element firing order to achieve;
+ * e.g., {@code {GREEN, PURPLE, PURPLE}} means GREEN fires first
+ * @return {@code true} if the drum was successfully rotated to the starting slot;
+ * {@code false} if validation failed (wrong colour mix or invalid input)
+ */
public boolean prepareQuickspin(ArtifactColor[] desiredOrder) {
- // Validate input
- if (desiredOrder == null || desiredOrder.length != 3) return false;
- if (!isTwoPurpleOneGreen(desiredOrder)) return false;
+ if (desiredOrder == null || desiredOrder.length != 3) return false; // input sanity check
+ if (!isTwoPurpleOneGreen(desiredOrder)) return false; // must be exactly 2P + 1G
- // Validate stored slots
- if (!storedSlotsValidForQuickspin()) return false;
+ if (!storedSlotsValidForQuickspin()) return false; // drum must contain 2P + 1G
- // Quickspin must start in intaking mode
- setIntaking(true);
+ setIntaking(true); // quickspin must start in intaking mode for correct angle offsets
- // Find correct starting slot
+ // Search all three starting positions for one that produces the desired order.
for (IndexerState s : IndexerState.values()) {
int x = s.index;
if (matchesQuickspin(x, desiredOrder)) {
- moveTo(s);
+ moveTo(s); // found the right starting position; rotate there
return true;
}
}
- // Should never happen if validation passed
+ // Should never reach here if validation passed (all permutations were checked above).
return false;
}
+ // -----------------------------------------------------------------------
+ // Open-loop power control (used during timed outtake blast)
+ // -----------------------------------------------------------------------
+
+ /**
+ * Commands the servo to run at a fixed open-loop power.
+ * Overrides the closed-loop position controller until
+ * {@link #stopIndexerPower()} is called. Used during the "full blast"
+ * outtake where the drum spins continuously at a set power for a timed duration.
+ *
+ * @param power servo power in [-1.0, 1.0]; positive = forward rotation
+ */
/** Open-loop blast: full power until caller stops it (or sets another power). */
public void setIndexerPower(double power) { servoControl.setOpenLoopPower(power); }
- public void stopIndexerPower() { servoControl.setOpenLoopPower(0); servoControl.clearOpenLoop(); }
+ /**
+ * Stops the open-loop blast and returns to closed-loop mode.
+ * Sets power to 0 first, then clears the override flag so the
+ * position controller resumes on the next {@link #update()} call.
+ */
+ public void stopIndexerPower() {
+ servoControl.setOpenLoopPower(0); // zero the motor first
+ servoControl.clearOpenLoop(); // re-enable closed-loop position control
+ }
+
+ // -----------------------------------------------------------------------
+ // Main update loop
+ // -----------------------------------------------------------------------
+
+ /**
+ * Must be called every robot loop iteration to keep the indexer running.
+ *
+ *
+ *
+ */
// update loop:
// 1) Handle dashboard overrides
// 2) Update loaded state and servo control
@@ -243,133 +696,226 @@ public boolean prepareQuickspin(ArtifactColor[] desiredOrder) {
// 5) If intaking and any EMPTY exists, always seek an EMPTY slot (skip UNKNOWN too)
// 6) If full and still have UNKNOWN slots, optionally move to unknown slots for rescan
public void update() {
- handleDashboardCommands();
- refreshLoadedAndServo();
+ handleDashboardCommands(); // step 1: process any FTC Dashboard manual overrides
+ refreshLoadedAndServo(); // step 2: update sensor presence, loaded state, servo PD
+
+ // Step 3: run colour classification only while actively intaking and scanning is on,
+ // and only while at least one slot is still unclassified (avoid unnecessary work).
if (intaking && SCAN_COLORS && !allClassified()) {
- updateSlotClassification(debugClosestSlot());
+ updateSlotClassification(debugClosestSlot()); // classify the slot currently under the sensor
}
- // Update full flag every loop based on stored memory
+ // Step 4: update the FULL flag based on latest slot colour memories.
recomputeNoEmpty();
+ // Step 5a: SEEK_EMPTY auto-advance – move to an empty slot every iteration if settled.
if (AUTO_ADVANCEMENT_MODE == AutoAdvancement.SEEK_EMPTY &&
intaking &&
!noEmpty &&
isWithinTargetDegrees(ADVANCE_ANGLE_TOLERANCE)) {
- moveTo(findEmptySlot());
+ moveTo(findEmptySlot()); // advance to the nearest empty slot
}
+
+ // Step 5b: QUICK_SEEK auto-advance – only advance when the sensor currently sees an artifact.
if(AUTO_ADVANCEMENT_MODE == AutoAdvancement.QUICK_SEEK)
{
+ // Trigger only if: artifact currently detected AND drum has an empty slot AND drum is settled.
if(colorSensor.hasArtifact() && !noEmpty && isWithinTargetDegrees(ADVANCE_ANGLE_TOLERANCE))
{
- moveTo(findEmptySlot());
+ moveTo(findEmptySlot()); // move to the next available empty slot
}
}
- // If full and still have UNKNOWN slots, go look at them (intaking only)
+
+ // Step 6: if drum is full but some slots are still UNKNOWN, rotate to them for a rescan.
+ // A cooldown prevents spinning the drum in a tight loop if the colour can't be determined.
if (ENABLE_FULL_UNKNOWN_SCAN &&
intaking &&
noEmpty &&
unknownScanTimer.milliseconds() >= UNKNOWN_SCAN_COOLDOWN_MS) {
IndexerState target = findNextSlotWithStoredColor(state.last(), ArtifactColor.UNKNOWN);
if (target != null) {
- moveTo(target);
- unknownScanTimer.reset();
+ moveTo(target); // rotate to the unclassified slot for re-read
+ unknownScanTimer.reset(); // reset cooldown so we don't spam this move
}
}
}
+ // -----------------------------------------------------------------------
+ // Private: dashboard override handling
+ // -----------------------------------------------------------------------
+
+ /**
+ * Handles the FTC Dashboard advance and target-slot commands.
+ * Called once per {@link #update()} iteration.
+ * Uses edge detection to execute commands exactly once per dashboard change.
+ */
private void handleDashboardCommands() {
+ // Rising-edge detection: only advance once when dashAdvance goes false→true.
if (dashAdvance && !lastDashAdvance) {
- moveTo(state.next());
+ moveTo(state.next()); // advance to the next slot (wraps from 2 → 0)
}
- lastDashAdvance = dashAdvance;
+ lastDashAdvance = dashAdvance; // store current value for next-iteration comparison
+ // Direct target-slot: only re-issue moveTo when the slot selection changes.
if (dashTargetSlot != lastDashTargetSlot) {
if (dashTargetSlot >= 0 && dashTargetSlot <= 2) {
- moveTo(IndexerState.values()[dashTargetSlot], true);
+ moveTo(IndexerState.values()[dashTargetSlot], true); // force-move to the selected slot
}
- lastDashTargetSlot = dashTargetSlot;
+ lastDashTargetSlot = dashTargetSlot; // store for next-iteration comparison
}
}
+ // -----------------------------------------------------------------------
+ // Private: loaded state and servo update
+ // -----------------------------------------------------------------------
+
+ /**
+ * Determines the "loaded" flag (is any artifact present?) and runs one
+ * iteration of the servo position controller.
+ *
+ *
+ *
+ * The flag is passed to {@link CRServoPositionControl#setLoaded(boolean)} to
+ * switch between the heavier "loaded" PD gains and lighter "unloaded" gains.
+ */
private void refreshLoadedAndServo() {
- // Determine loaded state BEFORE control update
+ // Start with the live sensor reading.
boolean hasAnyArtifact = colorSensor.hasArtifact();
+
+ // Also check stored slot memories: if any slot remembers a real artifact, we're loaded.
for (SlotState slot : slots) {
if (slot.color == ArtifactColor.GREEN || slot.color == ArtifactColor.PURPLE) {
- hasAnyArtifact = true;
+ hasAnyArtifact = true; // confirmed artifact in memory; no need to check further
break;
}
}
- loaded = hasAnyArtifact;
- servoControl.setLoaded(loaded);
- servoControl.update();
+
+ loaded = hasAnyArtifact; // update cached loaded flag
+ servoControl.setLoaded(loaded); // switch servo gains based on load state
+ servoControl.update(); // run one PD control iteration
}
- // slot geometry
+ // -----------------------------------------------------------------------
+ // Private: slot geometry
+ // -----------------------------------------------------------------------
+
+ /**
+ * Computes the physical drum angle (degrees) that aligns the given slot
+ * with the active sensing/shooting position, accounting for the current mode.
+ *
+ * @param s the slot whose center angle to compute
+ * @return drum angle in [0, 360°) for the given slot in the current mode
+ */
private double getSlotCenterAngle(IndexerState s) {
- double angle = s.index * SLOT_SPACING_DEG;
- angle += offsetAngle;
+ double angle = s.index * SLOT_SPACING_DEG; // base angle: 0°, 120°, or 240°
+ angle += offsetAngle; // apply calibration zero-offset
if (autoOuttaking) {
- angle += autoOuttakeOffsetAngle; // NOT 180 unless confirmed mechanically
+ angle += autoOuttakeOffsetAngle; // autonomous outtake uses a separate offset
}
else if (!intaking) {
- angle += outtakeOffsetAngle; // NOT 180 unless confirmed mechanically
+ angle += outtakeOffsetAngle; // teleop outtake: rotate ~186° to face shooter
}
+ // In intaking mode no additional offset is applied (sensor is at 0 + offsetAngle).
- return mod(angle, 360.0);
+ return mod(angle, 360.0); // wrap to [0, 360) so angles are unambiguous
}
+ // -----------------------------------------------------------------------
+ // Colour assignment (manual override)
+ // -----------------------------------------------------------------------
+
+ /**
+ * Manually assigns a colour to a specific slot and resets its observation
+ * history. Used by autonomous routines where the pre-loaded colours are known,
+ * so the sensor does not need to re-classify.
+ *
+ * @param slotState the slot to assign
+ * @param color the colour to store in that slot's memory
+ */
public void assignSlotColor(IndexerState slotState, ArtifactColor color) {
SlotState slot = slot(slotState);
- // Assign color
- slot.color = color;
+ slot.color = color; // set the stored colour directly, bypassing the sensor
- // Reset observations so classifier doesn't fight this
- slot.obs.reset();
+ slot.obs.reset(); // clear sensor observations so the classifier doesn't override this assignment
- // Update empty tracking
+ // Update the empty-tracking latch based on the new assigned colour.
boolean isEmpty = (color == ArtifactColor.EMPTY || color == ArtifactColor.UNKNOWN);
- slot.wasEmpty = isEmpty;
- slot.fillingHits = 0;
- slot.fillCycleActive = false;
+ slot.wasEmpty = isEmpty; // true for empty/unknown; false for green/purple
+ slot.fillingHits = 0; // clear fill counter
+ slot.fillCycleActive = false; // deactivate fill cycle
- recomputeNoEmpty();
+ recomputeNoEmpty(); // update the full flag with the new assignment
}
+ // -----------------------------------------------------------------------
+ // Private: angle helpers
+ // -----------------------------------------------------------------------
+
+ /**
+ * Computes the absolute angular error (shortest path) between two angles.
+ *
+ * @param a first angle in degrees
+ * @param b second angle in degrees
+ * @return absolute angular difference in [0, 180°]
+ */
private double angleError(double a, double b) {
+ // Shift by 180°, take mod 360°, subtract 180° to fold into [-180, 180], then abs.
return Math.abs(mod(a - b + 180.0, 360.0) - 180.0);
}
- // classification + auto-advance
+ // -----------------------------------------------------------------------
+ // Private: classification and telemetry
+ // -----------------------------------------------------------------------
+
+ /**
+ * Reads the colour sensor and records one observation for each slot that is
+ * currently within {@link #SLOT_ASSIGN_TOLERANCE} of the sensor position.
+ * After accumulating {@link #MIN_HITS_FOR_DECISION} samples, resolves the
+ * probable colour and updates the slot's memory.
+ *
+ *
+ *
+ *
+ * @param desired the colour we want to fire
+ * @param slotColor the colour stored in the candidate slot
+ * @return integer score in {0, 60, 80, 100}
+ */
// gives preferences
private int scoreSlotForTarget(ArtifactColor desired, ArtifactColor slotColor) {
- if (slotColor == desired) return 100;
+ if (slotColor == desired) return 100; // perfect match; highest priority
if (desired == ArtifactColor.GREEN) {
- if (slotColor == ArtifactColor.UNKNOWN) return 80;
- if (slotColor == ArtifactColor.PURPLE) return 60;
- if (slotColor == ArtifactColor.EMPTY) return 0;
+ if (slotColor == ArtifactColor.UNKNOWN) return 80; // might be green; take a chance
+ if (slotColor == ArtifactColor.PURPLE) return 60; // wrong colour but non-empty; use as fallback
+ if (slotColor == ArtifactColor.EMPTY) return 0; // no artifact; skip
}
if (desired == ArtifactColor.PURPLE) {
- if (slotColor == ArtifactColor.UNKNOWN) return 80;
- if (slotColor == ArtifactColor.GREEN) return 60;
- if (slotColor == ArtifactColor.EMPTY) return 0;
+ if (slotColor == ArtifactColor.UNKNOWN) return 80; // might be purple; take a chance
+ if (slotColor == ArtifactColor.GREEN) return 60; // wrong colour but non-empty; use as fallback
+ if (slotColor == ArtifactColor.EMPTY) return 0; // no artifact; skip
}
- return 0;
+ return 0; // default: this slot has no value for the desired colour
}
+ /**
+ * Returns {@code true} if the drum is currently within {@code toleranceDeg} degrees
+ * of its commanded slot center. Used to confirm that the servo has settled at
+ * the target position before performing an operation that requires alignment.
+ *
+ * @param toleranceDeg maximum acceptable angular error in degrees
+ * @return {@code true} if the drum is settled at the current target slot
+ */
public boolean isWithinTargetDegrees(double toleranceDeg) {
- double measured = getMeasuredAngle();
- double target = getSlotCenterAngle(state);
- return angleError(measured, target) <= toleranceDeg;
+ double measured = getMeasuredAngle(); // read current drum angle
+ double target = getSlotCenterAngle(state); // compute the expected angle for the current slot
+ return angleError(measured, target) <= toleranceDeg; // true if error is within tolerance
}
+ // -----------------------------------------------------------------------
+ // Private math helpers
+ // -----------------------------------------------------------------------
+
+ /** Clamps {@code v} to the range [min, max]. */
private double clamp(double v, double min, double max) {
return Math.max(min, Math.min(max, v));
}
+ /**
+ * Positive modulo: always returns a non-negative result in [0, m).
+ * Java's {@code %} can return negative values for negative operands.
+ */
private double mod(double v, double m) {
double r = v % m;
- return r < 0 ? r + m : r;
+ return r < 0 ? r + m : r; // shift negative remainder into [0, m)
}
+ /**
+ * Convenience accessor: returns the {@link SlotState} for the given slot index.
+ * @param s the slot whose state to retrieve
+ * @return the SlotState object for slot {@code s}
+ */
private SlotState slot(IndexerState s) { return slots[s.index]; }
+ // -----------------------------------------------------------------------
+ // Private quickspin helpers
+ // -----------------------------------------------------------------------
+
+ /**
+ * Returns {@code true} if the array contains exactly 2 PURPLE and 1 GREEN,
+ * which is the only valid firing mix for a quickspin outtake in this game.
+ *
+ * @param arr a 3-element colour array
+ * @return {@code true} if the array is valid (2P + 1G, no other colours)
+ */
//quickspin helper
private boolean isTwoPurpleOneGreen(ArtifactColor[] arr) {
int purple = 0;
@@ -585,11 +1283,17 @@ private boolean isTwoPurpleOneGreen(ArtifactColor[] arr) {
for (ArtifactColor c : arr) {
if (c == ArtifactColor.PURPLE) purple++;
else if (c == ArtifactColor.GREEN) green++;
- else return false;
+ else return false; // any other colour (EMPTY, UNKNOWN) makes this invalid
}
- return purple == 2 && green == 1;
+ return purple == 2 && green == 1; // must be exactly 2 purple and 1 green
}
+ /**
+ * Returns {@code true} if the drum's stored slot colours are valid for a
+ * quickspin (all three slots must contain exactly 2 PURPLE + 1 GREEN).
+ *
+ * @return {@code true} if the drum is loaded with the correct mix
+ */
private boolean storedSlotsValidForQuickspin() {
int purple = 0;
int green = 0;
@@ -597,21 +1301,49 @@ private boolean storedSlotsValidForQuickspin() {
for (SlotState slot : slots) {
if (slot.color == ArtifactColor.PURPLE) purple++;
else if (slot.color == ArtifactColor.GREEN) green++;
- else return false;
+ else return false; // EMPTY or UNKNOWN slots disqualify quickspin
}
- return purple == 2 && green == 1;
+ return purple == 2 && green == 1; // valid only if exactly 2P + 1G are stored
}
+ /**
+ * Checks whether starting a quickspin at position {@code x} would produce
+ * the desired firing order.
+ *
+ *
+ *
+ * All four motors use {@link DcMotor.ZeroPowerBehavior#BRAKE} so the robot stops
+ * crisply when the driver releases the sticks.
*/
public class Movement {
- private final DcMotor leftFront, leftBack, rightFront, rightBack;
+
+ // -----------------------------------------------------------------------
+ // Hardware
+ // -----------------------------------------------------------------------
+
+ /** Left-front mecanum wheel motor. */
+ private final DcMotor leftFront;
+
+ /** Left-rear mecanum wheel motor. */
+ private final DcMotor leftBack;
+
+ /** Right-front mecanum wheel motor. */
+ private final DcMotor rightFront;
+
+ /** Right-rear mecanum wheel motor. */
+ private final DcMotor rightBack;
+
+ /** Road Runner MecanumDrive reference used for field-centric heading reads
+ * and for updating the pose estimate via the localizer. */
private final MecanumDrive drive;
- //private final IMU imu;
- private final double STRAFE_MULTIPLIER = 1.0, ROTATION_MULTIPLIER = 0.8;
+
+ // IMU is currently unused (commented out) because the bot heading is
+ // obtained from the Road Runner localizer instead.
+ // private final IMU imu;
+
+ // -----------------------------------------------------------------------
+ // Scaling constants
+ // -----------------------------------------------------------------------
+
+ /** Multiplier applied to both the forward/axial and strafe/lateral
+ * components. A value of 1.0 means full stick deflection produces
+ * full motor power in those axes. */
+ private final double STRAFE_MULTIPLIER = 1.0;
+
+ /** Multiplier applied to the rotational/yaw component. Reduced to 0.8 to
+ * make turning slightly less sensitive than strafing/driving. */
+ private final double ROTATION_MULTIPLIER = 0.8;
+
+ // -----------------------------------------------------------------------
+ // Constructor
+ // -----------------------------------------------------------------------
/**
- * Initializes a Movement instance.
- * @param map {@link com.qualcomm.robotcore.hardware.HardwareMap}
+ * Initialises the Movement subsystem by fetching all four drive motors from
+ * the hardware map, configuring their directions, and setting their zero-
+ * power behavior to BRAKE.
+ *
+ * @param map the robot's hardware map, non-null
+ * @param drive the Road Runner MecanumDrive used to read heading for
+ * field-centric mode and to keep the pose estimate updated
*/
public Movement(@NonNull HardwareMap map, MecanumDrive drive){
- leftFront = map.get(DcMotor.class, "leftFront");
- leftBack = map.get(DcMotor.class, "leftBack");
+ // Retrieve each motor by the name registered in the hardware configuration.
+ leftFront = map.get(DcMotor.class, "leftFront");
+ leftBack = map.get(DcMotor.class, "leftBack");
rightFront = map.get(DcMotor.class, "rightFront");
- rightBack = map.get(DcMotor.class, "rightBack");
+ rightBack = map.get(DcMotor.class, "rightBack");
- this.drive = drive;
- //this.imu = drive.lazyImu.get();
+ this.drive = drive; // store Road Runner drive for localizer access
+ // this.imu = drive.lazyImu.get(); // IMU path: currently disabled
+ // Reverse left-side motors so that all motors spin "forward" with a
+ // positive power value, given the physical wiring of this robot.
leftFront.setDirection(DcMotorSimple.Direction.REVERSE);
leftBack.setDirection(DcMotorSimple.Direction.REVERSE);
rightFront.setDirection(DcMotorSimple.Direction.FORWARD);
rightBack.setDirection(DcMotorSimple.Direction.FORWARD);
+ // Enable BRAKE mode on all motors so the robot holds its position when
+ // power is cut, instead of coasting unpredictably.
leftFront.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
leftBack.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
rightFront.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
rightBack.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
}
- // tick for teleop
+ // -----------------------------------------------------------------------
+ // Teleop drive methods
+ // -----------------------------------------------------------------------
+ /**
+ * Robot-centric mecanum drive tick called every loop iteration during teleop.
+ *
+ *
+ *
+ *
+ *
+ *
+ *
+ * @param x desired power (POWER mode) or desired RPM (RPM mode)
+ */
public void set(double x) {
if (mode == Mode.POWER) {
+ // Clamp power to [0, 1] so the motors are never commanded backward
+ // (flywheels should only spin in one direction).
motorPower = clamp(x, 0.0, 1.0);
} else {
+ // RPM mode: if the target changes by more than 25 RPM we treat it
+ // as a new spin-up request and reset the timing state.
if (Math.abs(x - targetRPM) > 25) {
- spinupStartTime = -1;
- inRangeStartTime = -1;
+ spinupStartTime = -1; // mark spin-up as not started
+ inRangeStartTime = -1; // mark "in range" timer as not started
}
- targetRPM = x;
+ targetRPM = x; // store the new target RPM for use in periodic()
}
}
+ // -----------------------------------------------------------------------
+ // Getters
+ // -----------------------------------------------------------------------
+ /**
+ * Returns the most recently measured flywheel speed in RPM.
+ * Updated each call to {@link #periodic()}.
+ *
+ * @return current flywheel RPM (may lag real speed by one control cycle)
+ */
public double getRPM() { return currentRPM; }
+
+ /**
+ * Returns the currently commanded target RPM.
+ *
+ * @return target RPM (0 when stopped)
+ */
public double getTargetRPM() { return targetRPM; }
+
+ /**
+ * Returns the last motor power written to the flywheels.
+ *
+ * @return motor power in the range [-1.0, 1.0] (POWER mode) or the PIDF
+ * output clamped to [-1.0, 1.0] (RPM mode)
+ */
public double getPower() { return motorPower; }
+ // -----------------------------------------------------------------------
+ // Control loop
+ // -----------------------------------------------------------------------
+
+ /**
+ * Must be called once per robot loop iteration. Reads the current RPM
+ * from the encoder, computes PIDF output in RPM mode (or open-loop in
+ * POWER mode), and writes the resulting power to both flywheel motors.
+ */
public void periodic() {
- // Update current RPM from motor encoder
+ // Compute current RPM from the primary motor's raw encoder velocity.
+ // getVelocity() returns ticks/second; divide by TPR and multiply by 60
+ // to get rotations per minute.
currentRPM = shooter.getVelocity() / TPR * 60.0;
if (mode == Mode.POWER) {
- // Open-loop mode
+ // Open-loop path: simply write the stored motorPower to both motors.
shooter.set(motorPower);
shooter2.set(motorPower);
- return;
+ return; // skip PIDF calculation entirely
}
- // Update PID gains live from dashboard
+ // Sync PID gains from dashboard-editable static fields so changes take
+ // effect immediately without redeploying code.
controller.setPID(p, i, d);
- // Compute PID term
+ // PID term: controller compares current RPM to target and outputs a
+ // correction value (positive = need to speed up, negative = overspun).
double pid = controller.calculate(currentRPM, targetRPM);
- // Compute feedforward term from earlier code
+ // Feedforward term: gives the motor a head-start power proportional to
+ // the target RPM, reducing the burden on the P term.
double ff = targetRPM * f;
- // Combined PIDF output
+ // Sum PID and feedforward for the combined PIDF output.
motorPower = pid + ff;
- // Constrain power
+ // Clamp combined output to valid motor power range.
motorPower = clamp(motorPower, -1.0, 1.0);
+ // Apply the same power to both flywheels so they stay synchronised.
shooter.set(motorPower);
shooter2.set(motorPower);
}
+ // -----------------------------------------------------------------------
+ // Range-to-RPM regression helpers
+ // -----------------------------------------------------------------------
+
+ /**
+ * Quartic polynomial regression mapping target range (inches) to shooter RPM.
+ * Derived by fitting a degree-4 polynomial to the full empirical data set.
+ * Kept for reference; the cubic version is currently used in production.
+ *
+ * @param range distance from robot to goal in inches
+ * @return predicted optimal shooter RPM for that range
+ */
private double quarticRegressionRPM(double range) {
+ // Evaluate: -0.000297337·r⁴ + 0.0958661·r³ - 11.09971·r² + 562.06918·r - 6981.95351
return range * (range * (range * (range * -0.000297337 + 0.0958661) - 11.09971) + 562.06918) - 6981.95351;
}
+ /**
+ * Cubic polynomial regression mapping target range (inches) to shooter RPM.
+ * Fits a degree-3 polynomial to the empirical data set.
+ * This is the function currently used by {@link #getRegressionRPM(double)}.
+ *
+ * @param range distance from robot to goal in inches
+ * @return predicted optimal shooter RPM for that range
+ */
private double cubicRegressionRPM(double range) {
+ // Evaluate: -0.000281754·r³ + 0.228245·r² - 13.14333·r + 3623.28132
return range * (range * (range * -0.000281754 + 0.228245) - 13.14333) + 3623.28132;
}
+ /**
+ * Performs piecewise linear interpolation over a given data table to map
+ * range (x) to RPM (y). Out-of-bounds ranges are clamped to the nearest
+ * end segment (extrapolation via the first/last interval).
+ *
+ * @param range distance from robot to goal in inches
+ * @param data two-column array of {range, RPM} key points, sorted ascending
+ * by range
+ * @return linearly-interpolated RPM value
+ */
private double linearInterpolation(double range, double[][] data) {
double sum = 0;
for (int i = 0; i < data.length - 1; i++) {
+ // For the first segment, extend left to -∞; for the last, extend right to +∞.
double min = i == 0 ? Double.MIN_VALUE : data[i][0];
double max = i == data.length - 2 ? Double.MAX_VALUE : data[i + 1][0];
+ // If range falls in this interval, compute and accumulate the interpolated value.
if (range >= min && range < max) sum +=
- (range - data[i][0]) / (data[i + 1][0] - data[i][0]) *
- (data[i + 1][1] - data[i][1]) + data[i][1];
+ (range - data[i][0]) / (data[i + 1][0] - data[i][0]) * // fractional position in interval
+ (data[i + 1][1] - data[i][1]) + data[i][1]; // interpolated RPM
}
- return sum;
+ return sum; // only one segment fires, so sum equals that segment's result
}
+ /**
+ * Linear interpolation using the full {@link #REGRESSION_DATA} table.
+ *
+ * @param range distance from robot to goal in inches
+ * @return interpolated optimal shooter RPM
+ */
private double linearInterpolationRegressionRPM(double range) {
return linearInterpolation(range, REGRESSION_DATA);
}
+ /**
+ * Linear interpolation using the sparse {@link #REGRESSION_DATA_REDUCED} table.
+ * Faster to compute than the full-table version but less precise between key points.
+ *
+ * @param range distance from robot to goal in inches
+ * @return interpolated optimal shooter RPM (reduced accuracy)
+ */
private double linearInterpolationRegressionReducedRPM(double range) {
return linearInterpolation(range, REGRESSION_DATA_REDUCED);
}
+ /**
+ * Public facade: returns the recommended shooter RPM for a given target range.
+ * Validates the range and falls back to {@link #INTAKE_MIN_RPM} for invalid
+ * or zero-distance inputs. Currently delegates to the cubic polynomial.
+ *
+ * @param range distance from robot to goal in inches; must be > 0 and finite
+ * @return optimal shooter RPM, or {@link #INTAKE_MIN_RPM} if range is invalid
+ */
public double getRegressionRPM(double range)
{
if (Double.isNaN(range) || range <= 0) {
+ // No valid range data available (tag not visible, or called too early);
+ // return a safe default RPM so the shooter is still usable.
return INTAKE_MIN_RPM;
}
- // Just use one of the three functions above
+ // Delegate to the cubic polynomial regression (best single-function fit).
return cubicRegressionRPM(range);
}
- // Within the range and has been in range for spinupInRangeMinTime
+ // -----------------------------------------------------------------------
+ // Spin-up readiness check
+ // -----------------------------------------------------------------------
+
+ /**
+ * Returns {@code true} when the flywheels are considered ready to shoot.
+ *
+ *
+ *
+ *
+ *
+ *
+ *
+ *
+ *
+ *
+ *
+ *
+ */
public void update() {
- if (manualOverride) return; // skip closed-loop when in override
+ if (manualOverride) return; // open-loop active: skip all closed-loop logic
- updateContinuousAngle();
+ updateContinuousAngle(); // integrate any wrap-arounds since the last call
- double error = targetDeg - continuousDeg;
+ // Compute the signed angular error (continuous units).
+ double error = targetDeg - continuousDeg;
double absErr = Math.abs(error);
if (absErr < deadbandDeg) {
+ // Within deadband: stop the servo (no restoring force needed).
servo.setPower(0);
+ // Reset derivative state so the next non-deadband motion starts clean.
lastAngleDeg = continuousDeg;
- lastTimeNs = System.nanoTime();
+ lastTimeNs = System.nanoTime();
return;
}
- long now = System.nanoTime();
- double dt = (now - lastTimeNs) * 1e-9;
- if (dt <= 0) dt = 1e-3;
+ // Compute time delta for derivative calculation.
+ long now = System.nanoTime();
+ double dt = (now - lastTimeNs) * 1e-9; // convert nanoseconds to seconds
+ if (dt <= 0) dt = 1e-3; // guard against zero/negative dt
- double velocity = (continuousDeg - lastAngleDeg) / dt; // deg/s
+ // Compute shaft angular velocity (degrees per second).
+ double velocity = (continuousDeg - lastAngleDeg) / dt;
+ // Update derivative state for next iteration.
lastAngleDeg = continuousDeg;
- lastTimeNs = now;
+ lastTimeNs = now;
+ // PD output: proportional term drives toward target; derivative damps velocity.
+ // stiffnessGain scales the P term to allow a stiffer hold under load.
double output = kP * error * stiffnessGain - kD * velocity + kI * error;
- // static friction compensation toward target
+ // Static friction compensation: if the output is in the same direction as
+ // the error (we need to move that way), ensure it is at least kS so the
+ // servo actually starts moving despite gearbox stiction.
if (Math.signum(output) == Math.signum(error)) {
double sign = Math.signum(output);
output = sign * Math.max(Math.abs(output), kS);
}
- output = clamp(output, -maxPower, maxPower);
- servo.setPower(output);
+ output = clamp(output, -maxPower, maxPower); // hard-limit to maxPower
+ servo.setPower(output); // write computed power to hardware
}
+ // -----------------------------------------------------------------------
+ // Position commands
+ // -----------------------------------------------------------------------
+
+ /**
+ * Commands the servo to move to a wrapped (0–360°) target angle using
+ * the preferred rotation direction ({@link #rotateClockwise}).
+ *
+ *
+ *
+ *
+ *
+ *
+ *
+ * @param rgba the RGBA sample to classify
+ * @return classified {@link Indexer.ArtifactColor}
+ */
private Indexer.ArtifactColor classifyColorOnlyFromRGBA(NormalizedRGBA rgba) {
- float[] hsv = rgbToHsv(rgba.red, rgba.green, rgba.blue);
- float h = hsv[0];
- float s = hsv[1];
- float v = hsv[2];
+ float[] hsv = rgbToHsv(rgba.red, rgba.green, rgba.blue); // convert to HSV
+ float h = hsv[0]; // hue in degrees
+ float s = hsv[1]; // saturation [0, 1]
+ float v = hsv[2]; // value/brightness [0, 1]
+ // Quality gate: reject achromatic or very dark readings.
if (s < MIN_SATURATION || v < MIN_VALUE) {
- return Indexer.ArtifactColor.UNKNOWN;
+ return Indexer.ArtifactColor.UNKNOWN; // colour is too grey or too dark to trust
}
+ // Check whether the hue falls in the green range.
if (inHueRange(h, GREEN_H_MIN, GREEN_H_MAX)) {
return Indexer.ArtifactColor.GREEN;
}
+ // Check whether the hue falls in the purple range.
if (inHueRange(h, PURPLE_H_MIN, PURPLE_H_MAX)) {
return Indexer.ArtifactColor.PURPLE;
}
- return Indexer.ArtifactColor.UNKNOWN;
+ return Indexer.ArtifactColor.UNKNOWN; // hue did not match any known colour range
}
- //
+ /**
+ * Returns {@code true} if {@code hueDeg} falls within the circular hue
+ * range [minDeg, maxDeg]. Handles wrap-around ranges that cross 0°/360°
+ * (e.g., red hues near 0°/360°).
+ *
+ * @param hueDeg hue angle in degrees to test
+ * @param minDeg start of the hue range (inclusive)
+ * @param maxDeg end of the hue range (inclusive)
+ * @return {@code true} if the hue is within the specified range
+ */
private boolean inHueRange(float hueDeg, float minDeg, float maxDeg) {
- hueDeg = wrapHue(hueDeg);
+ hueDeg = wrapHue(hueDeg); // normalize input to [0, 360)
minDeg = wrapHue(minDeg);
maxDeg = wrapHue(maxDeg);
if (minDeg <= maxDeg) {
+ // Non-wrapping range: simply check if hue is between min and max.
return hueDeg >= minDeg && hueDeg <= maxDeg;
} else {
- // wrap-around (e.g., 300..30)
+ // Wrap-around range (e.g., 300–30°): hue is in range if ≥ min OR ≤ max.
return hueDeg >= minDeg || hueDeg <= maxDeg;
}
}
+ /**
+ * Wraps a hue value into [0, 360).
+ *
+ * @param h hue angle in degrees (any value)
+ * @return equivalent hue in [0, 360)
+ */
private float wrapHue(float h) {
- h %= 360f;
- if (h < 0) h += 360f;
+ h %= 360f; // reduce to (-360, 360)
+ if (h < 0) h += 360f; // shift negatives into [0, 360)
return h;
}
- //Normalised RGB to HSV conversion
+ /**
+ * Converts normalized RGB to HSV colour space.
+ *
+ *
+ *
+ *
+ * @param r normalized red channel [0, 1]
+ * @param g normalized green channel [0, 1]
+ * @param b normalized blue channel [0, 1]
+ * @return {@code float[]{ H, S, V }} where H ∈ [0, 360), S ∈ [0, 1], V ∈ [0, 1]
+ */
+ // Normalized RGB to HSV conversion
private float[] rgbToHsv(float r, float g, float b) {
- float max = Math.max(r, Math.max(g, b));
- float min = Math.min(r, Math.min(g, b));
- float delta = max - min;
+ float max = Math.max(r, Math.max(g, b)); // largest channel = Value
+ float min = Math.min(r, Math.min(g, b)); // smallest channel
+ float delta = max - min; // chroma = range of the three channels
float h;
if (delta < 1e-6f) {
+ // All channels equal → achromatic grey; hue is undefined, use 0.
h = 0f;
} else if (max == r) {
+ // Red is dominant channel: hue in [−60°, 60°) relative to red.
h = 60f * (((g - b) / delta) % 6f);
} else if (max == g) {
+ // Green is dominant: hue in [60°, 180°).
h = 60f * (((b - r) / delta) + 2f);
} else {
+ // Blue is dominant: hue in [180°, 300°).
h = 60f * (((r - g) / delta) + 4f);
}
- if (h < 0f) h += 360f;
+ if (h < 0f) h += 360f; // ensure hue is non-negative
- float s = (max <= 1e-6f) ? 0f : (delta / max);
- float v = max;
+ float s = (max <= 1e-6f) ? 0f : (delta / max); // saturation; 0 for near-black
+ float v = max; // value = brightness = max channel
return new float[]{ h, s, v };
}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/indexerUtil/SlotObservation.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/indexerUtil/SlotObservation.java
index 479fd018eb3a..c3049bff0198 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/indexerUtil/SlotObservation.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/indexerUtil/SlotObservation.java
@@ -2,51 +2,150 @@
import org.firstinspires.ftc.teamcode.subsystems.Indexer;
+/**
+ * SlotObservation maintains a running tally of colour classifications
+ * recorded by the colour sensor while the indexer is positioned at a
+ * particular slot.
+ *
+ *
+ *
+ *
+ *
+ *
+ *
+ *
+ *
+ *
+ *
+ *
+ *
+ * @return {@code double[]{turnPower, range, bearing}} where:
+ *
+ *
+ */
public double[] calculateLocalizedTurnPower() {
+ // Get the robot's current estimated field pose from the Road Runner localizer.
Pose2d robotPose = drive.localizer.getPose();
- // Vector from robot -> tag in field coordinates
+ // 2-D vector from robot to goal tag (field coordinates, inches).
double dx = tagPose.position.x - robotPose.position.x;
double dy = tagPose.position.y - robotPose.position.y;
+ // Horizontal (ground-plane) distance from robot to goal.
double horizontalDistance = Math.hypot(dx, dy);
+ // Height difference between the goal tag and the camera mounting height.
double dz = goalAprilTagHeight - cameraHeight;
- // point-to-point distance=
+ // 3-D straight-line distance: combines horizontal and vertical separation.
double range = Math.hypot(horizontalDistance, dz);
+ // Desired robot heading to face the goal (radians, field frame).
double desiredHeading = Math.atan2(dy, dx);
+ // Current robot heading from the localizer (radians, field frame).
double currentHeading = robotPose.heading.toDouble();
+ // Bearing = angular difference between desired and current heading (degrees).
double bearing = Math.toDegrees(desiredHeading - currentHeading);
- bearing = angleWrapDegrees(bearing);
+ bearing = angleWrapDegrees(bearing); // wrap to [-180, 180]
+ // Compute the PID turn power (negated so positive bearing → positive turn output).
double turnPower = -calculateTurnPowerFromBearing(bearing);
return new double[]{turnPower, range, bearing};
}
+ // -----------------------------------------------------------------------
+ // Angle helpers
+ // -----------------------------------------------------------------------
+
+ /**
+ * Wraps a degree angle into the range (-180, 180].
+ *
+ *
+ *
+ *
+ *
+ *
+ */
// TODO IMPORTANT NOTES: For goalTagID, just have separate teleops one for red alliance one for blue where blue teleop can setGoalTagID(20) and red teleop can setGoalTagID(24)
// TODO We will see whether we want separate auto for either alliance, probably yes its just easier that way and there may be some functionality requiring that.
public class AprilTag {
+
+ // -----------------------------------------------------------------------
+ // Scan results (updated by scan methods)
+ // -----------------------------------------------------------------------
+
+ /** ID of the last tag seen by {@link #scanGoalTag()}.
+ * {@code -1} if no goal tag has been detected yet. */
private int id = -1;
+
+ /** ID of the last obelisk tag seen by {@link #scanObeliskTag()}.
+ * Shared across all instances (static) so it can be read globally.
+ * {@code -1} if no obelisk tag has been detected. */
public static int obeliskId = -1;
+
+ /** ID of the target alliance goal tag. Set externally before scanning
+ * so the scan methods can filter for the correct tag. */
private int goalTagID; // our current alliance goal
+
+ /** The most recently detected goal tag ID. May differ from {@link #goalTagID}
+ * if the camera sees a tag other than the intended goal. */
private int cameraScannedId;
+
+ /** Horizontal bearing to the goal tag in degrees (positive = tag is to the
+ * right of the camera center; negative = to the left). */
private double bearing;
+
+ /** Vertical elevation angle of the goal tag in degrees above the camera
+ * horizontal (positive = tag is above camera center). */
private double elevation;
+
+ /** Straight-line distance from the camera to the goal tag in inches,
+ * computed from the elevation angle and known heights. */
private double range;
+
+ /** Pixel area fraction of the goal tag in the camera frame.
+ * Larger = closer; used as a rough confidence metric. */
private double tagSize;
+
+ // -----------------------------------------------------------------------
+ // Hardware
+ // -----------------------------------------------------------------------
+
+ /** The Limelight 3A co-processor hardware object, mapped to config name
+ * {@code "limelight"}. */
private final Limelight3A limelight;
+
+ /** FTC telemetry reference (currently unused but reserved for debug output). */
private final Telemetry telemetry;
- private final double LIMELIGHT_HEIGHT = 11.815;
- private final double LIMELIGHT_ANGLE = 15;
- private final double TARGET_HEIGHT = 29.5;
+ // -----------------------------------------------------------------------
+ // Camera / target geometry constants
+ // -----------------------------------------------------------------------
+
+ /** Height of the Limelight camera above the floor in inches. */
+ private final double LIMELIGHT_HEIGHT = 11.815; // inches
+
+ /** Upward tilt angle of the Limelight camera from horizontal in degrees.
+ * Added to the tag's reported elevation when computing the range. */
+ private final double LIMELIGHT_ANGLE = 15; // degrees above horizontal
+
+ /** Height of the goal AprilTag above the floor in inches. */
+ private final double TARGET_HEIGHT = 29.5; // inches
+ // -----------------------------------------------------------------------
+ // Constructor
+ // -----------------------------------------------------------------------
+
+ /**
+ * Creates an AprilTag instance, retrieves the Limelight from the hardware
+ * map, switches to pipeline 0 (blue goal), and starts the vision pipeline.
+ *
+ * @param hardwareMap the robot's hardware map used to find the Limelight
+ * device registered under the config name {@code "limelight"}
+ * @param telemetry the FTC telemetry object (reserved for future debug output)
+ */
public AprilTag(HardwareMap hardwareMap, Telemetry telemetry) {
this.telemetry = telemetry;
+ // Retrieve the Limelight co-processor from the hardware map.
limelight = hardwareMap.get(Limelight3A.class, "limelight");
- limelight.pipelineSwitch(0);
- limelight.start();
+ limelight.pipelineSwitch(0); // default to pipeline 0 (blue alliance goal)
+ limelight.start(); // begin streaming and processing frames
}
+ // -----------------------------------------------------------------------
+ // Limelight power control
+ // -----------------------------------------------------------------------
+
+ /**
+ * Starts or stops the Limelight pipeline.
+ * Stopping it when not in use conserves battery and reduces CPU load.
+ *
+ * @param bool {@code true} to start the pipeline; {@code false} to stop it
+ */
public void toggle(boolean bool) {
- if (bool) { limelight.start(); }
- else { limelight.stop(); }
+ if (bool) { limelight.start(); } // resume vision processing
+ else { limelight.stop(); } // pause vision processing
}
+ // -----------------------------------------------------------------------
+ // Scan methods
+ // -----------------------------------------------------------------------
+
+ /**
+ * Scans for the obelisk AprilTag (pipeline 2) and stores the detected
+ * tag ID in {@link #obeliskId}.
+ *
+ *
+ * range = (TARGET_HEIGHT - LIMELIGHT_HEIGHT) / sin(elevation + LIMELIGHT_ANGLE)
+ *
+ * where all angles are in degrees.
+ *
+ * @param elevation vertical angle of the tag above camera centre (degrees)
+ * @return slant distance from camera to tag in inches
+ */
private double calculateDistance(double elevation) {
+ // sin() of the total angle from horizontal to the tag gives the vertical component;
+ // dividing the known height difference yields the slant distance.
return (TARGET_HEIGHT - LIMELIGHT_HEIGHT) / Math.sin(Math.toRadians(elevation + LIMELIGHT_ANGLE));
}
+ // -----------------------------------------------------------------------
+ // Goal tag scan
+ // -----------------------------------------------------------------------
+
+ /**
+ * Scans for the alliance goal AprilTag and updates {@link #bearing},
+ * {@link #elevation}, {@link #range}, and {@link #tagSize}.
+ *
+ *
+ *
+ *
+ * @param pipeline pipeline index to activate (0, 1, or 2)
+ */
public void setPipeline(int pipeline) {
// 0 blue, 1 red, 2 obelisk
- limelight.pipelineSwitch(pipeline);
+ limelight.pipelineSwitch(pipeline); // send the pipeline switch command to the co-processor
}
+
+ // -----------------------------------------------------------------------
+ // Getters
+ // -----------------------------------------------------------------------
+
+ /**
+ * Returns the ID of the most recently detected goal tag.
+ *
+ * @return AprilTag ID, or 0 if no tag has been scanned yet
+ */
public int getCurrentId() {
- return cameraScannedId;
+ return cameraScannedId; // last goal tag ID seen by the camera
}
+
+ /**
+ * Returns the ID of the most recently detected obelisk tag.
+ * This is a static field shared across all instances.
+ *
+ * @return obelisk AprilTag ID (21, 22, or 23 for valid motif tags),
+ * or {@code -1} if none detected
+ */
public int getObeliskId() {
- return obeliskId;
+ return obeliskId; // shared static field; same value regardless of which instance reads it
}
+
+ /**
+ * Returns the vertical elevation of the goal tag in degrees above the
+ * camera's horizontal axis.
+ *
+ * @return elevation angle in degrees; {@link Double#NaN} if no tag detected
+ */
public double getElevation() {
- return elevation;
+ return elevation; // degrees above camera centre; used to compute range
}
+
+ /**
+ * Returns the computed slant range to the goal tag in inches.
+ *
+ * @return range in inches; {@link Double#NaN} if no tag detected
+ */
public double getRange() {
- return range;
+ return range; // slant distance from camera to goal tag (inches)
}
+
+ /**
+ * Returns the horizontal bearing to the goal tag in degrees.
+ * Positive = tag is to the right of the camera; negative = to the left.
+ *
+ * @return bearing in degrees; {@link Double#NaN} if no tag detected
+ */
public double getBearing() {
- return bearing;
+ return bearing; // horizontal angle from camera centre to goal tag (degrees)
}
+
+ /**
+ * Returns the fractional pixel area of the last detected goal tag.
+ * A larger value indicates the robot is closer to the tag.
+ *
+ * @return tag area fraction (0.0–100.0); 0 if no tag detected
+ */
public double getArea() {
- return tagSize;
+ return tagSize; // tag pixel area as a fraction of the frame; proxy for proximity
}
+
+ /**
+ * Manually overrides the cached camera-scanned goal tag ID.
+ * Useful during testing or when the obelisk tag ID is known in advance.
+ *
+ * @param i the tag ID to store as the current camera-scanned ID
+ */
public void setCurrentCameraScannedId(int i) {
- cameraScannedId = i;
+ cameraScannedId = i; // override the last detected goal tag ID
}
}
\ No newline at end of file
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/limelight/InertiaAutoAim.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/limelight/InertiaAutoAim.java
index d7f6b11467d9..c53f35c6afd6 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/limelight/InertiaAutoAim.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/limelight/InertiaAutoAim.java
@@ -1,26 +1,108 @@
package org.firstinspires.ftc.teamcode.subsystems.limelight;
+/**
+ * InertiaAutoAim computes the yaw correction needed to hit a stationary goal
+ * when the robot itself is moving.
+ *
+ *
+ *
+ *
+ *
+ * ff = kS * sign(setpoint) + kV * setpoint // static + velocity feedforward
+ * pOut = Kp * (setpoint - measured) // proportional error correction
+ * output = clamp(ff + pOut, -1.0, 1.0) // combined, clamped to motor range
+ *
+ *
+ *
+ *
*
- * @param setpointRPM desired shooter RPM
- * @param measuredRPM current shooter RPM
- * @return motor power (-1 to 1)
+ * @param setpointRPM desired shooter flywheel speed in RPM
+ * @param measuredRPM current measured flywheel speed in RPM (from encoder)
+ * @return commanded motor power in the range [−1.0, 1.0]
*/
public double update(double setpointRPM, double measuredRPM) {
+ // RPM error: positive means we need to speed up, negative to slow down.
double error = setpointRPM - measuredRPM;
- // Feedforward
+ // Feedforward: static term ensures the motor starts moving (overcomes stiction),
+ // velocity term pre-loads the output with the approximate sustaining power.
double ff = kS * Math.signum(setpointRPM) + kV * setpointRPM;
- // P control
+ // Proportional correction on top of feedforward to close residual RPM error.
double pOut = Kp * error;
- // Clamp output
+ // Sum feedforward and P correction, then clamp to the valid motor power range.
double output = clamp(ff + pOut, -1.0, 1.0);
- return output;
+ return output; // return the combined motor power command
}
}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/ActionHost.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/ActionHost.java
index 22e85f6a0784..8cc6f969db6f 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/ActionHost.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/ActionHost.java
@@ -1,30 +1,116 @@
package org.firstinspires.ftc.teamcode.teleop;
+
import com.acmerobotics.dashboard.FtcDashboard;
import com.acmerobotics.dashboard.telemetry.TelemetryPacket;
import com.acmerobotics.roadrunner.Action;
+/**
+ * ActionHost manages the execution of a single Road Runner {@link Action} at
+ * a time in the teleop loop.
+ *
+ *
+ *
+ *
+ *
+ *
+ */
public void update() {
- if (current == null) return;
+ if (current == null) return; // no action running; nothing to do this iteration
+
+ // Create a fresh telemetry packet; the action can write debug data into it.
TelemetryPacket packet = new TelemetryPacket();
+
+ // Execute one step of the action. Returns true while still running, false when done.
boolean stillRunning = current.run(packet);
+
+ // Forward the packet to FTC Dashboard so live telemetry is visible during the action.
FtcDashboard.getInstance().sendTelemetryPacket(packet);
+
if (!stillRunning) {
- current = null;
+ current = null; // action finished; clear the host so new actions can be started
}
}
}
\ No newline at end of file
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/Bot.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/Bot.java
index 19ba45a3e5df..10ad6c6c470b 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/Bot.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/Bot.java
@@ -14,294 +14,578 @@
import org.firstinspires.ftc.teamcode.auto.roadrunner.miscRR.MecanumDrive;
import org.firstinspires.ftc.teamcode.subsystems.Indexer;
+/**
+ * Bot is the primary teleop controller for the robot.
+ *
+ *
+ *
+ *
+ *
+ *
+ *
+ *
+ *
+ */
protected void handleAllianceSelection() {
if (g1.wasJustPressed(GamepadKeys.Button.BACK)) {
- aprilTag.setPipeline(0);
+ aprilTag.setPipeline(0); // pipeline 0 = blue alliance goal tag
aimer.setBlueTarget();
+ // Set gamepad 1 LED to blue for the configured duration.
g1.gamepad.setLedColor(0, 0, 1, TeleopConstants.Gamepad.GAMEPAD_LIGHT_COLOR_DURATION);
colorGoalSelected = "Blue";
}
if (g1.wasJustPressed(GamepadKeys.Button.START)) {
- aprilTag.setPipeline(1);
+ aprilTag.setPipeline(1); // pipeline 1 = red alliance goal tag
aimer.setRedTarget();
+ // Set gamepad 1 LED to red for the configured duration.
g1.gamepad.setLedColor(1, 0, 0, TeleopConstants.Gamepad.GAMEPAD_LIGHT_COLOR_DURATION);
colorGoalSelected = "Red";
}
}
+ /**
+ * Handles driver inputs while in the {@link FSM#QuickOuttake} state.
+ *
+ *
+ *
+ */
private void handleQuickOuttakeState() {
- // Allow press-and-hold pre-spin while in QuickOuttake (before running actions)
+ // Allow press-and-hold pre-spin while waiting to confirm the dump.
if (!actionHost.isRunning()) {
if (g2.gamepad.right_bumper) {
- applyPreSpinRPM();
- rangeRequested = true;
+ applyPreSpinRPM(); // keep shooter at pre-spin RPM
+ rangeRequested = true; // keep requesting range/turn correction
} else {
rangeRequested = false;
- outtake.stop();
+ outtake.stop(); // bumper released: stop shooter
}
}
+ // X: fire the non-indexed dump action; reset rumble latch and return to Intake after.
if (!actionHost.isRunning() && g2.wasJustPressed(GamepadKeys.Button.X)) {
- actionHost.start(actionNonIndexedDump());
- rumbledAlready = false;
- state = FSM.Intake;
+ actionHost.start(actionNonIndexedDump()); // start the multi-step dump sequence
+ rumbledAlready = false; // allow re-triggering the rumble for the next fill cycle
+ state = FSM.Intake; // FSM transitions back to Intake when the action finishes
}
+
+ // DPAD_UP: re-prepare the quick-spin ordering for the current motif.
if(g2.wasJustPressed(GamepadKeys.Button.DPAD_UP))
indexer.prepareQuickspin(motif);
+
+ // BACK: abort whatever action is currently running.
if (g2.wasJustPressed(GamepadKeys.Button.BACK)) {
- actionHost.abort();
+ actionHost.abort(); // cancel the in-progress action immediately
}
+
+ // A: cancel quick outtake; re-enable intake auto-advance and return to Intake.
if (g2.wasJustPressed(GamepadKeys.Button.A)) {
state = FSM.Intake;
- indexer.setIntaking(true);
- rumbledAlready = false;
+ indexer.setIntaking(true); // re-enable auto-advance so new artifacts can be indexed
+ rumbledAlready = false; // reset rumble latch
}
}
+ /**
+ * Handles driver inputs while in the {@link FSM#SortOuttake} state.
+ *
+ *
+ *
+ */
private void handleSortOuttakeState() {
if (!actionHost.isRunning()) {
+ // X: fire the next available green artifact.
if (g2.wasJustPressed(GamepadKeys.Button.X)) {
- actionHost.start(actionFireGreen());
+ actionHost.start(actionFireGreen()); // start green-shot sequence
}
+ // Y: fire the next available purple artifact.
if (g2.wasJustPressed(GamepadKeys.Button.Y)) {
- actionHost.start(actionFirePurple());
+ actionHost.start(actionFirePurple()); // start purple-shot sequence
}
}
+ // BACK: abort any currently running shot sequence.
if (g2.wasJustPressed(GamepadKeys.Button.BACK)) {
- actionHost.abort();
+ actionHost.abort(); // cancel the in-progress shot action
}
+ // A: exit sort-outtake; re-enable intake and return to collecting.
if (g2.wasJustPressed(GamepadKeys.Button.A)) {
- indexer.setIntaking(true);
+ indexer.setIntaking(true); // re-enable auto-advance on the indexer
state = FSM.Intake;
}
}
-
+ /**
+ * Handles driver inputs while in the {@link FSM#Endgame} state.
+ *
+ *
+ *
+ */
private void handleEndgameState() {
+ // A: leave endgame and resume normal intake.
if (g2.wasJustPressed(GamepadKeys.Button.A)) {
state = FSM.Intake;
-
- indexer.setIntaking(true);
+ indexer.setIntaking(true); // re-enable auto-advance so the drum can fill again
}
}
+ // -----------------------------------------------------------------------
+ // RPM helper
+ // -----------------------------------------------------------------------
+
+ /**
+ * Commands the shooter to spin up to the "quick-spin" RPM (base target RPM
+ * scaled by {@link #QUICKSPIN_OUTTAKE_RPM_SCALE}).
+ * Called while the driver holds the right bumper to pre-heat the flywheel.
+ */
private void applyPreSpinRPM() {
- outtake.set(getTargetRPM()*QUICKSPIN_OUTTAKE_RPM_SCALE); // RPM mode: set shooter target RPM
+ outtake.set(getTargetRPM() * QUICKSPIN_OUTTAKE_RPM_SCALE); // RPM mode: set shooter target RPM
}
+ // -----------------------------------------------------------------------
+ // Action builders
+ // -----------------------------------------------------------------------
+
+ /**
+ * Builds the "non-indexed dump" Road Runner {@link Action} sequence.
+ *
+ *
+ *
+ *
+ * @return the fully-constructed dump {@link Action}
+ */
private Action actionNonIndexedDump() {
- final double rpm = getTargetRPM() * QUICKSPIN_OUTTAKE_RPM_SCALE;
+ final double rpm = getTargetRPM() * QUICKSPIN_OUTTAKE_RPM_SCALE; // pre-compute RPM to use
return new SequentialAction(
- new InstantAction(actuator::upQuick),
- new InstantAction(() -> outtake.set(rpm)),
+ new InstantAction(actuator::upQuick), // 1. raise actuator quickly
+ new InstantAction(() -> outtake.set(rpm)), // 2. start shooter spin-up
new Action() {
@Override
public boolean run(TelemetryPacket packet) {
- return !outtake.inRange(50);
+ return !outtake.inRange(50); // 3. block until RPM is within 50 of target
}
},
- new InstantAction(() -> indexer.setIndexerPower(FULL_BLAST_POWER)),
- new SleepAction(NON_INDEX_SPIN_TIME),
- new InstantAction(indexer::stopIndexerPower),
- new InstantAction(outtake::stop),
- new InstantAction(actuator::down),
- new InstantAction(() -> indexer.setIntaking(true)),
- new InstantAction(indexer::initializeColors),
- new InstantAction(() -> indexer.moveTo(Indexer.IndexerState.zero))
+ new InstantAction(() -> indexer.setIndexerPower(FULL_BLAST_POWER)), // 4. blast drum
+ new SleepAction(NON_INDEX_SPIN_TIME), // 5. timed blast duration
+ new InstantAction(indexer::stopIndexerPower), // 6. stop drum
+ new InstantAction(outtake::stop), // 7. stop shooter
+ new InstantAction(actuator::down), // 8. lower actuator
+ new InstantAction(() -> indexer.setIntaking(true)), // 9. re-enable intake
+ new InstantAction(indexer::initializeColors), // 10. mark all slots EMPTY
+ new InstantAction(() -> indexer.moveTo(Indexer.IndexerState.zero)) // 11. home indexer
);
}
+ /**
+ * Builds the "fire green artifact" Road Runner {@link Action} sequence.
+ *
+ *
+ *
+ *
+ * @return the green-shot {@link Action}, or a no-op if no green slot exists
+ */
private Action actionFireGreen() {
+ // Find the indexer slot that best matches a GREEN artifact.
final Indexer.IndexerState slot =
indexer.findBestSlotForColor(Indexer.ArtifactColor.GREEN);
if (slot == null) {
+ // No green artifact loaded: return an empty no-op action.
return new InstantAction(() -> {});
}
- final double rpm = getTargetRPM();
+ final double rpm = getTargetRPM(); // read target RPM now, not at action-run time
return new SequentialAction(
- new InstantAction(() -> indexer.setIntaking(false)),
- new InstantAction(actuator::down),
- new InstantAction(() -> indexer.moveTo(slot, true)),
- new InstantAction(() -> outtake.set(rpm)),
- // wait for shooter RPM to be within 100 instead of fixed spinup
+ new InstantAction(() -> indexer.setIntaking(false)), // 1. stop auto-advance
+ new InstantAction(actuator::down), // 2. lower actuator (clear path)
+ new InstantAction(() -> indexer.moveTo(slot, true)), // 3. rotate to green slot
+ new InstantAction(() -> outtake.set(rpm)), // 4. spin up shooter
+ // 5. wait for shooter to reach target RPM within 100 RPM
new Action() {
@Override
public boolean run(TelemetryPacket p) {
- return !outtake.inRange(100.0);
+ return !outtake.inRange(100.0); // returns true while NOT in range (keep waiting)
}
},
- new InstantAction(actuator::upIndexed),
- new SleepAction(1),
+ new InstantAction(actuator::upIndexed), // 6. raise actuator to fire position
+ new SleepAction(1), // 7. allow 1 s for artifact to exit
+ // 8. mark the fired slot as empty so the indexer knows it's clear
new InstantAction(() ->
indexer.assignSlotColor(slot, Indexer.ArtifactColor.EMPTY)
),
- new InstantAction(outtake::stop),
- new InstantAction(actuator::down)
+ new InstantAction(outtake::stop), // 9. stop shooter
+ new InstantAction(actuator::down) // 10. lower actuator back to intake pos
);
}
+ /**
+ * Builds the "fire purple artifact" Road Runner {@link Action} sequence.
+ *
+ *
+ *
+ *
+ *
+ *
+ */
protected void handlePeriodics()
{
+ // Refresh button edge-detection state for both gamepads.
g1.readButtons();
g2.readButtons();
+ // DPAD_DOWN on gamepad 2 toggles passive (slow continuous) intake on/off.
if(g2.wasJustPressed(GamepadKeys.Button.DPAD_DOWN))
continuousIntake = !continuousIntake;
- double leftTrigger = g1.getTrigger(GamepadKeys.Trigger.LEFT_TRIGGER);
+ // Read trigger values from both gamepads.
+ double leftTrigger = g1.getTrigger(GamepadKeys.Trigger.LEFT_TRIGGER);
double leftTrigger2 = g2.getTrigger(GamepadKeys.Trigger.LEFT_TRIGGER);
- double rightTrigger = g1.getTrigger(GamepadKeys.Trigger.RIGHT_TRIGGER);
+ double rightTrigger = g1.getTrigger(GamepadKeys.Trigger.RIGHT_TRIGGER);
double rightTrigger2 = g2.getTrigger(GamepadKeys.Trigger.RIGHT_TRIGGER);
- boolean leftDown = leftTrigger > TeleopConstants.Gamepad.TRIGGER_DEADZONE || leftTrigger2 > TeleopConstants.Gamepad.TRIGGER_DEADZONE;
- boolean rightDown = rightTrigger > TeleopConstants.Gamepad.TRIGGER_DEADZONE || rightTrigger2 > TeleopConstants.Gamepad.TRIGGER_DEADZONE;
+ // True if either driver is pressing the left (run) or right (reverse) trigger.
+ boolean leftDown = leftTrigger > TeleopConstants.Gamepad.TRIGGER_DEADZONE
+ || leftTrigger2 > TeleopConstants.Gamepad.TRIGGER_DEADZONE;
+ boolean rightDown = rightTrigger > TeleopConstants.Gamepad.TRIGGER_DEADZONE
+ || rightTrigger2 > TeleopConstants.Gamepad.TRIGGER_DEADZONE;
+ // Check whether the indexer drum is aligned with the intake port.
boolean inRange = indexer.isWithinTargetDegrees(5);
+ // Intake control priority: left trigger > right trigger > passive slow > stop.
if(leftDown){
- intake.run();
+ intake.run(); // full-speed forward intake
}
- else if(rightDown) intake.runBackwards();
- else if(continuousIntake) intake.runSlow();
- else intake.stop();
+ else if(rightDown) intake.runBackwards(); // reverse intake to unjam
+ else if(continuousIntake) intake.runSlow(); // passive slow intake in background
+ else intake.stop(); // all triggers released and passive disabled
- // driver one (constant
- handleAprilTagLock();
- handleMovement();
- handleAllianceSelection();
+ // --- driver 1 periodic handlers (run every loop) ---
+ handleAprilTagLock(); // update auto-aim heading lock state
+ handleMovement(); // send joystick inputs to the drive motors
+ handleAllianceSelection(); // check for alliance selection button presses
- handleTelemetry();
+ handleTelemetry(); // push diagnostic data to Driver Station / Dashboard
- indexer.update();
- outtake.periodic();
- actionHost.update();
- drive.updatePoseEstimate();
+ // Tick all subsystems that need a per-loop update.
+ indexer.update(); // run indexer servo PD control and color sensing
+ outtake.periodic(); // update flywheel RPM measurement and controller
+ actionHost.update(); // advance any running teleop Action by one step
+ drive.updatePoseEstimate(); // integrate odometry/IMU into the pose estimate
+ // If the shooter is spinning, continuously refresh the RPM setpoint.
if (outtake.getTargetRPM() > 0) {
- outtake.set(targetRPM * Bot.QUICKSPIN_OUTTAKE_RPM_SCALE);
+ outtake.set(targetRPM * Bot.QUICKSPIN_OUTTAKE_RPM_SCALE); // hold RPM at the dialed-in target
}
+ // If range feedback is requested or continuous lock is active, periodically
+ // recalculate the bearing turn correction and the range-dependent target RPM.
if(rangeRequested || continuousAprilTagLock){
long now = System.currentTimeMillis();
+ // Rate-limit the aimer recalculation to AIM_UPDATE_INTERVAL_MS.
if (now - lastAimUpdate >= AIM_UPDATE_INTERVAL_MS) {
lastAimUpdate = now;
- targetData = aimer.calculateLocalizedTurnPower();
- lastTurnCorrection = targetData[0];
- targetRPM = outtake.getRegressionRPM(targetData[1]);
- bearingTurnCorrection = targetData[2];
+ targetData = aimer.calculateLocalizedTurnPower(); // {turnPower, range, bearing}
+ lastTurnCorrection = targetData[0]; // save turn correction for the drive command
+ targetRPM = outtake.getRegressionRPM(targetData[1]); // set RPM from range regression
+ bearingTurnCorrection = targetData[2]; // save bearing for telemetry
}
- turnCorrection = lastTurnCorrection;
+ turnCorrection = lastTurnCorrection; // apply the most recent turn correction to drive
}
}
- // Periodic Handlers
+
+ // -----------------------------------------------------------------------
+ // Periodic handler helpers
+ // -----------------------------------------------------------------------
+
+ /**
+ * Writes diagnostic data for all subsystems to the Driver Station
+ * telemetry panel and FTC Dashboard. Called once per loop iteration.
+ */
protected void handleTelemetry()
{
- telemetry.addData("Field Centric", fieldCentric);
+ telemetry.addData("Field Centric", fieldCentric); // display current drive mode
telemetry.addData("Indexer State", "%s -> %s",
- indexer.getState(), indexer.getState().next());
+ indexer.getState(), indexer.getState().next()); // current and next slot
telemetry.addData("Indexer Voltages",
"Target: %.3f , Actual: %.3f",
- indexer.getTargetVoltage(), indexer.getVoltage());
- telemetry.addData("Outtake RPM", outtake.getRPM());
- telemetry.addData("Target RMP", outtake.getTargetRPM());
- telemetry.addData("Actuator up?", actuator.isActivated());
- telemetry.addData("Indexer Loaded?", indexer.isLoaded());
- telemetry.addData("April Lock", continuousAprilTagLock);
- telemetry.addData("Bot Range", targetData[1]);
- telemetry.addData("Alliance selected", colorGoalSelected);
- telemetry.addData("Turn Correction:", turnCorrection);
- telemetry.addData("Intake power: ", intake.getPower());
- telemetry.addData("Last Turn Correction", lastTurnCorrection);
+ indexer.getTargetVoltage(), indexer.getVoltage()); // servo position tracking
+ telemetry.addData("Outtake RPM", outtake.getRPM()); // measured flywheel RPM
+ telemetry.addData("Target RMP", outtake.getTargetRPM()); // commanded RPM setpoint
+ telemetry.addData("Actuator up?", actuator.isActivated()); // actuator position flag
+ telemetry.addData("Indexer Loaded?", indexer.isLoaded()); // drum load state
+ telemetry.addData("April Lock", continuousAprilTagLock); // heading lock active?
+ telemetry.addData("Bot Range", targetData[1]); // distance to goal (inches)
+ telemetry.addData("Alliance selected", colorGoalSelected); // selected alliance colour
+ telemetry.addData("Turn Correction:", turnCorrection); // current yaw correction
+ telemetry.addData("Intake power: ", intake.getPower()); // intake motor power
+ telemetry.addData("Last Turn Correction", lastTurnCorrection); // previous yaw correction
+ // Print per-slot colour and positional error for all three indexer slots.
for (Indexer.IndexerState s : Indexer.IndexerState.values()) {
telemetry.addData(
"Slot " + s.index,
"%s (err=%.1f°)",
- indexer.getColorAt(s),
- indexer.debugSlotErrorDeg(s)
+ indexer.getColorAt(s), // color stored in this slot
+ indexer.debugSlotErrorDeg(s) // how far off the slot is from its target angle
);
}
- telemetry.update();
+ telemetry.update(); // flush all data to the Driver Station display
}
+ /**
+ * Checks for alliance selection button presses from driver 1 and updates
+ * the Limelight pipeline, the aimer's target pose, and the telemetry label.
+ *
+ *
+ *
+ *
+ *
+ *
+ *
+ *
+ *
+ *
+ *
+ *
+ */
@TeleOp(name = "OneDriver", group = "AA_main")
public class OneDriverTeleop extends LinearOpMode {
+ /**
+ * Entry point called by the FTC runtime.
+ * Initialises hardware, waits for the start signal, then loops.
+ *
+ * @throws InterruptedException if the OpMode is interrupted while waiting
+ */
@Override
public void runOpMode() throws InterruptedException {
+ // Mirror telemetry to FTC Dashboard so live data is visible on a laptop.
telemetry = new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry());
+
+ // Create Road Runner drive at origin heading 0°.
MecanumDrive drive = new MecanumDrive(hardwareMap, new Pose2d(0, 0, Math.toRadians(0)));
- Bot bot = new Bot(hardwareMap, telemetry, drive, gamepad1, gamepad2,false);
- bot.teleopInit();
- waitForStart();
- bot.teleopStart();
+
+ // Construct the Bot. false = use gamepad 1 for driving (single-driver mode).
+ Bot bot = new Bot(hardwareMap, telemetry, drive, gamepad1, gamepad2, false);
+ bot.teleopInit(); // reset all subsystems before the match starts
+
+ waitForStart(); // pause here until the driver presses PLAY
+
+ bot.teleopStart(); // position hardware for match start
+
+ // Control loop: run every iteration until OpMode is stopped.
while (opModeIsActive() && !isStopRequested()) {
- bot.teleopTick();
+ bot.teleopTick(); // execute one full control cycle
}
}
}
\ No newline at end of file
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopConstants.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopConstants.java
index bbd255525282..b095ce9b902d 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopConstants.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopConstants.java
@@ -2,14 +2,55 @@
import com.acmerobotics.dashboard.config.Config;
+/**
+ * TeleopConstants is a collection of compile-time (and Dashboard-editable)
+ * tuning constants used throughout the teleop OpModes.
+ *
+ *
+ *
+ *
+ *