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; } From 6150b1df9607073dde16a03370d962ce7060c7ec Mon Sep 17 00:00:00 2001 From: "copilot-swe-agent[bot]" <198982749+Copilot@users.noreply.github.com> Date: Sun, 15 Mar 2026 17:25:19 +0000 Subject: [PATCH 3/3] Add detailed documentation to auto opmodes, teleop, and utility files Co-authored-by: ngerow2025 <44300132+ngerow2025@users.noreply.github.com> --- .../ftc/teamcode/auto/opmodes/FarLeave.java | 55 +- .../auto/opmodes/FasterBlueClose.java | 209 ++++++- .../auto/opmodes/FasterBlueCloseMotif.java | 283 +++++++-- .../teamcode/auto/opmodes/FasterRedClose.java | 214 ++++++- .../auto/opmodes/FasterRedCloseMotif.java | 426 ++++++++++++- .../auto/opmodes/FasterRedCloseMotif15.java | 308 +++++++++- .../opmodes/FasterRedCloseMotifLinear.java | 261 +++++++- .../ftc/teamcode/auto/utils/Action.java | 98 ++- .../ftc/teamcode/auto/utils/BotActions.java | 563 ++++++++++++++---- .../ftc/teamcode/auto/utils/Hardware.java | 82 ++- .../ftc/teamcode/auto/utils/Paths.java | 105 +++- .../teamcode/subsystems/limelight/Aimer.java | 293 +++++++-- .../subsystems/limelight/AprilTag.java | 279 ++++++++- .../subsystems/limelight/InertiaAutoAim.java | 106 +++- .../subsystems/outtakeUtil/RpmController.java | 97 ++- .../ftc/teamcode/teleop/ActionHost.java | 96 ++- .../ftc/teamcode/teleop/Bot.java | 478 ++++++++++++--- .../ftc/teamcode/teleop/BotPeriodics.java | 362 ++++++++--- .../ftc/teamcode/teleop/MainTeleop.java | 51 +- .../ftc/teamcode/teleop/OneDriverTeleop.java | 45 +- .../ftc/teamcode/teleop/TeleopConstants.java | 47 +- 21 files changed, 3896 insertions(+), 562 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auto/opmodes/FarLeave.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auto/opmodes/FarLeave.java index b522fa762ec1..67c546ec8787 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auto/opmodes/FarLeave.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auto/opmodes/FarLeave.java @@ -15,35 +15,78 @@ import org.firstinspires.ftc.teamcode.auto.roadrunner.miscRR.MecanumDrive; import org.firstinspires.ftc.teamcode.subsystems.Indexer; +/** + * FarLeave is a minimal "leave the starting zone" autonomous OpMode. + * + *
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:
+ * 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/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.
+ *
+ *
+ *
+ *
+ *
+ *
+ *
+ *
+ *
+ *
+ *
+ *
+ * @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.
+ *
+ *
+ *
+ *
+ *