It is very easy to link a button to a command. For instance, you could link the trigger button - * of a joystick to a "score" command. - * - *
Triggers can easily be composed for advanced functionality using the {@link - * #and(BooleanSupplier)}, {@link #or(BooleanSupplier)}, {@link #negate()} operators. - * - *
This class is provided by the NewCommands VendorDep - * - *
Spectrum modified in Fall 2024 to allow triggers to default start condition of false, so if - * something is already true when bound it will activate the trigger. We needed this for a trigger - * to activate only if Teleop was enabled. - */ -public class Trigger implements BooleanSupplier { - private final BooleanSupplier m_condition; - private final EventLoop m_loop; - public static final Trigger kFalse = new Trigger(() -> false); - public static final Trigger kTrue = new Trigger(() -> true); - - /** - * Creates a new trigger based on the given condition. - * - * @param loop The loop instance that polls this trigger. - * @param condition the condition represented by this trigger - */ - public Trigger(EventLoop loop, BooleanSupplier condition) { - m_loop = requireNonNullParam(loop, "loop", "Trigger"); - m_condition = requireNonNullParam(condition, "condition", "Trigger"); - } - - /** - * Creates a new trigger based on the given condition. - * - *
Polled by the default scheduler button loop. - * - * @param condition the condition represented by this trigger - */ - public Trigger(BooleanSupplier condition) { - this(CommandScheduler.getInstance().getDefaultButtonLoop(), condition); - } - - /** - * Starts the command when the condition changes. - * - * @param command the command to start - * @return this trigger, so calls can be chained - */ - public Trigger onChange(Command command) { - requireNonNullParam(command, "command", "onChange"); - m_loop.bind( - new Runnable() { - private boolean m_pressedLast = m_condition.getAsBoolean(); - - @Override - public void run() { - boolean pressed = m_condition.getAsBoolean(); - - if (m_pressedLast != pressed) { - CommandScheduler.getInstance().schedule(command); - } - - m_pressedLast = pressed; - } - }); - return this; - } - - /** - * Starts the given command whenever the condition changes from `false` to `true`. - * - * @param command the command to start - * @return this trigger, so calls can be chained - */ - public Trigger onTrue(Command command) { - requireNonNullParam(command, "command", "onTrue"); - m_loop.bind( - new Runnable() { - private boolean m_pressedLast = false; - - @Override - public void run() { - boolean pressed = m_condition.getAsBoolean(); - - if (!m_pressedLast && pressed) { - CommandScheduler.getInstance().schedule(command); - } - - m_pressedLast = pressed; - } - }); - return this; - } - - /** - * Starts the given commands whenever the condition changes from `false` to `true`. - * - * @param commands the commands to start - * @return this trigger, so calls can be chained - */ - public Trigger onTrue(Command... commands) { - requireNonNullParam(commands, "command", "onTrue"); - m_loop.bind( - new Runnable() { - private boolean m_pressedLast = false; - - @Override - public void run() { - boolean pressed = m_condition.getAsBoolean(); - - if (!m_pressedLast && pressed) { - for (Command command : commands) { - CommandScheduler.getInstance().schedule(command); - } - } - - m_pressedLast = pressed; - } - }); - return this; - } - - /** - * Starts the given command whenever the condition changes from `true` to `false`. - * - * @param command the command to start - * @return this trigger, so calls can be chained - */ - public Trigger onFalse(Command command) { - requireNonNullParam(command, "command", "onFalse"); - m_loop.bind( - new Runnable() { - private boolean m_pressedLast = true; - - @Override - public void run() { - boolean pressed = m_condition.getAsBoolean(); - - if (m_pressedLast && !pressed) { - CommandScheduler.getInstance().schedule(command); - } - - m_pressedLast = pressed; - } - }); - return this; - } - - public Trigger onFalse(Command... commands) { - requireNonNullParam(commands, "command", "onFalse"); - m_loop.bind( - new Runnable() { - private boolean m_pressedLast = true; - - @Override - public void run() { - boolean pressed = m_condition.getAsBoolean(); - - if (m_pressedLast && !pressed) { - for (Command command : commands) { - CommandScheduler.getInstance().schedule(command); - } - } - - m_pressedLast = pressed; - } - }); - return this; - } - - /** - * Starts the given command whenever the condition changes from `true` to `false`, but has to - * have run once prior - * - * @param command the command to start - * @return this trigger, so calls can be chained - */ - public Trigger onChangeToFalse(Command command) { - requireNonNullParam(command, "command", "onFalse"); - m_loop.bind( - new Runnable() { - private boolean m_pressedLast = false; - - @Override - public void run() { - boolean pressed = m_condition.getAsBoolean(); - - if (m_pressedLast && !pressed) { - CommandScheduler.getInstance().schedule(command); - } - - m_pressedLast = pressed; - } - }); - return this; - } - - /** - * Starts the given command whenever the condition changes from `false` to `true`, but has to - * have run once prior - * - * @param command the command to start - * @return this trigger, so calls can be chained - */ - public Trigger onChangeToTrue(Command command) { - requireNonNullParam(command, "command", "onTrue"); - m_loop.bind( - new Runnable() { - private boolean m_pressedLast = true; - - @Override - public void run() { - boolean pressed = m_condition.getAsBoolean(); - - if (!m_pressedLast && pressed) { - CommandScheduler.getInstance().schedule(command); - } - - m_pressedLast = pressed; - } - }); - return this; - } - - /** - * Starts the given command when the condition changes to `true` and cancels it when the - * condition changes to `false`. - * - *
Doesn't re-start the command if it ends while the condition is still `true`. If the - * command should restart, see {@link edu.wpi.first.wpilibj2.command.RepeatCommand}. - * - * @param command the command to start - * @return this trigger, so calls can be chained - */ - public Trigger whileTrue(Command command) { - requireNonNullParam(command, "command", "whileTrue"); - m_loop.bind( - new Runnable() { - private boolean m_pressedLast = false; - - @Override - public void run() { - boolean pressed = m_condition.getAsBoolean(); - - if (!m_pressedLast && pressed) { - CommandScheduler.getInstance().schedule(command); - } else if (m_pressedLast && !pressed) { - command.cancel(); - } - - m_pressedLast = pressed; - } - }); - return this; - } - - /** - * Starts the given command when the condition changes to `true` and cancels it when the - * condition changes to `false`. - * - *
Doesn't re-start the command if it ends while the condition is still `true`. If the - * command should restart, see {@link edu.wpi.first.wpilibj2.command.RepeatCommand}. - * - * @param commands the commands to start - * @return this trigger, so calls can be chained - */ - public Trigger whileTrue(Command... commands) { - requireNonNullParam(commands, "command", "whileTrue"); - m_loop.bind( - new Runnable() { - private boolean m_pressedLast = false; - - @Override - public void run() { - boolean pressed = m_condition.getAsBoolean(); - - for (Command command : commands) { - if (!m_pressedLast && pressed) { - CommandScheduler.getInstance().schedule(command); - } else if (m_pressedLast && !pressed) { - command.cancel(); - } - } - - m_pressedLast = pressed; - } - }); - return this; - } - - /** - * Starts the given command when the condition changes to `false` and cancels it when the - * condition changes to `true`. - * - *
Doesn't re-start the command if it ends while the condition is still `false`. If the - * command should restart, see {@link edu.wpi.first.wpilibj2.command.RepeatCommand}. - * - * @param command the command to start - * @return this trigger, so calls can be chained - */ - public Trigger whileFalse(Command command) { - requireNonNullParam(command, "command", "whileFalse"); - m_loop.bind( - new Runnable() { - private boolean m_pressedLast = true; - - @Override - public void run() { - boolean pressed = m_condition.getAsBoolean(); - - if (m_pressedLast && !pressed) { - CommandScheduler.getInstance().schedule(command); - } else if (!m_pressedLast && pressed) { - command.cancel(); - } - - m_pressedLast = pressed; - } - }); - return this; - } - - public Trigger whileFalse(Command... commands) { - requireNonNullParam(commands, "command", "whileFalse"); - m_loop.bind( - new Runnable() { - private boolean m_pressedLast = true; - - @Override - public void run() { - boolean pressed = m_condition.getAsBoolean(); - - for (Command command : commands) { - if (m_pressedLast && !pressed) { - CommandScheduler.getInstance().schedule(command); - } else if (!m_pressedLast && pressed) { - command.cancel(); - } - } - - m_pressedLast = pressed; - } - }); - return this; - } - - /** - * Toggles a command when the condition changes from `false` to `true`. - * - * @param command the command to toggle - * @return this trigger, so calls can be chained - */ - public Trigger toggleOnTrue(Command command) { - requireNonNullParam(command, "command", "toggleOnTrue"); - m_loop.bind( - new Runnable() { - private boolean m_pressedLast = false; - - @Override - public void run() { - boolean pressed = m_condition.getAsBoolean(); - - if (!m_pressedLast && pressed) { - if (command.isScheduled()) { - command.cancel(); - } else { - CommandScheduler.getInstance().schedule(command); - } - } - - m_pressedLast = pressed; - } - }); - return this; - } - - /** - * Toggles a command when the condition changes from `true` to `false`. - * - * @param command the command to toggle - * @return this trigger, so calls can be chained - */ - public Trigger toggleOnFalse(Command command) { - requireNonNullParam(command, "command", "toggleOnFalse"); - m_loop.bind( - new Runnable() { - private boolean m_pressedLast = true; - - @Override - public void run() { - boolean pressed = m_condition.getAsBoolean(); - - if (m_pressedLast && !pressed) { - if (command.isScheduled()) { - command.cancel(); - } else { - CommandScheduler.getInstance().schedule(command); - } - } - - m_pressedLast = pressed; - } - }); - return this; - } - - /** - * Run a command while true. Also runs a command for a certain timeout when released. - * - * @param runCommand the command to run while true - * @param endCommand the command to run when released - * @param endTimeout the time to run the end command - * @return this trigger, so calls can be chained - */ - public Trigger runWithEndSequence(Command runCommand, Command endCommand, double endTimeout) { - this.whileTrue(runCommand); - this.onFalse(endCommand.withTimeout(endTimeout).withName(endCommand.getName())); - return this; - } - - @Override - public boolean getAsBoolean() { - return m_condition.getAsBoolean(); - } - - /** - * Composes two triggers with logical AND. - * - * @param trigger the condition to compose with - * @return A trigger which is active when both component triggers are active. - */ - public Trigger and(BooleanSupplier trigger) { - return new Trigger(m_loop, () -> m_condition.getAsBoolean() && trigger.getAsBoolean()); - } - - /** - * Combines multiple BooleanSupplier triggers using a logical AND operation. - * - * @param triggers an array of BooleanSupplier triggers to be combined. - * @return a new Trigger that represents the logical AND of all provided triggers. - */ - public Trigger and(BooleanSupplier... triggers) { - Trigger trig = this; - for (BooleanSupplier t : triggers) { - trig = trig.and(t); - } - return trig; - } - - /** - * Combines multiple BooleanSupplier triggers using a logical OR operation. - * - * @param triggers an array of BooleanSupplier triggers to be combined. - * @return a new NewTrigger instance that represents the logical OR of the provided triggers. - */ - public Trigger or(BooleanSupplier... triggers) { - Trigger trig = this; - for (BooleanSupplier t : triggers) { - trig = trig.or(t); - } - return trig; - } - - /** - * Composes two triggers with logical OR. - * - * @param trigger the condition to compose with - * @return A trigger which is active when either component trigger is active. - */ - public Trigger or(BooleanSupplier trigger) { - return new Trigger(m_loop, () -> m_condition.getAsBoolean() || trigger.getAsBoolean()); - } - - /** - * Creates a new trigger that is active when this trigger is inactive, i.e. that acts as the - * negation of this trigger. - * - * @return the negated trigger - */ - public Trigger negate() { - return new Trigger(m_loop, () -> !m_condition.getAsBoolean()); - } - - /** - * renamed negate - * - * @return the negated trigger - */ - public Trigger not() { - return negate(); - } - - /** - * Creates a new debounced trigger from this trigger - it will become active when this trigger - * has been active for longer than the specified period. - * - * @param seconds The debounce period. - * @return The debounced trigger (rising edges debounced only) - */ - public Trigger debounce(double seconds) { - return debounce(seconds, Debouncer.DebounceType.kRising); - } - - /** - * Creates a new debounced trigger from this trigger - it will become active when this trigger - * has been active for longer than the specified period. - * - * @param seconds The debounce period. - * @param type The debounce type. - * @return The debounced trigger. - */ - public Trigger debounce(double seconds, Debouncer.DebounceType type) { - return new Trigger( - m_loop, - new BooleanSupplier() { - final Debouncer m_debouncer = new Debouncer(seconds, type); - - @Override - public boolean getAsBoolean() { - return m_debouncer.calculate(m_condition.getAsBoolean()); - } - }); - } -} diff --git a/src/main/java/frc/rebuilt/Field.java b/src/main/java/frc/rebuilt/Field.java index 6a578163..b1f7fbec 100644 --- a/src/main/java/frc/rebuilt/Field.java +++ b/src/main/java/frc/rebuilt/Field.java @@ -1,9 +1,3 @@ -// Copyright (c) 2025 FRC 6328 -// http://github.com/Mechanical-Advantage -// -// Use of this source code is governed by an MIT-style -// license that can be found in the LICENSE file at -// the root directory of this project. package frc.rebuilt; import edu.wpi.first.math.geometry.*; @@ -39,7 +33,7 @@ public class Field { new Translation2d(fieldLength - 1, fieldWidth - 1); public static final Translation2d deepFeedBlueLeft = new Translation2d(1, fieldWidth - 2.5); - public static final Translation2d deepFeedBlueRight = new Translation2d(21, 2.5); + public static final Translation2d deepFeedBlueRight = new Translation2d(1, 2.5); public static final Translation2d deepFeedRedLeft = new Translation2d(fieldLength - 1, 2.5); public static final Translation2d deepFeedRedRight = new Translation2d(fieldLength - 1, fieldWidth - 2.5); @@ -166,7 +160,7 @@ public static class BlueTower { public static final double midRungZ = Units.inchesToMeters(45.0); public static final double highRungZ = Units.inchesToMeters(63.0); - public static final double tag31Y = Units.inchesToMeters(fieldWidth / 2); + public static final double tag31Y = fieldWidth / 2.0; // Fixed X location public static final double frontFaceX = Units.inchesToMeters(43.51); @@ -225,19 +219,8 @@ public static double BlueToRed(double translation) { return Field.fieldLength - translation; } - @Getter - public static final Translation3d blueHubCenter = BlueHub.topCenter; // new Translation3d( - // Units.inchesToMeters(182.11), - // Units.inchesToMeters(158.84), - // Units.inchesToMeters(72)); - @Getter - public static final Translation3d redHubCenter = - BlueToRed(BlueHub.topCenter); // new Translation3d( - // Units.inchesToMeters(469.11), - // Units.inchesToMeters(158.84), - // Units.inchesToMeters(72)); - - @Getter private static final double aprilTagWidth = Units.inchesToMeters(6.50); + @Getter public static final Translation3d blueHubCenter = BlueHub.topCenter; + @Getter public static final Translation3d redHubCenter = BlueToRed(BlueHub.topCenter); /** Returns {@code true} if the robot is on the blue alliance. */ public static boolean isBlue() { diff --git a/src/main/java/frc/rebuilt/FieldHelpers.java b/src/main/java/frc/rebuilt/FieldHelpers.java index b9de0245..961eab1d 100644 --- a/src/main/java/frc/rebuilt/FieldHelpers.java +++ b/src/main/java/frc/rebuilt/FieldHelpers.java @@ -47,22 +47,6 @@ public static Pose2d flipIfRed(Pose2d red) { return new Pose2d(flipIfRed(red.getTranslation()), flipAngleIfRed(red.getRotation())); } - public static Translation2d flipIfRedSide(Translation2d red) { - if (Zones.blueFieldSide.getAsBoolean()) { - return red; - } - return new Translation2d(flipX(red.getX()), flipY(red.getY())); - } - - public static Pose2d flipIfRedSide(Pose2d red) { - if (Zones.blueFieldSide.getAsBoolean()) { - return red; - } - return new Pose2d( - flipIfRedSide(new Translation2d(red.getX(), red.getY())), - flipAngle(red.getRotation())); - } - public static double flipX(double xCoordinate) { return Field.fieldLength - xCoordinate; } diff --git a/src/main/java/frc/rebuilt/FuelPhysicsSim.java b/src/main/java/frc/rebuilt/FuelPhysicsSim.java new file mode 100644 index 00000000..91171aec --- /dev/null +++ b/src/main/java/frc/rebuilt/FuelPhysicsSim.java @@ -0,0 +1,2494 @@ +/* + * FuelPhysicsSim.java - Full-field ball physics simulation for FRC 2026 REBUILT + * + * MIT License + * + * Copyright (c) 2026 FRC Team 5962 perSEVERE + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND. + */ + +package frc.rebuilt; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.geometry.Translation3d; +import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.networktables.DoublePublisher; +import edu.wpi.first.networktables.IntegerPublisher; +import edu.wpi.first.networktables.NetworkTableInstance; +import edu.wpi.first.networktables.StructArrayPublisher; +import java.util.ArrayList; +import java.util.List; +import java.util.Random; +import java.util.function.BooleanSupplier; +import java.util.function.Supplier; + +/** + * Full-field ball physics simulation for FRC 2026 REBUILT. Handles drag, Magnus lift, friction, + * ball-ball collisions, wall bounces, hub scoring, sleeping, CCD, and robot interaction. Single + * file, only depends on WPILib (wpimath + ntcore). Drop it into your sim and watch balls fly. + * + *
Physics: symplectic Euler integration, 3D angular velocity for Magnus (omega x v cross + * product), Coulomb friction with spin transfer, sequential impulse collision solver with warm + * starting and Baumgarte stabilization. Spatial hashing for ball-ball broadphase. Ball sleeping + * keeps 350+ resting balls under 2ms/tick. + * + *
Usage: + * + *
+ * FuelPhysicsSim ballSim = new FuelPhysicsSim("Sim/Fuel");
+ * ballSim.enable();
+ * ballSim.placeFieldBalls(); // spawns all the game pieces
+ * // in simulationPeriodic():
+ * ballSim.configureRobot(width, length, bumperH, poseSupplier, speedsSupplier);
+ * ballSim.tick(); // runs physics, publishes to NT
+ *
+ */
+public class FuelPhysicsSim {
+
+ // Physics constants
+
+ private static final double GRAVITY = 9.81; // m/s^2
+ private static final double AIR_DENSITY = 1.225; // kg/m^3, standard atmosphere
+ private static final double BALL_MASS = 0.215; // kg, game manual 5.10.1 midpoint
+ private static final double BALL_DIAMETER = 0.1501; // m, game manual 5.10.1
+ private static final double BALL_RADIUS = BALL_DIAMETER / 2.0;
+ private static final double BALL_CROSS_AREA = Math.PI * BALL_RADIUS * BALL_RADIUS;
+ private static final double BALL_MOMENT_OF_INERTIA =
+ 0.4 * BALL_MASS * BALL_RADIUS * BALL_RADIUS; // 2/5 * m * r^2, solid sphere
+
+ // Aerodynamic coefficients
+ private static final double DEFAULT_CD = 0.47; // drag coefficient, smooth sphere
+ private static final double DEFAULT_CM = 0.2; // Magnus coefficient, conservative estimate
+
+ // Precomputed force factors (divided by mass to get acceleration factors)
+ private static final double DRAG_ACCEL_FACTOR =
+ 0.5 * AIR_DENSITY * DEFAULT_CD * BALL_CROSS_AREA / BALL_MASS;
+ // Extra BALL_RADIUS factor converts the omega x v cross product to acceleration
+ private static final double MAGNUS_ACCEL_FACTOR =
+ 0.5 * AIR_DENSITY * DEFAULT_CM * BALL_CROSS_AREA * BALL_RADIUS / BALL_MASS;
+
+ // Coefficients of restitution (per-material, from field element build instructions)
+ private static final double COR_CARPET = 0.65; // foam on low-pile carpet
+ private static final double COR_WALL =
+ 0.70; // foam on polycarbonate (alliance walls, guardrails)
+ private static final double COR_STEEL = 0.72; // foam on powder-coated steel (tower, rungs)
+ private static final double COR_HUB = 0.70; // foam on polycarbonate (hub body panels)
+ private static final double COR_HDPE = 0.60; // foam on textured HDPE (bump ramps, 15deg)
+ private static final double COR_NET = 0.15; // mesh fabric absorbs most of the energy
+ private static final double COR_BUMPER = 0.08; // polycarb-backed foam, nearly inelastic
+ private static final double COR_BALL_BALL = 0.45; // foam-on-foam, high deformation loss
+
+ // Friction coefficients
+ private static final double MU_GROUND_KINETIC = 0.3; // kinetic friction on carpet
+ private static final double MU_GROUND_ROLLING = 0.05; // rolling friction
+ private static final double MU_WALL = 0.4; // foam on polycarbonate/fabric
+ private static final double MU_BALL_BALL = 0.3; // foam on foam
+
+ // Velocity-dependent COR reference speed
+ private static final double COR_VREF = 3.0; // m/s
+ private static final double COR_EXPONENT = 0.15;
+
+ // Reusable axis-aligned unit normals (avoids allocating these in hot loops)
+ private static final Translation3d AXIS_X_POS = new Translation3d(1, 0, 0);
+ private static final Translation3d AXIS_X_NEG = new Translation3d(-1, 0, 0);
+ private static final Translation3d AXIS_Y_POS = new Translation3d(0, 1, 0);
+ private static final Translation3d AXIS_Y_NEG = new Translation3d(0, -1, 0);
+ private static final Translation3d AXIS_Z_POS = new Translation3d(0, 0, 1);
+ private static final Translation3d AXIS_Z_NEG = new Translation3d(0, 0, -1);
+
+ // Period
+ private static final double PERIOD = 0.02; // 20ms
+
+ // Field geometry
+
+ private static final double FIELD_LENGTH = 16.541; // m
+ private static final double FIELD_WIDTH = 8.052; // m
+
+ // Alliance wall and guardrail heights
+ private static final double ALLIANCE_WALL_HEIGHT = 0.935; // 36.8 in
+ private static final double GUARDRAIL_HEIGHT = 0.508; // 20 in
+
+ // Bump geometry (tent-shaped, 15-degree ramps)
+ private static final double BUMP_HEIGHT = 0.165; // m, 6.513 in
+
+ // Hub positions (from game manual field layout)
+ private static final Translation2d BLUE_HUB_CENTER = new Translation2d(4.5974, 4.035);
+ private static final Translation2d RED_HUB_CENTER = new Translation2d(11.938, 4.035);
+
+ // Hub dimensions
+ private static final double HUB_ENTRY_HEIGHT = 1.829; // m, 72 in
+ private static final double HUB_ENTRY_RADIUS = 0.5295; // m, 41.7 in / 2 across flats
+ private static final double HUB_SIDE = 1.194; // m, 47 in square base
+ // Hub base structure height for ball collision (not a solid wall to scoring height,
+ // just the base frame that deflects ground-level balls)
+ private static final double HUB_BASE_HEIGHT = 0.5; // m, ~20 in base frame
+
+ // Net dimensions
+ private static final double NET_HEIGHT_MIN = 1.5; // m
+ private static final double NET_HEIGHT_MAX = 3.057; // m
+ private static final double NET_WIDTH = 1.484; // m
+ private static final double NET_OFFSET = HUB_SIDE / 2.0 + 0.261;
+
+ // Trench geometry
+ private static final double TRENCH_WIDTH = 1.265; // m
+ private static final double TRENCH_BLOCK_WIDTH = 0.305; // m, 12 in
+ private static final double TRENCH_HEIGHT = 0.565; // m, 22.25 in underpass
+ private static final double TRENCH_PILLAR_HEIGHT = 1.346; // m, 53 in
+
+ // Tower geometry
+ private static final double TOWER_POLE_WIDTH = 0.051; // m, 2 in
+ private static final double TOWER_POLE_HEIGHT = 1.194; // m, 47 in
+ private static final double TOWER_UPRIGHT_THICK = 0.038; // m, 1.5 in
+ private static final double TOWER_UPRIGHT_DEPTH = 0.089; // m, 3.5 in
+ private static final double TOWER_UPRIGHT_HEIGHT = 1.831; // m, 72.1 in
+ private static final double TOWER_UPRIGHT_SPACING = 0.819; // m, 32.25 in center-to-center
+
+ // Tower rung geometry (Schedule 40 pipe)
+ private static final double RUNG_OUTER_DIAMETER = 0.042; // m, 1.66 in OD
+ private static final double RUNG_RADIUS = RUNG_OUTER_DIAMETER / 2.0;
+ private static final double RUNG_OVERHANG = 0.149; // m, 5.875 in past upright
+ private static final double RUNG_LOW_HEIGHT = 0.686; // m, 27 in
+ private static final double RUNG_MID_HEIGHT = 1.143; // m, 45 in
+ private static final double RUNG_HIGH_HEIGHT = 1.600; // m, 63 in
+
+ // Tower bracing
+ private static final double BRACING_BOTTOM = 0.721; // m, 28.4 in
+ private static final double BRACING_TOP = 1.102; // m, 43.4 in
+
+ // Hub ramp area
+ private static final double HUB_RAMP_WIDTH = 1.194; // m, 47 in
+ private static final double HUB_RAMP_LENGTH = 5.512; // m, 217 in
+ private static final double HUB_RAMP_HEIGHT = 0.165; // m, 6.5 in
+
+ // Spatial hash
+ private static final double CELL_SIZE = 0.25;
+ private static final int GRID_COLS = (int) Math.ceil(FIELD_LENGTH / CELL_SIZE);
+ private static final int GRID_ROWS = (int) Math.ceil(FIELD_WIDTH / CELL_SIZE);
+
+ // Max balls
+ private static final int MAX_BALLS = 2000;
+
+ // Bump ramp segments (XZ line segments extruded along Y)
+
+ private static final BumpSegment[] BUMP_SEGMENTS = {
+ // Blue lower bump ramp up/down
+ new BumpSegment(3.96, 0, 4.524, BUMP_HEIGHT, 1.88, 3.73),
+ new BumpSegment(4.524, BUMP_HEIGHT, 5.088, 0, 1.88, 3.73),
+ // Blue upper bump ramp up/down
+ new BumpSegment(3.96, 0, 4.524, BUMP_HEIGHT, 4.32, 6.17),
+ new BumpSegment(4.524, BUMP_HEIGHT, 5.088, 0, 4.32, 6.17),
+ // Red lower bump ramp up/down
+ new BumpSegment(FIELD_LENGTH - 5.088, 0, FIELD_LENGTH - 4.524, BUMP_HEIGHT, 1.88, 3.73),
+ new BumpSegment(FIELD_LENGTH - 4.524, BUMP_HEIGHT, FIELD_LENGTH - 3.96, 0, 1.88, 3.73),
+ // Red upper bump ramp up/down
+ new BumpSegment(FIELD_LENGTH - 5.088, 0, FIELD_LENGTH - 4.524, BUMP_HEIGHT, 4.32, 6.17),
+ new BumpSegment(FIELD_LENGTH - 4.524, BUMP_HEIGHT, FIELD_LENGTH - 3.96, 0, 4.32, 6.17),
+ };
+
+ // AABB obstacles
+
+ private static final AABB[] AABB_OBSTACLES;
+
+ static {
+ ListSimulates the robot's 3D pose (Z height, pitch, and roll) as it drives over the raised bumps + * on the 2026 FRC REBUILT field. It also implements a frictionless-slide model that prevents the + * robot from "ghosting" through a bump when it lacks the speed to crest it. + * + *
Minimum crossing speed: {@code sqrt(2·g·h) ≈ sqrt(2·9.81·0.165) ≈ 1.80 m/s}. + * + *
{@code
+ * // 1. Construct once, typically in startSimThread() alongside your MapleSim drivetrain.
+ * RobotBumpSim robotBumpSim = new RobotBumpSim(drivetrain.getModuleLocations());
+ *
+ * // 2. Call every simulationPeriodic() AFTER MapleSim has stepped.
+ * Pose2d simPose = mapleSimDrive.getSimulatedDriveTrainPose();
+ * ChassisSpeeds fieldRelativeSpeeds = mapleSimSwerveDrivetrain.mapleSimDrive.getDriveTrainSimulatedChassisSpeedsFieldRelative();
+ *
+ * // subticks should match the number of physics sub-steps you pass to your ball/object sim.
+ * // A value of 5 works well for a 20 ms loop (4 ms sub-steps).
+ * Pose3d simPose3d = robotBumpSim.update(simPose, fieldSpeeds, subticks);
+ *
+ * // 3. When on the ramp, override MapleSim's pose so the robot physically slides back
+ * // instead of the correction being purely visual.
+ * if (robotBumpSim.isOnRamp()) {
+ * mapleSimDrive.setSimulationWorldPose(robotBumpSim.getSimWorldPose(simPose));
+ * }
+ *
+ * // 4. Log or visualise the 3D pose (e.g. with AdvantageScope).
+ * Logger.recordOutput("Drive/Pose3d", simPose3d);
+ * }
+ *
+ * The bump geometry is encoded as XZ line segments with Y-range guards. Each bump has two + * segments per side (ascending face + descending face) at X positions symmetric about field centre. + * See {@link #BUMP_LINE_STARTS} / {@link #BUMP_LINE_ENDS} for the raw coordinates. All positions + * are in metres, origin at the Blue Alliance driver-station corner. + */ +public class RobotBumpSim { + + // ------------------------------------------------------------------------- + // Field / physics constants + // ------------------------------------------------------------------------- + + /** Robot control-loop period (seconds). Matches the WPILib default of 20 ms. */ + private static final double PERIOD = 0.02; + + /** Gravitational acceleration vector (m/s², pointing in the -Z direction). */ + private static final Translation3d GRAVITY = new Translation3d(0, 0, -9.81); + + /** Full length of the REBUILT field (metres). */ + private static final double FIELD_LENGTH = 16.51; + + /** Full width of the REBUILT field (metres). */ + private static final double FIELD_WIDTH = 8.04; + + /** + * Start points of the eight bump XZ line segments (four per alliance, ascending + descending). + * + *
Each {@link Translation3d} stores {@code (fieldX, yMin, fieldZ)}: the world-X start of the + * ramp face, the minimum field-Y at which this segment is present, and the ramp Z at that X. + * The companion {@link #BUMP_LINE_ENDS} array stores the end point including the maximum Y. + * + *
Indices 0–3 are the Blue-side bump (near X ≈ 3.96–5.18 m); indices 4–7 are the Red-side + * bump (near X ≈ FIELD_LENGTH−5.18 – FIELD_LENGTH−3.96 m). + */ + static final Translation3d[] BUMP_LINE_STARTS = { + // Blue bump — ascending faces (Z rises from 0 → 0.165 m) + new Translation3d(3.96, 1.57, 0), + new Translation3d(3.96, FIELD_WIDTH / 2 + 0.60, 0), + // Blue bump — descending faces (Z falls from 0.165 → 0 m) + new Translation3d(4.61, 1.57, 0.165), + new Translation3d(4.61, FIELD_WIDTH / 2 + 0.60, 0.165), + // Red bump — ascending faces + new Translation3d(FIELD_LENGTH - 5.18, 1.57, 0), + new Translation3d(FIELD_LENGTH - 5.18, FIELD_WIDTH / 2 + 0.60, 0), + // Red bump — descending faces + new Translation3d(FIELD_LENGTH - 4.61, 1.57, 0.165), + new Translation3d(FIELD_LENGTH - 4.61, FIELD_WIDTH / 2 + 0.60, 0.165), + }; + + /** + * End points of the eight bump XZ line segments. + * + *
Each {@link Translation3d} stores {@code (fieldX, yMax, fieldZ)}: the world-X end of the + * ramp face, the maximum field-Y at which this segment is present, and the ramp Z at that X. + */ + static final Translation3d[] BUMP_LINE_ENDS = { + // Blue bump — ascending faces + new Translation3d(4.61, FIELD_WIDTH / 2 - 0.60, 0.165), + new Translation3d(4.61, FIELD_WIDTH - 1.57, 0.165), + // Blue bump — descending faces + new Translation3d(5.18, FIELD_WIDTH / 2 - 0.60, 0), + new Translation3d(5.18, FIELD_WIDTH - 1.57, 0), + // Red bump — ascending faces + new Translation3d(FIELD_LENGTH - 4.61, FIELD_WIDTH / 2 - 0.60, 0.165), + new Translation3d(FIELD_LENGTH - 4.61, FIELD_WIDTH - 1.57, 0.165), + // Red bump — descending faces + new Translation3d(FIELD_LENGTH - 3.96, FIELD_WIDTH / 2 - 0.60, 0), + new Translation3d(FIELD_LENGTH - 3.96, FIELD_WIDTH - 1.57, 0), + }; + + /** Index of the first bump segment in {@link #BUMP_LINE_STARTS} (inclusive). */ + private static final int BUMP_LINE_FIRST = 0; + + /** Index of the last bump segment in {@link #BUMP_LINE_STARTS} (inclusive). */ + private static final int BUMP_LINE_LAST = BUMP_LINE_STARTS.length - 1; + + // ------------------------------------------------------------------------- + // Tunable physics constants — adjust for your robot + // ------------------------------------------------------------------------- + + /** Effective wheel contact radius against the bump surface (metres). */ + private static final double WHEEL_RADIUS = 0.048; + + /** Height offset from the average module-contact Z to the robot-body origin (metres). */ + private static final double CHASSIS_HEIGHT = 0.0; + + /** + * Coefficient of restitution for vertical (Z) robot-bump collisions. 0 = perfectly inelastic + * (no bounce), 1 = perfectly elastic. + */ + private static final double BUMP_COR = 0.15; + + /** + * Floating-point epsilon used to decide whether a projected point falls within a line segment. + * A small positive tolerance prevents false misses caused by rounding when the module lands + * exactly on a segment endpoint. + */ + private static final double SEGMENT_PROJECTION_TOLERANCE = 1e-6; + + // ------------------------------------------------------------------------- + // Per-instance state + // ------------------------------------------------------------------------- + + /** Robot-relative module positions, order: FL(0), FR(1), BL(2), BR(3). */ + private final Translation2d[] moduleOffsets; + + /** Absolute Z position of each module contact point (metres above the floor). */ + private final double[] moduleZPos; + + /** Z velocity of each module contact point (m/s). */ + private final double[] moduleZVel; + + /** Distance between front and back module pairs along the robot X axis (metres). */ + private final double frontBackDist; + + /** Distance between left and right module pairs along the robot Y axis (metres). */ + private final double leftRightDist; + + /** + * True while the robot is on the ramp and this sim owns the field-X position. The caller must + * call {@link #getSimWorldPose(Pose2d)} and apply it to MapleSim via {@code + * setSimulationWorldPose} whenever this is true. + */ + private boolean onRamp = false; + + /** + * Absolute field-X position of the robot while on the ramp (frictionless model). Ignored when + * {@link #onRamp} is false. + */ + private double simXPos = 0.0; + + /** + * Field-X velocity of the robot on the ramp (m/s, frictionless model). Initialized to the + * robot's field-Vx at first ramp contact; decelerated by gravity-along-ramp each subtick. + * Ignored when {@link #onRamp} is false. + */ + private double simXVel = 0.0; + + // ------------------------------------------------------------------------- + // Constructor + // ------------------------------------------------------------------------- + + /** + * Creates a new {@code RobotBumpSim} for a swerve drivetrain. + * + * @param moduleOffsets Robot-relative module positions in order FL, FR, BL, BR (metres). + * Typically obtained via {@code CommandSwerveDrivetrain#getModuleLocations()}. + */ + public RobotBumpSim(Translation2d[] moduleOffsets) { + this.moduleOffsets = moduleOffsets; + this.moduleZPos = new double[4]; + this.moduleZVel = new double[4]; + + double frontX = (moduleOffsets[0].getX() + moduleOffsets[1].getX()) / 2.0; + double backX = (moduleOffsets[2].getX() + moduleOffsets[3].getX()) / 2.0; + frontBackDist = Math.max(Math.abs(frontX - backX), 1e-3); + + double leftY = (moduleOffsets[0].getY() + moduleOffsets[2].getY()) / 2.0; + double rightY = (moduleOffsets[1].getY() + moduleOffsets[3].getY()) / 2.0; + leftRightDist = Math.max(Math.abs(leftY - rightY), 1e-3); + } + + // ------------------------------------------------------------------------- + // Public API + // ------------------------------------------------------------------------- + + /** + * Returns {@code true} while the robot is on the ramp in frictionless-slide mode. + * + *
When {@code true} the caller must call {@link #getSimWorldPose(Pose2d)} to obtain the + * corrected 2D pose and feed it to MapleSim via {@code setSimulationWorldPose}, so the robot + * actually slides backward rather than just appearing to. + */ + public boolean isOnRamp() { + return onRamp; + } + + /** + * Returns the 2D pose that should be set on MapleSim while on the ramp. + * + *
The X coordinate is replaced with the frictionless {@link #simXPos}; Y and rotation are + * taken from {@code latestMaplePose} so MapleSim continues to own lateral motion. + * + * @param latestMaplePose The most recent 2D pose read from MapleSim (used for Y and rotation). + * @return A corrected {@link Pose2d} to pass to {@code setSimulationWorldPose}. + */ + public Pose2d getSimWorldPose(Pose2d latestMaplePose) { + return new Pose2d(simXPos, latestMaplePose.getY(), latestMaplePose.getRotation()); + } + + /** + * Advances the bump simulation by one 20 ms period and returns the robot's 3D pose. + * + *
When the robot is on the ramp the returned pose uses {@link #simXPos} for X, giving a + * physically accurate visual position. The caller must also call {@link + * #getSimWorldPose(Pose2d)} and apply it to MapleSim so the actual simulation position matches + * (see the class-level Javadoc for a complete integration example). + * + * @param robotPose2d Robot's 2D pose from the MapleSim drivetrain. + * @param fieldRelativeSpeeds Field-relative chassis speeds from the MapleSim drivetrain. + * @param subticks Physics sub-steps per period. Must match the {@code subticks} value used by + * any companion ball/object sim so they stay in sync. Typical value: 5 (= 4 ms sub-steps + * per 20 ms loop). + * @return A {@link Pose3d} with physically correct X, Z, pitch, and roll. + */ + public Pose3d update(Pose2d robotPose2d, ChassisSpeeds fieldRelativeSpeeds, int subticks) { + double vx = fieldRelativeSpeeds.vxMetersPerSecond; + double dt = PERIOD / subticks; + + // contactFactor reduces effective gravity deceleration for diagonal crossings. + // yaw = 0° -> |cos(0)| = 1.0 -> full deceleration (straight-on, hardest to cross) + // yaw = 45° -> |cos(90°)| = 0.71 -> reduced deceleration (diagonal, easier) + // yaw = 90° -> |cos(180°)| = 1.0 -> full deceleration (sideways approach also hard) + double contactFactor = Math.abs(Math.cos(2 * robotPose2d.getRotation().getRadians())); + + // Y positions of each module (MapleSim-owned, constant for the whole period) + double[] worldY = new double[4]; + for (int i = 0; i < 4; i++) { + Translation2d wo = moduleOffsets[i].rotateBy(robotPose2d.getRotation()); + worldY[i] = robotPose2d.getY() + wo.getY(); + } + + for (int tick = 0; tick < subticks; tick++) { + // Use simXPos (frictionless) when on ramp, else follow MapleSim's X + double currentRobotX = onRamp ? simXPos : robotPose2d.getX(); + + double gravAccelXSum = 0.0; + int contactCount = 0; + + for (int i = 0; i < 4; i++) { + // Compute this module's world-X from current robot-X + Translation2d wo = moduleOffsets[i].rotateBy(robotPose2d.getRotation()); + double wx = currentRobotX + wo.getX(); + + // Z gravity and integration + moduleZVel[i] += GRAVITY.getZ() * dt; + moduleZPos[i] += moduleZVel[i] * dt; + + // Bump collisions + for (int lineIdx = BUMP_LINE_FIRST; lineIdx <= BUMP_LINE_LAST; lineIdx++) { + double gax = + handleModuleBumpCollision( + i, wx, worldY[i], onRamp ? simXVel : vx, lineIdx); + if (!Double.isNaN(gax)) { + gravAccelXSum += gax; + contactCount++; + } + } + + // Floor + if (moduleZPos[i] < 0.0) { + moduleZPos[i] = 0.0; + if (moduleZVel[i] < 0.0) moduleZVel[i] = -moduleZVel[i] * BUMP_COR; + } + } + + if (contactCount > 0) { + if (!onRamp) { + // First ramp contact: capture current kinematics + onRamp = true; + simXPos = robotPose2d.getX(); + simXVel = vx; + } + // Apply average gravity-along-ramp deceleration (frictionless surface) + double avgGravAccelX = (gravAccelXSum / contactCount) * contactFactor; + simXVel += avgGravAccelX * dt; + simXPos += simXVel * dt; + } else if (onRamp) { + // No ramp contact; check if all modules have settled onto flat ground + boolean allFlat = true; + for (int i = 0; i < 4; i++) { + if (moduleZPos[i] > 0.01) { + allFlat = false; + break; + } + } + if (allFlat) { + // Robot has backed out or crossed — MapleSim reclaims X + onRamp = false; + } else { + // Briefly airborne after cresting the peak: keep sliding under simXVel + simXPos += simXVel * dt; + } + } + } + + return computePose3d(robotPose2d); + } + + // ------------------------------------------------------------------------- + // Private helpers + // ------------------------------------------------------------------------- + + /** + * Handles the XZ-plane bump collision for module {@code moduleIdx} against line segment {@code + * lineIdx}. Applies a Z position correction and Z velocity impulse. Returns the + * gravity-along-ramp X acceleration (m/s²) if in contact, or {@link Double#NaN} if not. + * + *
The returned acceleration is: + * + *
+ * a_X = -g * normalX * normalZ + *+ * + * Ascending face (normalX < 0, normalZ > 0): a_X < 0 — gravity pulls robot back.
Warning: This method will cause a very large loop overrun, as it rebinds all states - * to their triggers. Be careful when you call this as it will cause delays in the robot code. - * It is recommended to call this method at the end of disabledInit and teleopInit, as those are - * the most common places to need to reset commands and bindings. - */ - public void resetCommandsAndButtons() { - CommandScheduler.getInstance().cancelAll(); // Disable any currently running commands - CommandScheduler.getInstance().getActiveButtonLoop().clear(); - - // Reset Config for all gamepads and other button bindings - pilot.resetConfig(); - operator.resetConfig(); - - // Bind Triggers for all subsystems - setupStates(); - RobotStates.setupStates(); + public void configureBindings() { + // LT alone → intake fuel; do nothing if RT is already held (RT+LT handled below) + pilot.LT.onTrue( + Commands.either( + superStructure.setStateCommand(WantedSuperState.INTAKE_FUEL), + Commands.none(), + pilot.RT.negate())); + + // RT alone → launch; do nothing if LT is already held (RT+LT handled below) + pilot.RT.onTrue( + Commands.either( + superStructure.setStateCommand(WantedSuperState.LAUNCH_WITH_SQUEEZE), + Commands.none(), + pilot.LT.negate())); + + // RT + LT both held → launch (intake stays extended; resolves to LAUNCH_WITHOUT_SQUEEZE) + pilot.RT + .and(pilot.LT) + .onTrue(superStructure.setStateCommand(WantedSuperState.LAUNCH_WITHOUT_SQUEEZE)); + + // LT released while RT still held → launch (no delay; resolves to + // LAUNCH_WITH_SQUEEZE_WITH_NO_DELAY) + pilot.LT.onFalse( + Commands.either( + superStructure.setStateCommand( + WantedSuperState.LAUNCH_WITH_SQUEEZE_WITH_NO_DELAY), + Commands.none(), + pilot.RT)); + + // RT released while LT still held → resume intaking + pilot.RT.onFalse( + Commands.either( + superStructure.setStateCommand(WantedSuperState.INTAKE_FUEL), + Commands.none(), + pilot.LT)); + + // Both released → idle + pilot.RT.or(pilot.LT).onFalse(superStructure.setStateCommand(WantedSuperState.IDLE)); + + pilot.XButton.whileTrue(superStructure.setStateCommand(WantedSuperState.TRACK_TARGET)); + pilot.XButton.onFalse(superStructure.setStateCommand(WantedSuperState.IDLE)); + + pilot.AButton.whileTrue(superStructure.setStateCommand(WantedSuperState.UNJAM)); + pilot.AButton.onFalse(superStructure.setStateCommand(WantedSuperState.IDLE)); + + pilot.selectButton.onTrue(superStructure.setStateCommand(WantedSuperState.FORCE_HOME)); + pilot.selectButton.onFalse(superStructure.setStateCommand(WantedSuperState.IDLE)); + + // operator.dPadDown.onTrue(ShotCalculator.decreaseFlywheelSpeedOffset()); + // operator.dPadUp.onTrue(ShotCalculator.increaseFlywheelSpeedOffset()); + // operator.dPadRight.onTrue(ShotCalculator.decreaseTurretAngleOffsetDegrees()); + // operator.dPadLeft.onTrue(ShotCalculator.increaseTurretAngleOffsetDegrees()); + + // Reset hub shift timer when enabling + Util.teleop.onTrue(Commands.runOnce(ShiftHelpers::initialize)); + Util.autoMode.onTrue(Commands.runOnce(ShiftHelpers::initialize)); + Util.disabled.onTrue(Commands.runOnce(ShiftHelpers::initialize).ignoringDisable(true)); + + // Auton Triggers + Auton.autonIntake.onTrue( + superStructure.setStateCommand(WantedSuperState.AUTON_INTAKE_FUEL)); + Auton.autonShotPrep.onTrue( + superStructure.setStateCommand(WantedSuperState.AUTON_TRACK_TARGET)); + Auton.autonUnjam.onTrue( + Commands.sequence( + superStructure.setStateCommand(WantedSuperState.UNJAM), + Commands.waitSeconds(1), + superStructure.setStateCommand(WantedSuperState.LAUNCH_WITH_SQUEEZE))); + Auton.autonClearState.onTrue(superStructure.setStateCommand(WantedSuperState.IDLE)); + + if (RobotBase.isSimulation()) { + pilot.YButton.whileTrue( + superStructure.setStateCommand(WantedSuperState.LAUNCH_WITH_SQUEEZE)); + pilot.YButton.onFalse(superStructure.setStateCommand(WantedSuperState.IDLE)); + pilot.BButton.whileTrue(superStructure.setStateCommand(WantedSuperState.INTAKE_FUEL)); + pilot.BButton.onFalse(superStructure.setStateCommand(WantedSuperState.IDLE)); + } } - /** - * This method cancels all commands and returns subsystems to their default commands. This - * method should be called when each mode is initialized. - * - *
Warning: This method will cause a very large loop overrun, as it rebinds all states
- * to their triggers. Be careful when you call this as it will cause delays in the robot code.
- * It is recommended to call this method at the end of disabledInit and teleopInit, as those are
- * the most common places to need to reset commands and bindings.
- */
- public void clearCommandsAndButtons() {
- CommandScheduler.getInstance().cancelAll(); // Disable any currently running commands
- CommandScheduler.getInstance().getActiveButtonLoop().clear();
-
- // Bind Triggers for all subsystems
- setupStates();
- RobotStates.setupStates();
+ public void configureSimBindings() {
+ RobotSim.simLaunching().whileTrue(robotSim.ballSimLaunchFuel());
}
/** Sets up the SmartDashboard data for visualization. */
@@ -264,7 +327,6 @@ public void robotPeriodic() {
"Match Data/TimeLeftInShift",
ShiftHelpers.getOfficialShiftInfo().remainingTime(),
"seconds");
- Telemetry.log("Applied State", RobotStates.getAppliedState().toString());
batteryLogger.setBatteryVoltage(RobotController.getBatteryVoltage());
batteryLogger.setRioCurrent(RobotController.getInputCurrent());
@@ -291,8 +353,6 @@ public void robotPeriodic() {
@Override
public void disabledInit() {
Telemetry.print("### Disabled Init Starting ### ");
- clearCommandsAndButtons();
- resetCommandsAndButtons();
if (!autonWarmedUp) {
Command autonStartCommand =
@@ -315,30 +375,30 @@ public void disabledInit() {
@Override
public void disabledPeriodic() {
- String newAutoName;
- boolean leftStart = true;
+ String fullAutoName = auton.getAutonomousCommand().getName();
+ boolean leftStart = !fullAutoName.endsWith(" - Right");
List Extends {@link SpectrumLEDs} to inherit the full pattern library (solid, stripe, blink,
+ * breathe, rainbow, chase, bounce, gradient, ombre, wave, countdown, etc.) and the CANdle hardware
+ * abstraction. Robot-specific convenience command methods are defined below; bind them via triggers
+ * in {@code Robot.java} or {@code SuperStructure}.
+ *
+ * Hardware: CANdle device ID 1 on the CANivore bus, 20-LED RGB external strip at brightness 0.5.
+ * LEDs are disabled on signal loss.
+ */
+public class Leds extends SpectrumLEDs {
+
+ // -------------------------------------------------------------------------
+ // Hardware configuration
+ // -------------------------------------------------------------------------
+
+ /** Number of external LEDs attached to the CANdle output (indices 8–27 on the device). */
+ public static final int NUM_LEDS = 20;
+
+ /**
+ * Static hardware config. Set {@code startIdx = 8} to address only the external strip (skipping
+ * the 8 onboard CANdle LEDs); keep at {@code 0} to address all 20 LEDs starting from the first
+ * onboard LED.
+ */
+ public static final Config ledsConfig;
+
+ static {
+ ledsConfig = new Config("Leds", 1, NUM_LEDS, new CANBus(Rio.CANIVORE));
+ ledsConfig.setStripType(StripTypeValue.RGB);
+ ledsConfig.setBrightness(0.5);
+ ledsConfig.setLossOfSignalBehavior(LossOfSignalBehaviorValue.DisableLEDs);
+ }
+
+ // -------------------------------------------------------------------------
+ // Constructor
+ // -------------------------------------------------------------------------
+
+ public Leds() {
+ super(ledsConfig);
+
+ setDefaultCommand(setPattern(breathe(purple, 2.0), -1).withName("Leds.idle"));
+
+ Telemetry.print(getName() + " Subsystem Initialized");
+ }
+
+ @Override
+ public void periodic() {
+ Telemetry.log("Leds/CurrentCommand", getCurrentCommandName());
+ Telemetry.log("Leds/CommandPriority", getCommandPriority());
+ Telemetry.log("Leds/IsAnimating", isAnimating());
+ }
+}
diff --git a/src/main/java/frc/robot/subsystems/spindexer/Spindexer.java b/src/main/java/frc/robot/subsystems/spindexer/Spindexer.java
new file mode 100644
index 00000000..702ec3f8
--- /dev/null
+++ b/src/main/java/frc/robot/subsystems/spindexer/Spindexer.java
@@ -0,0 +1,153 @@
+package frc.robot.subsystems.spindexer;
+
+import com.ctre.phoenix6.sim.TalonFXSimState;
+import edu.wpi.first.math.util.Units;
+import edu.wpi.first.wpilibj.smartdashboard.Mechanism2d;
+import frc.robot.RobotSim;
+import frc.spectrumLib.hardware.Rio;
+import frc.spectrumLib.mechanism.Mechanism;
+import frc.spectrumLib.sim.RollerConfig;
+import frc.spectrumLib.sim.RollerSim;
+import frc.spectrumLib.telemetry.Telemetry;
+import lombok.Getter;
+import lombok.Setter;
+
+public class Spindexer extends Mechanism {
+
+ public static class SpindexerConfig extends Config {
+
+ @Getter @Setter private double supplyCurrentLimit = 40;
+ @Getter @Setter private double statorCurrentLimit = 80;
+ @Getter @Setter private double velocityKp = 5;
+ @Getter @Setter private double velocityKv = 10;
+ @Getter @Setter private double velocityKs = 15;
+
+ /* Sim Configs */
+ @Getter @Setter
+ private double spindexerX = Units.inchesToMeters(RobotSim.leftViewWidth / 2.0);
+
+ @Getter @Setter
+ private double spindexerY = Units.inchesToMeters(RobotSim.leftViewHeight / 2.0);
+
+ @Getter @Setter private double spindexerDiameter = 12;
+
+ public SpindexerConfig() {
+ super("Spindexer", 8, Rio.CANIVORE);
+ configPIDGains(velocityKp, 0, 0);
+ configFeedForwardGains(velocityKs, velocityKv, 0, 0);
+ configSupplyCurrentLimit(supplyCurrentLimit, true);
+ configStatorCurrentLimit(statorCurrentLimit, true);
+ configClockwise_Positive();
+ }
+ }
+
+ // ---- State Machine ----
+
+ public enum WantedState {
+ OFF,
+ INDEX_MAX,
+ SLOW_INDEX,
+ UNJAM,
+ }
+
+ public enum SystemState {
+ OFF,
+ INDEX_MAX,
+ SLOW_INDEX,
+ UNJAM,
+ }
+
+ private WantedState wantedState = WantedState.OFF;
+ private SystemState systemState = SystemState.OFF;
+
+ public void setWantedState(WantedState state) {
+ this.wantedState = state;
+ }
+
+ private SystemState handleStateTransition() {
+ return switch (wantedState) {
+ case OFF -> SystemState.OFF;
+ case INDEX_MAX -> SystemState.INDEX_MAX;
+ case SLOW_INDEX -> SystemState.SLOW_INDEX;
+ case UNJAM -> SystemState.UNJAM;
+ };
+ }
+
+ // TODO: get actual values when robot is built
+ private void applyStates() {
+ double wantedRPM = 0;
+ switch (systemState) {
+ case OFF:
+ stop();
+ return;
+ case INDEX_MAX:
+ wantedRPM = 3000;
+ break;
+ case SLOW_INDEX:
+ wantedRPM = 1000;
+ break;
+ case UNJAM:
+ wantedRPM = -2000;
+ break;
+ }
+ final double finalWantedRPM = wantedRPM;
+ setVelocityTCFOCrpm(() -> finalWantedRPM);
+ }
+
+ @Getter private final SpindexerConfig config;
+ @Getter private SpindexerSim sim;
+
+ public Spindexer(SpindexerConfig config) {
+ super(config);
+ this.config = config;
+
+ simulationInit();
+ Telemetry.print(getName() + " Subsystem Initialized");
+ }
+
+ @Override
+ public void periodic() {
+ systemState = handleStateTransition();
+ applyStates();
+ logBatteryUsage();
+ Telemetry.log("Spindexer/WantedState", wantedState.toString());
+ Telemetry.log("Spindexer/SystemState", systemState.toString());
+ Telemetry.log("Spindexer/CurrentCommand", getCurrentCommandName());
+ Telemetry.log("Spindexer/Voltage", getVoltage(), "volts");
+ Telemetry.log("Spindexer/StatorCurrent", getStatorCurrent(), "amps");
+ Telemetry.log("Spindexer/SupplyCurrent", getSupplyCurrent(), "amps");
+ Telemetry.log("Spindexer/RPM", getVelocityRPM(), "RPM");
+ Telemetry.log("Spindexer/Temp", getTemp(), "deg_C");
+ }
+
+ // --------------------------------------------------------------------------------
+ // Simulation
+ // --------------------------------------------------------------------------------
+ public void simulationInit() {
+ if (isAttached()) {
+ // Create a new RollerSim with the top view, the motor's sim state, and a 12 in
+ // diameter
+ sim = new SpindexerSim(RobotSim.leftView, motor.getSimState());
+ }
+ }
+
+ // Must be called to enable the simulation
+ // if roller position changes configure x and y to set position.
+ @Override
+ public void simulationPeriodic() {
+ if (isAttached()) {
+ sim.simulationPeriodic();
+ }
+ }
+
+ class SpindexerSim extends RollerSim {
+ public SpindexerSim(Mechanism2d mech, TalonFXSimState rollerMotorSim) {
+ super(
+ new RollerConfig(config.getSpindexerDiameter())
+ .setPosition(config.getSpindexerX(), config.getSpindexerY()),
+ mech,
+ rollerMotorSim,
+ config.getName());
+ }
+ }
+}
diff --git a/src/main/java/frc/robot/swerve/Swerve.java b/src/main/java/frc/robot/subsystems/swerve/Swerve.java
similarity index 61%
rename from src/main/java/frc/robot/swerve/Swerve.java
rename to src/main/java/frc/robot/subsystems/swerve/Swerve.java
index 9236d35c..a654416f 100644
--- a/src/main/java/frc/robot/swerve/Swerve.java
+++ b/src/main/java/frc/robot/subsystems/swerve/Swerve.java
@@ -1,6 +1,6 @@
// Based on
// https://github.com/CrossTheRoadElec/Phoenix6-Examples/blob/main/java/SwerveWithPathPlanner/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java
-package frc.robot.swerve;
+package frc.robot.subsystems.swerve;
import static edu.wpi.first.units.Units.Inches;
import static edu.wpi.first.units.Units.Pounds;
@@ -10,14 +10,17 @@
import com.ctre.phoenix6.hardware.CANcoder;
import com.ctre.phoenix6.hardware.TalonFX;
import com.ctre.phoenix6.swerve.SwerveDrivetrain;
+import com.ctre.phoenix6.swerve.SwerveModule;
import com.ctre.phoenix6.swerve.SwerveModule.DriveRequestType;
import com.ctre.phoenix6.swerve.SwerveModule.SteerRequestType;
import com.ctre.phoenix6.swerve.SwerveRequest;
+import com.ctre.phoenix6.swerve.utility.PhoenixPIDController;
import com.pathplanner.lib.auto.AutoBuilder;
import com.pathplanner.lib.config.PIDConstants;
import com.pathplanner.lib.config.RobotConfig;
import com.pathplanner.lib.controllers.PPHolonomicDriveController;
import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Pose3d;
import edu.wpi.first.math.geometry.Rectangle2d;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.geometry.Translation2d;
@@ -30,46 +33,72 @@
import edu.wpi.first.wpilibj.Notifier;
import edu.wpi.first.wpilibj.Timer;
import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.Subsystem;
import edu.wpi.first.wpilibj2.command.button.Trigger;
import frc.rebuilt.Field;
import frc.rebuilt.FieldHelpers;
+import frc.rebuilt.RobotBumpSim;
+import frc.rebuilt.ShotCalculator;
import frc.robot.Robot;
-import frc.robot.swerve.controllers.RotationController;
-import frc.robot.swerve.controllers.TranslationXController;
-import frc.robot.swerve.controllers.TranslationYController;
-import frc.spectrumLib.SpectrumSubsystem;
-import frc.spectrumLib.Telemetry;
import frc.spectrumLib.swerve.MapleSimSwerveDrivetrain;
+import frc.spectrumLib.telemetry.Telemetry;
import frc.spectrumLib.util.Util;
import java.util.Arrays;
import java.util.Optional;
-import java.util.function.DoubleSupplier;
import java.util.function.Supplier;
import lombok.Getter;
+import lombok.Setter;
/**
* Class that extends the Phoenix SwerveDrivetrain class and implements subsystem so it can be used
* in command-based projects easily.
*/
-public class Swerve extends SwerveDrivetrain Each robot loop iteration the subsystem:
+ *
+ * Instantiates all three Limelights, registers their loggers, applies initial camera
+ * settings (LEDs off, IMU mode 1), loads the AprilTag field layout, and registers this
+ * subsystem with the WPILib scheduler.
+ *
+ * @param config the static configuration object
+ */
+ public Vision(VisionConfig config) {
+ this.config = config;
+
+ backLL = new Limelight(config.backLL, config.backTagPipeline, config.backConfig);
+ leftLL = new Limelight(config.leftLL, config.leftTagPipeline, config.leftConfig);
+ rightLL = new Limelight(config.rightLL, config.rightTagPipeline, config.rightConfig);
+
+ allLimelights = new Limelight[] {backLL, leftLL, rightLL};
+
+ backLogger = new VisionLogger("BackLL", backLL);
+ leftLogger = new VisionLogger("LeftLL", leftLL);
+ rightLogger = new VisionLogger("RightLL", rightLL);
+ allLoggers = new VisionLogger[] {backLogger, leftLogger, rightLogger};
+
+ for (Limelight limelight : allLimelights) {
+ limelight.setLEDMode(false);
+ setImuModeIfChanged(limelight, 1);
+ }
+
+ tagLayout = AprilTagFieldLayout.loadField(AprilTagFields.k2026RebuiltWelded);
+
+ this.register();
+ Telemetry.print(getName() + " Subsystem Initialized");
+ }
+
+ /**
+ * @return the subsystem name defined in {@link VisionConfig}.
+ */
+ @Override
+ public String getName() {
+ return config.getName();
+ }
+
+ // =========================================================================
+ // Subsystem Periodic
+ // =========================================================================
+
+ /**
+ * Called every robot loop iteration by the WPILib scheduler.
+ *
+ * Rejection criteria (any one triggers rejection):
+ *
+ * Accepted estimates are assigned std-dev vectors based on how many tags are visible and how
+ * large the target appears. {@code forceIntegrateXY} overrides the std-devs to near-zero, used
+ * during disabled pre-seeding.
+ *
+ * @param ll the Limelight to query
+ * @param forceIntegrateXY if {@code true}, bypass std-dev selection and use very tight
+ * covariance (disabled pre-seeding)
+ * @return a {@link VisionFieldPoseEstimate} ready to pass to the pose estimator, or {@code
+ * null} if rejected
+ */
+ private VisionFieldPoseEstimate getMT1VisionEstimate(Limelight ll, boolean forceIntegrateXY) {
+ if (!ll.targetInView()) {
+ ll.setTagStatus("No Targets in View");
+ ll.sendInvalidStatus("No Targets in View Rejection");
+ return null;
+ }
+
+ boolean multiTags = ll.multipleTagsInView();
+ double targetSize = ll.getTargetSize();
+ Pose3d megaTag1Pose3d = ll.getMegaTag1_Pose3d();
+ Pose2d megaTag1Pose2d = megaTag1Pose3d.toPose2d();
+ RawFiducial[] tags = ll.getRawFiducial();
+ double highestAmbiguity = -1;
+ ChassisSpeeds robotSpeed = Robot.getSwerve().getCurrentRobotChassisSpeeds();
+ double robotLinearSpeed =
+ Math.hypot(robotSpeed.vxMetersPerSecond, robotSpeed.vyMetersPerSecond);
+
+ // Distance from current odometry pose to the MT1 estimate
+ double mt1PoseDifference =
+ Robot.getSwerve()
+ .getRobotPose()
+ .getTranslation()
+ .getDistance(megaTag1Pose2d.getTranslation());
+
+ // Ambiguity scan — reject immediately if any tag exceeds 0.9
+ ll.setTagStatus("");
+ if (tags != null) {
+ for (RawFiducial tag : tags) {
+ if (highestAmbiguity < 0 || tag.ambiguity > highestAmbiguity) {
+ highestAmbiguity = tag.ambiguity;
+ }
+ if (tag.ambiguity > 0.9) {
+ ll.sendInvalidStatus("High Ambiguity Rejection");
+ return null;
+ }
+ }
+ }
+
+ // Field boundary, spin rate, and target-size rejections
+ if (rejectionCheck(ll, megaTag1Pose2d, targetSize)) {
+ return null;
+ }
+
+ // Reject if the camera pose shows significant roll or pitch (> 5°)
+ if (Math.abs(megaTag1Pose3d.getRotation().getX()) > Math.toRadians(5)
+ || Math.abs(megaTag1Pose3d.getRotation().getY()) > Math.toRadians(5)) {
+ ll.sendInvalidStatus("Roll/Pitch Rejection");
+ return null;
+ }
+
+ // Select std-devs based on confidence tier
+ double xyStds;
+ double degStds;
+
+ if (robotLinearSpeed <= 0.2 && targetSize > 4) {
+ ll.sendValidStatus("Stationary close integration");
+ xyStds = 0.1;
+ degStds = 0.1;
+ } else if (multiTags && targetSize > 2) {
+ ll.sendValidStatus("Strong Multi integration");
+ xyStds = 0.1;
+ degStds = 0.1;
+ } else if (multiTags && targetSize > 0.2) {
+ ll.sendValidStatus("Multi integration");
+ xyStds = 0.25;
+ degStds = 8;
+ } else if (targetSize > 2 && mt1PoseDifference < 0.5) {
+ ll.sendValidStatus("Close integration");
+ xyStds = 0.5;
+ degStds = config.getKLargeVariance();
+ } else if (targetSize > 1 && mt1PoseDifference < 0.25) {
+ ll.sendValidStatus("Proximity integration");
+ xyStds = 1.0;
+ degStds = config.getKLargeVariance();
+ } else if (highestAmbiguity < 0.25 && targetSize >= 0.03) {
+ ll.sendValidStatus("Stable integration");
+ xyStds = 1.5;
+ degStds = config.getKLargeVariance();
+ } else {
+ ll.sendInvalidStatus("Integration Criteria not Met");
+ return null;
+ }
+
+ // Widen heading std-dev when ambiguity is moderate
+ if (highestAmbiguity > 0.5) {
+ degStds = 15;
+ }
+
+ // Discard heading during fast rotation (MegaTag1 heading unreliable while spinning)
+ if (robotSpeed.omegaRadiansPerSecond >= 0.5) {
+ degStds = 50;
+ }
+
+ // Override covariance for disabled pre-seeding
+ if (forceIntegrateXY) {
+ xyStds = 0.01;
+ degStds = 0.01;
+ }
+
+ Pose2d integratedPose =
+ new Pose2d(megaTag1Pose2d.getTranslation(), megaTag1Pose2d.getRotation());
+ double timestamp = Utils.fpgaToCurrentTime(ll.getMegaTag1PoseTimestamp());
+ // The pose estimator expects the heading std-dev in radians; degStds is in degrees.
+ Matrix MegaTag2 heading is always discarded ({@link VisionConfig#kLargeVariance}) because it
+ * relies on the IMU rather than tag geometry. This method is only called while the robot is
+ * disabled.
+ *
+ * Rejection criteria:
+ *
+ * Rejects on:
+ *
+ * Uses {@link DriverStation#getAlliance()} and falls back to Red if the alliance is unknown.
+ */
+ public boolean tagsInView() {
+ DriverStation.Alliance alliance =
+ DriverStation.getAlliance().orElse(DriverStation.Alliance.Blue);
+ int[] allianceTags = (alliance == DriverStation.Alliance.Blue) ? blueTags : redTags;
+ return Arrays.stream(allLimelights)
+ .mapToInt(ll -> (int) ll.getClosestTagID())
+ .anyMatch(id -> Arrays.stream(allianceTags).anyMatch(tag -> tag == id));
+ }
+
+ /**
+ * Triggers a rewind-capture snapshot on all Limelights (captures 165 seconds of history for
+ * post-match review).
+ */
+ public void triggerRewindCaptureForAllCameras() {
+ for (Limelight limelight : allLimelights) {
+ LimelightHelpers.triggerRewindCapture(limelight.getName(), 165);
+ }
+ }
+
+ // =========================================================================
+ // Pose Reset
+ // =========================================================================
+
+ /**
+ * Resets the robot pose to the best Limelight's vision pose. Delegates to {@link
+ * #resetPoseToVision(boolean, Pose3d, Pose2d, double)}.
+ */
+ public void resetPoseToVision() {
+ Limelight ll = getBestLimelight();
+ resetPoseToVision(
+ ll.targetInView(),
+ ll.getMegaTag1_Pose3d(),
+ ll.getMegaTag2_Pose2d(),
+ ll.getMegaTag1PoseTimestamp());
+ }
+
+ /**
+ * Resets the robot pose to a specific vision pose if the data passes sanity checks.
+ *
+ * Uses a very tight covariance ({@code 0.00001}) so the pose estimator snaps immediately to
+ * the vision measurement. The MT1 heading is preserved while MT2 provides the translation.
+ *
+ * Rejection criteria:
+ *
+ * This command ramps up the robot's rotation rate to a specified maximum while recording the
- * change in gyro angle and wheel positions. When the command is cancelled, it calculates and
- * prints the effective wheel radius based on the recorded data.
- */
- public static Command wheelRadiusCharacterization() {
- SlewRateLimiter limiter = new SlewRateLimiter(WHEEL_RADIUS_RAMP_RATE);
- WheelRadiusCharacterizationState state = new WheelRadiusCharacterizationState();
-
- return Commands.parallel(
- // Drive control sequence
- Commands.sequence(
- Commands.runOnce(() -> limiter.reset(0.0)),
- Commands.run(
- () -> {
- double speed = limiter.calculate(WHEEL_RADIUS_MAX_VELOCITY);
- swerve.setControl(
- FIELD_CENTRIC_DRIVE
- .withVelocityX(0)
- .withVelocityY(0)
- .withRotationalRate(speed));
- },
- swerve)),
-
- // Measurement sequence
- Commands.sequence(
- Commands.waitSeconds(1.0),
- Commands.runOnce(
- () -> {
- state.positions = getWheelRadiusCharacterizationPositions();
- state.lastAngle = swerve.getRotation();
- state.gyroDelta = 0.0;
- }),
- Commands.run(
- () -> {
- var rotation = swerve.getRotation();
- state.gyroDelta +=
- Math.abs(
- rotation.minus(state.lastAngle)
- .getRadians());
- state.lastAngle = rotation;
- })
- .finallyDo(
- () -> {
- double[] positions =
- getWheelRadiusCharacterizationPositions();
- double wheelDelta = 0.0;
- for (int i = 0; i < 4; i++) {
- wheelDelta +=
- Math.abs(positions[i] - state.positions[i])
- / 4.0;
- }
- double wheelRadius =
- (state.gyroDelta
- * config
- .getDrivebaseRadiusMeters())
- / wheelDelta;
-
- NumberFormat formatter = new DecimalFormat("#0.000");
- Telemetry.log(
- "WheelRadiusCharacterization/WheelDelta",
- formatter.format(wheelDelta) + " radians");
- Telemetry.log(
- "WheelRadiusCharacterization/GyroDelta",
- formatter.format(state.gyroDelta) + " radians");
- Telemetry.log(
- "WheelRadiusCharacterization/WheelRadiusMeters",
- formatter.format(wheelRadius) + " meters");
- Telemetry.log(
- "WheelRadiusCharacterization/WheelRadiusInches",
- formatter.format(
- Units.metersToInches(
- wheelRadius))
- + " inches");
- })));
- }
-
- private static class WheelRadiusCharacterizationState {
- double[] positions = new double[4];
- Rotation2d lastAngle = Rotation2d.kZero;
- double gyroDelta = 0.0;
- }
-
- // ------------------------------------------------------------------------
- // Reorient commands
- // ------------------------------------------------------------------------
- protected static Command reorientForward() {
- return swerve.reorientPilotAngle(0).withName("Swerve.reorientForward");
- }
-
- protected static Command reorientLeft() {
- return swerve.reorientPilotAngle(90).withName("Swerve.reorientLeft");
- }
-
- protected static Command reorientBack() {
- return swerve.reorientPilotAngle(180).withName("Swerve.reorientBack");
- }
-
- protected static Command reorientRight() {
- return swerve.reorientPilotAngle(270).withName("Swerve.reorientRight");
- }
-
- protected static Command cardinalReorient() {
- return swerve.cardinalReorient().withName("Swerve.cardinalReorient");
- }
-
- // ------------------------------------------------------------------------
- // Telemetry
- // ------------------------------------------------------------------------
- protected static Command log(Command cmd) {
- return Telemetry.log(cmd);
- }
-}
diff --git a/src/main/java/frc/robot/swerve/controllers/RotationController.java b/src/main/java/frc/robot/swerve/controllers/RotationController.java
deleted file mode 100644
index ef42add2..00000000
--- a/src/main/java/frc/robot/swerve/controllers/RotationController.java
+++ /dev/null
@@ -1,89 +0,0 @@
-package frc.robot.swerve.controllers;
-
-import edu.wpi.first.math.controller.PIDController;
-import edu.wpi.first.math.controller.ProfiledPIDController;
-import edu.wpi.first.math.trajectory.TrapezoidProfile.Constraints;
-import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
-import frc.robot.swerve.SwerveConfig;
-
-public class RotationController {
- private final ProfiledPIDController motionController;
- private final PIDController holdController;
- private final SwerveConfig config;
-
- @SuppressWarnings("unused")
- private double lastOutput = 0.0;
-
- private final double deadband = 1e-3;
-
- public RotationController(SwerveConfig config) {
- this.config = config;
-
- motionController =
- new ProfiledPIDController(
- config.getKPRotationController(),
- config.getKIRotationController(),
- config.getKDRotationController(),
- new Constraints(
- config.getMaxAngularVelocity(),
- config.getMaxAngularAcceleration()));
-
- motionController.enableContinuousInput(-Math.PI, Math.PI);
- motionController.setTolerance(config.getRotationTolerance());
- SmartDashboard.putData("PID Controllers/Rotation Controller", motionController);
-
- holdController =
- new PIDController(
- config.getKPHoldController(),
- config.getKIHoldController(),
- config.getKDHoldController());
-
- holdController.enableContinuousInput(-Math.PI, Math.PI);
- holdController.setTolerance(config.getRotationTolerance() / 2.0);
- SmartDashboard.putData("PID Controllers/Hold Controller", holdController);
- }
-
- public double calculate(double goalRadians, double currentRadians, boolean useHold) {
- double output;
-
- if (useHold && atGoal()) {
- output = holdController.calculate(currentRadians, goalRadians);
- } else {
- output = motionController.calculate(currentRadians, goalRadians);
- }
-
- // SmartDashboard.putNumber("Rotation Output (raw)", output);
-
- if (Math.abs(output) > deadband) {
- output += config.getKSsteer() * Math.signum(output);
- }
-
- lastOutput = output;
- // SmartDashboard.putNumber("Rotation Output (final)", output);
- // SmartDashboard.putBoolean("Rotation At Goal", motionController.atGoal());
-
- return output;
- }
-
- public double calculate(double goalRadians, double currentRadians) {
- return calculate(goalRadians, currentRadians, true);
- }
-
- public boolean atGoal() {
- return motionController.atGoal();
- }
-
- public boolean atSetpoint() {
- return motionController.atSetpoint();
- }
-
- public void reset(double currentRadians) {
- motionController.reset(currentRadians);
- holdController.reset();
- lastOutput = 0.0;
- }
-
- public void updatePID(double kP, double kI, double kD) {
- motionController.setPID(kP, kI, kD);
- }
-}
diff --git a/src/main/java/frc/robot/swerve/controllers/TagCenterAlignController.java b/src/main/java/frc/robot/swerve/controllers/TagCenterAlignController.java
deleted file mode 100644
index a64c4061..00000000
--- a/src/main/java/frc/robot/swerve/controllers/TagCenterAlignController.java
+++ /dev/null
@@ -1,65 +0,0 @@
-package frc.robot.swerve.controllers;
-
-import edu.wpi.first.math.controller.PIDController;
-import edu.wpi.first.math.trajectory.TrapezoidProfile.Constraints;
-import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
-import frc.robot.Robot;
-import frc.robot.swerve.Swerve;
-import frc.robot.swerve.SwerveConfig;
-
-/**
- * Uses a profiled PID Controller to quickly turn the robot to a specified angle. Once the robot is
- * within a certain tolerance of the goal angle, a PID controller is used to hold the robot at that
- * angle.
- */
-public class TagCenterAlignController {
- Swerve swerve;
- SwerveConfig config;
- PIDController controller;
- Constraints constraints;
-
- double calculatedValue = 0;
- double maxVelocity;
-
- public TagCenterAlignController(SwerveConfig config) {
- this.config = config;
- maxVelocity = Robot.getConfig().swerve.getSpeedAt12Volts().baseUnitMagnitude() * 0.5;
- controller =
- new PIDController(
- config.getKPTagCenterController(),
- config.getKITagCenterController(),
- config.getKDTagCenterController());
- controller.setTolerance(0.0);
-
- SmartDashboard.putData("PID Controllers/tagCenterController", controller);
- }
-
- public double calculate(double goalMeters, double currentMeters) {
- calculatedValue = controller.calculate(currentMeters, goalMeters);
-
- if (atGoal(currentMeters)) {
- calculatedValue = 0;
- return calculatedValue;
- } else {
- if (Math.abs(calculatedValue) > maxVelocity) {
- calculatedValue = maxVelocity * Math.signum(calculatedValue);
- }
- return calculatedValue + (config.getKSdrive() * Math.signum(calculatedValue));
- }
- }
-
- public boolean atGoal(double current) {
- double goal = controller.getSetpoint();
- boolean atGoal = Math.abs(current - goal) < config.getTagCenterTolerance();
- System.out.println("At Goal: " + atGoal + " Goal: " + goal + " Current: " + current);
- return atGoal;
- }
-
- public boolean atSetpoint() {
- return controller.atSetpoint();
- }
-
- public void updatePID(double kP, double kI, double kD) {
- controller.setPID(kP, kI, kD);
- }
-}
diff --git a/src/main/java/frc/robot/swerve/controllers/TagDistanceAlignController.java b/src/main/java/frc/robot/swerve/controllers/TagDistanceAlignController.java
deleted file mode 100644
index 45725e81..00000000
--- a/src/main/java/frc/robot/swerve/controllers/TagDistanceAlignController.java
+++ /dev/null
@@ -1,59 +0,0 @@
-package frc.robot.swerve.controllers;
-
-import edu.wpi.first.math.controller.PIDController;
-import edu.wpi.first.math.trajectory.TrapezoidProfile.Constraints;
-import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
-import frc.robot.swerve.Swerve;
-import frc.robot.swerve.SwerveConfig;
-
-/**
- * Uses a profiled PID Controller to quickly turn the robot to a specified angle. Once the robot is
- * within a certain tolerance of the goal angle, a PID controller is used to hold the robot at that
- * angle.
- */
-public class TagDistanceAlignController {
- Swerve swerve;
- SwerveConfig config;
- PIDController controller;
- Constraints constraints;
-
- double calculatedValue = 0;
-
- public TagDistanceAlignController(SwerveConfig config) {
- this.config = config;
- controller =
- new PIDController(
- config.getKPTagDistanceController(),
- config.getKITagDistanceController(),
- config.getKDTagDistanceController());
-
- controller.setTolerance(0.0);
- SmartDashboard.putData("PID Controllers/tagDistanceController", controller);
- }
-
- public double calculate(double goalArea, double currentArea) {
- calculatedValue = controller.calculate(currentArea, goalArea);
-
- if (atGoal(currentArea)) {
- calculatedValue = 0;
- return calculatedValue;
- } else {
- return calculatedValue + (config.getKSdrive() * Math.signum(calculatedValue));
- }
- }
-
- public boolean atGoal(double current) {
- double goal = controller.getSetpoint();
- boolean atGoal = Math.abs(current - goal) < config.getTagDistanceTolerance();
- System.out.println("At Goal: " + atGoal + " Goal: " + goal + " Current: " + current);
- return atGoal;
- }
-
- public void reset(double current) {
- // controller.reset(current);
- }
-
- public void updatePID(double kP, double kI, double kD) {
- controller.setPID(kP, kI, kD);
- }
-}
diff --git a/src/main/java/frc/robot/swerve/controllers/TranslationXController.java b/src/main/java/frc/robot/swerve/controllers/TranslationXController.java
deleted file mode 100644
index 900f1d72..00000000
--- a/src/main/java/frc/robot/swerve/controllers/TranslationXController.java
+++ /dev/null
@@ -1,66 +0,0 @@
-package frc.robot.swerve.controllers;
-
-import edu.wpi.first.math.MathUtil;
-import edu.wpi.first.math.controller.ProfiledPIDController;
-import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
-import frc.robot.swerve.SwerveConfig;
-
-/**
- * Uses a profiled PID Controller to quickly turn the robot to a specified angle. Once the robot is
- * within a certain tolerance of the goal angle, a PID controller is used to hold the robot at that
- * angle.
- */
-public class TranslationXController {
- private final SwerveConfig config;
- private final ProfiledPIDController controller;
- private final double deadband = 1e-3;
-
- public TranslationXController(SwerveConfig config) {
- this.config = config;
- this.controller =
- new ProfiledPIDController(
- config.getKPTranslationController(),
- config.getKITranslationController(),
- config.getKDTranslationController(),
- config.getTranslationConstraints());
-
- controller.setTolerance(config.getTranslationTolerance());
- SmartDashboard.putData("PID Controllers/X Controller", controller);
- }
-
- public double calculate(double goalMeters, double currentMeters) {
- if (controller.atGoal()) {
- return 0.0;
- }
-
- double output = controller.calculate(currentMeters, goalMeters);
-
- if (Math.abs(output) > deadband) {
- output += config.getKSdrive() * Math.signum(output);
- }
-
- output =
- MathUtil.clamp(
- output,
- -config.getTranslationConstraints().maxVelocity,
- config.getTranslationConstraints().maxVelocity);
-
- // SmartDashboard.putNumber("X Controller Output", output);
- // SmartDashboard.putBoolean("X At Goal", controller.atGoal());
- // SmartDashboard.putNumber("X Position Error", controller.getPositionError());
- // SmartDashboard.putNumber("X Tolerance", controller.getPositionTolerance());
- return output;
- }
-
- public boolean atGoal() {
- return controller.atGoal();
- }
-
- public void reset(double currentMeters) {
- controller.reset(currentMeters);
- }
-
- public void updatePID(double kP, double kI, double kD) {
- controller.setPID(kP, kI, kD);
- }
-}
diff --git a/src/main/java/frc/robot/swerve/controllers/TranslationYController.java b/src/main/java/frc/robot/swerve/controllers/TranslationYController.java
deleted file mode 100644
index 42c682a5..00000000
--- a/src/main/java/frc/robot/swerve/controllers/TranslationYController.java
+++ /dev/null
@@ -1,68 +0,0 @@
-package frc.robot.swerve.controllers;
-
-import edu.wpi.first.math.MathUtil;
-import edu.wpi.first.math.controller.ProfiledPIDController;
-import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
-import frc.robot.swerve.SwerveConfig;
-
-/**
- * Uses a profiled PID Controller to quickly turn the robot to a specified angle. Once the robot is
- * within a certain tolerance of the goal angle, a PID controller is used to hold the robot at that
- * angle.
- */
-public class TranslationYController {
- private final SwerveConfig config;
- private final ProfiledPIDController controller;
- private final double deadband = 1e-3;
-
- double calculatedValue = 0;
-
- public TranslationYController(SwerveConfig config) {
- this.config = config;
- this.controller =
- new ProfiledPIDController(
- config.getKPTranslationController(),
- config.getKITranslationController(),
- config.getKDTranslationController(),
- config.getTranslationConstraints());
-
- controller.setTolerance(config.getTranslationTolerance());
- SmartDashboard.putData("PID Controllers/Y Controller", controller);
- }
-
- public double calculate(double goalMeters, double currentMeters) {
- if (controller.atGoal()) {
- return 0.0;
- }
-
- double output = controller.calculate(currentMeters, goalMeters);
-
- if (Math.abs(output) > deadband) {
- output += config.getKSdrive() * Math.signum(output);
- }
-
- output =
- MathUtil.clamp(
- output,
- -config.getTranslationConstraints().maxVelocity,
- config.getTranslationConstraints().maxVelocity);
-
- // SmartDashboard.putNumber("Y Controller Output", output);
- // SmartDashboard.putBoolean("Y At Goal", controller.atGoal());
- // SmartDashboard.putNumber("Y Position Error", controller.getPositionError());
- // SmartDashboard.putNumber("Y Tolerance", controller.getPositionTolerance());
- return output;
- }
-
- public boolean atGoal() {
- return controller.atGoal();
- }
-
- public void reset(double currentMeters) {
- controller.reset(currentMeters);
- }
-
- public void updatePID(double kP, double kI, double kD) {
- controller.setPID(kP, kI, kD);
- }
-}
diff --git a/src/main/java/frc/robot/vision/Vision.java b/src/main/java/frc/robot/vision/Vision.java
deleted file mode 100644
index 4aecc233..00000000
--- a/src/main/java/frc/robot/vision/Vision.java
+++ /dev/null
@@ -1,726 +0,0 @@
-package frc.robot.vision;
-
-import com.ctre.phoenix6.Utils;
-import edu.wpi.first.apriltag.AprilTagFieldLayout;
-import edu.wpi.first.apriltag.AprilTagFields;
-import edu.wpi.first.math.Matrix;
-import edu.wpi.first.math.VecBuilder;
-import edu.wpi.first.math.geometry.Pose2d;
-import edu.wpi.first.math.geometry.Pose3d;
-import edu.wpi.first.math.geometry.Rotation2d;
-import edu.wpi.first.math.geometry.Transform2d;
-import edu.wpi.first.math.geometry.Translation2d;
-import edu.wpi.first.math.kinematics.ChassisSpeeds;
-import edu.wpi.first.math.numbers.N1;
-import edu.wpi.first.math.numbers.N3;
-import edu.wpi.first.math.util.Units;
-import edu.wpi.first.wpilibj.DriverStation;
-import edu.wpi.first.wpilibj2.command.Command;
-import edu.wpi.first.wpilibj2.command.Subsystem;
-import frc.rebuilt.FieldHelpers;
-import frc.robot.Robot;
-import frc.robot.RobotStates;
-import frc.robot.auton.Auton;
-import frc.spectrumLib.Telemetry;
-import frc.spectrumLib.Telemetry.PrintPriority;
-import frc.spectrumLib.util.Util;
-import frc.spectrumLib.vision.Limelight;
-import frc.spectrumLib.vision.Limelight.LimelightConfig;
-import frc.spectrumLib.vision.LimelightHelpers;
-import frc.spectrumLib.vision.LimelightHelpers.RawFiducial;
-import java.util.ArrayList;
-import java.util.Arrays;
-import java.util.Comparator;
-import java.util.List;
-import lombok.Getter;
-
-public class Vision implements Subsystem {
-
- public static class VisionConfig {
- @Getter final String name = "Vision";
-
- /* Limelight Configuration */
- @Getter final String backLL = "limelight-back";
-
- @Getter
- final LimelightConfig backConfig =
- new LimelightConfig(backLL)
- .withTranslation(-0.3084987734, 0.2134100126, 0.6502249886)
- .withRotation(0, 0, Math.toRadians(180));
-
- @Getter final String leftLL = "limelight-left";
-
- @Getter
- final LimelightConfig leftConfig =
- new LimelightConfig(leftLL)
- .withTranslation(0, 0.215, 0.188)
- .withRotation(0, 0, Math.toRadians(90));
-
- @Getter final String rightLL = "limelight-right";
-
- @Getter
- final LimelightConfig rightConfig =
- new LimelightConfig(rightLL)
- .withTranslation(-0.04445, 0.3027487722, 0.7137249886)
- .withRotation(0, 0, -90);
-
- @Getter
- final Translation2d robotToTurretCenter =
- new Translation2d(Units.inchesToMeters(-5.5), Units.inchesToMeters(4.7));
-
- @Getter
- final Translation2d turretCenterToCamera =
- new Translation2d(Units.inchesToMeters(-5.641455), 0);
-
- /* Pipeline configs */
- @Getter final int backTagPipeline = 0;
- @Getter final int leftTagPipeline = 0;
- @Getter final int rightTagPipeline = 0;
-
- /* Pose Estimation Constants */
- @Getter double visionStdDevX = 0.5;
- @Getter double visionStdDevY = 0.5;
- @Getter double visionStdDevTheta = 0.2;
-
- @Getter
- final double kLargeVariance = 999999.0; // Don't fuse rotation if variance exceeds this
-
- @Getter final double kMaxTimeDeltaSeconds = 0.1; // Max time difference to consider fusion
-
- @Getter
- final Matrix Wraps a WPILib {@link CommandXboxController} and exposes:
+ *
+ * Subclass this once per operator role (pilot, copilot) and override {@link #setupStates()} and
+ * {@link #setupDefaultCommand()} to bind subsystem commands to triggers.
+ *
+ * When {@link Config#isAttached()} returns {@code false}, all triggers remain permanently {@code
+ * false} and axis reads return {@code 0.0}.
+ */
// Gamepad class
-public abstract class Gamepad implements SpectrumSubsystem {
+public abstract class Gamepad implements Subsystem {
+ /** WPILib alert displayed on the driver station when this gamepad is disconnected. */
private Alert disconnectedAlert;
+ /** A trigger that is always {@code false}; used as a safe default before hardware is ready. */
public static final Trigger kFalse = new Trigger(() -> false);
+ /** The underlying WPILib Xbox controller used to read button and axis states. */
private CommandXboxController xboxController;
+
+ /** Trigger for the A (cross) face button. */
protected Trigger A = kFalse;
+
+ /** Trigger for the B (circle) face button. */
protected Trigger B = kFalse;
+
+ /** Trigger for the X (square) face button. */
protected Trigger X = kFalse;
+
+ /** Trigger for the Y (triangle) face button. */
protected Trigger Y = kFalse;
+
+ /** Trigger for the left bumper (LB). */
protected Trigger leftBumper = kFalse;
+
+ /** Trigger for the right bumper (RB). */
protected Trigger rightBumper = kFalse;
+
+ /** Trigger active when the left analog trigger exceeds the configured deadzone threshold. */
protected Trigger leftTrigger = kFalse;
+
+ /** Trigger active when the right analog trigger exceeds the configured deadzone threshold. */
protected Trigger rightTrigger = kFalse;
+
+ /** Trigger for pressing the left analog stick (L3). */
protected Trigger leftStickClick = kFalse;
+
+ /** Trigger for pressing the right analog stick (R3). */
protected Trigger rightStickClick = kFalse;
+
+ /** Trigger for the Start / Menu button. */
protected Trigger start = kFalse;
+
+ /** Trigger for the Select / Back / View button. */
protected Trigger select = kFalse;
+
+ /** Trigger for D-pad up. */
protected Trigger upDpad = kFalse;
+
+ /** Trigger for D-pad down. */
protected Trigger downDpad = kFalse;
+
+ /** Trigger for D-pad left (including up-left and down-left diagonals). */
protected Trigger leftDpad = kFalse;
+
+ /** Trigger for D-pad right (including up-right and down-right diagonals). */
protected Trigger rightDpad = kFalse;
+
+ /** Trigger active when the left stick Y-axis exceeds the configured deadzone. */
protected Trigger leftStickY = kFalse;
+
+ /** Trigger active when the left stick X-axis exceeds the configured deadzone. */
protected Trigger leftStickX = kFalse;
+
+ /** Trigger active when the right stick Y-axis exceeds the configured deadzone. */
protected Trigger rightStickY = kFalse;
+
+ /** Trigger active when the right stick X-axis exceeds the configured deadzone. */
protected Trigger rightStickX = kFalse;
// Function bumper and trigger buttons
+
+ /** Active when neither bumper is pressed. */
public Trigger noBumpers;
+
+ /** Active when only the left bumper is pressed. */
public Trigger leftBumperOnly;
+
+ /** Active when only the right bumper is pressed. */
public Trigger rightBumperOnly;
+
+ /** Active when both bumpers are pressed simultaneously. */
public Trigger bothBumpers;
+
+ /** Active when neither analog trigger is pressed. */
public Trigger noTriggers;
+
+ /** Active when only the left trigger is pressed. */
public Trigger leftTriggerOnly;
+
+ /** Active when only the right trigger is pressed. */
public Trigger rightTriggerOnly;
+
+ /** Active when both analog triggers are pressed simultaneously. */
public Trigger bothTriggers;
+
+ /** Active when no bumpers and no triggers are pressed (no modifier held). */
public Trigger noModifiers;
+ /** Most recently computed left-stick direction; retained when the stick returns to center. */
private Rotation2d storedLeftStickDirection = new Rotation2d();
+
+ /** Most recently computed right-stick direction; retained when the stick returns to center. */
private Rotation2d storedRightStickDirection = new Rotation2d();
+
+ /**
+ * {@code true} once the gamepad has been detected as connected and its triggers have been
+ * configured.
+ */
private boolean configured =
false; // Used to determine if we detected the gamepad is plugged and we have configured
// it
+
+ /** {@code true} after the "gamepad not connected" warning has been printed once. */
private boolean printed = false; // Used to only print Gamepad Not Detected once
+ /** Exponential response curve applied to both left-stick axes. */
@Getter protected final ExpCurve leftStickCurve;
+
+ /** Exponential response curve applied to both right-stick axes. */
@Getter protected final ExpCurve rightStickCurve;
+
+ /** Exponential response curve applied to both analog trigger axes. */
@Getter protected final ExpCurve triggersCurve;
+ /** Trigger active during the teleoperated period. */
protected Trigger teleop = Util.teleop;
+
+ /** Trigger active during the autonomous period. */
protected Trigger autoMode = Util.autoMode;
+
+ /** Trigger active during the test mode period. */
protected Trigger testMode = Util.testMode;
+
+ /** Trigger active while the robot is disabled. */
protected Trigger disabled = Util.disabled;
+ /**
+ * Configuration for a {@link Gamepad} instance, defining the DriverStation USB port, axis curve
+ * parameters, and whether the controller should be used on this robot.
+ */
public static class Config {
+ /** Human-readable controller name used in alerts and telemetry. */
@Getter private String name;
+
+ /** USB port number as shown in the DriverStation application (0-indexed). */
@Getter private int port; // USB port on the DriverStation app
// A configured value to say if we should use this controller on this robot
+ /**
+ * Whether this controller should be used on the current robot; {@code false} disables it.
+ */
@Getter @Setter private boolean attached;
+ /** Deadzone applied to both left-stick axes before the exponential curve. */
@Getter @Setter double leftStickDeadzone = 0.001;
+
+ /** Exponent for the left-stick exponential response curve (1.0 = linear). */
@Getter @Setter double leftStickExp = 1.0;
+
+ /** Output scalar applied after the left-stick exponential curve. */
@Getter @Setter double leftStickScalar = 1.0;
+ /** Deadzone applied to both right-stick axes before the exponential curve. */
@Getter @Setter double rightStickDeadzone = 0.001;
+
+ /** Exponent for the right-stick exponential response curve. */
@Getter @Setter double rightStickExp = 1.0;
+
+ /** Output scalar applied after the right-stick exponential curve. */
@Getter @Setter double rightStickScalar = 1.0;
+ /** Deadzone applied to both analog trigger axes before the exponential curve. */
@Getter @Setter double triggersDeadzone = 0.002;
+
+ /** Exponent for the analog-trigger exponential response curve. */
@Getter @Setter double triggersExp = 1.0;
+
+ /** Output scalar applied after the analog-trigger exponential curve. */
@Getter @Setter double triggersScalar = 1.0;
+ /**
+ * Creates a gamepad configuration for the given port.
+ *
+ * @param name human-readable controller name (used in alerts)
+ * @param port DriverStation USB port number (0-indexed)
+ */
public Config(String name, int port) {
this.name = name;
this.port = port;
@@ -159,11 +295,13 @@ protected Gamepad(Config config) {
leftDpad =
xboxController
.povLeft()
- .or(xboxController.povUpLeft(), xboxController.povDownLeft());
+ .or(xboxController.povUpLeft())
+ .or(xboxController.povDownLeft());
rightDpad =
xboxController
.povRight()
- .or(xboxController.povDownRight(), xboxController.povUpRight());
+ .or(xboxController.povDownRight())
+ .or(xboxController.povUpRight());
leftStickY = leftYTrigger(Threshold.ABS_GREATER, config.leftStickDeadzone);
leftStickX = leftXTrigger(Threshold.ABS_GREATER, config.leftStickDeadzone);
rightStickY = rightYTrigger(Threshold.ABS_GREATER, config.rightStickDeadzone);
@@ -189,6 +327,11 @@ public void periodic() {
configure();
}
+ /**
+ * Detects whether the gamepad has been connected since power-on and prints a one-time
+ * confirmation message. Also raises a {@link Alert} whenever the controller is disconnected.
+ * Called automatically by {@link #periodic()}.
+ */
// Configure the pilot controller
public void configure() {
if (config.isAttached()) {
@@ -210,15 +353,23 @@ public void configure() {
}
}
- // Reset the controller configure, should be used with
- // CommandScheduler.getInstance.clearButtons()
- // to reset buttons
+ /**
+ * Resets the controller configuration state so that the next {@link #configure()} call will
+ * re-detect connection and re-apply button bindings. Should be paired with {@code
+ * CommandScheduler.getInstance().clearButtons()}.
+ */
public void resetConfig() {
configured = false;
configure();
}
- /* Zero is stick up, 90 is stick to the left*/
+ /**
+ * Returns the current direction of the left stick as a {@link Rotation2d}. Zero points up
+ * (toward positive Y), and 90° points to the left (toward negative X). The last non-zero
+ * direction is retained when the stick is released.
+ *
+ * @return left-stick direction; zero-up / 90-left convention
+ */
public Rotation2d getLeftStickDirection() {
double x = -1 * getLeftX();
double y = -1 * getLeftY();
@@ -229,6 +380,12 @@ public Rotation2d getLeftStickDirection() {
return storedLeftStickDirection;
}
+ /**
+ * Returns the current direction of the right stick as a {@link Rotation2d}. The last non-zero
+ * direction is retained when the stick is released.
+ *
+ * @return right-stick direction
+ */
public Rotation2d getRightStickDirection() {
double x = getRightX();
double y = getRightY();
@@ -239,6 +396,11 @@ public Rotation2d getRightStickDirection() {
return storedRightStickDirection;
}
+ /**
+ * Snaps the left-stick direction to the nearest cardinal angle (0, ±π/2, π radians).
+ *
+ * @return the snapped angle in radians
+ */
public double getLeftStickCardinals() {
double stickAngle = getLeftStickDirection().getRadians();
if (stickAngle > -Math.PI / 4 && stickAngle <= Math.PI / 4) {
@@ -252,6 +414,11 @@ public double getLeftStickCardinals() {
}
}
+ /**
+ * Snaps the right-stick direction to the nearest cardinal angle (0, ±π/2, π radians).
+ *
+ * @return the snapped angle in radians
+ */
public double getRightStickCardinals() {
double stickAngle = getRightStickDirection().getRadians();
if (stickAngle > -Math.PI / 4 && stickAngle <= Math.PI / 4) {
@@ -265,12 +432,23 @@ public double getRightStickCardinals() {
}
}
+ /**
+ * Returns the Euclidean magnitude of the left stick deflection (0–√2 before curve, 0–1 after
+ * typical scalar).
+ *
+ * @return left-stick vector magnitude
+ */
public double getLeftStickMagnitude() {
double x = -1 * getLeftX();
double y = -1 * getLeftY();
return Math.sqrt(x * x + y * y);
}
+ /**
+ * Returns the Euclidean magnitude of the right stick deflection.
+ *
+ * @return right-stick vector magnitude
+ */
public double getRightStickMagnitude() {
double x = getRightX();
double y = getRightY();
@@ -290,6 +468,12 @@ public double chooseCardinalDirections() {
return getBlueAllianceStickCardinals();
}
+ /**
+ * Snaps the right stick to the nearest 45° increment using the Blue-alliance field orientation
+ * (forward = 0 rad).
+ *
+ * @return the snapped heading in radians for the Blue alliance perspective
+ */
public double getBlueAllianceStickCardinals() {
double stickAngle = getRightStickDirection().getRadians();
if (stickAngle > -Math.PI / 8 && stickAngle <= Math.PI / 8) {
@@ -342,27 +526,73 @@ else if (stickAngle < -Math.PI / 8 && stickAngle >= -3 * Math.PI / 8) {
}
}
+ /**
+ * Returns a {@link Trigger} that fires based on the left-stick Y axis and the given threshold
+ * comparison.
+ *
+ * @param t the {@link Threshold} comparison type
+ * @param threshold the value to compare against
+ * @return trigger based on the left Y axis
+ */
public Trigger leftYTrigger(Threshold t, double threshold) {
return axisTrigger(t, threshold, this::getLeftY);
}
+ /**
+ * Returns a {@link Trigger} that fires based on the left-stick X axis and the given threshold
+ * comparison.
+ *
+ * @param t the {@link Threshold} comparison type
+ * @param threshold the value to compare against
+ * @return trigger based on the left X axis
+ */
public Trigger leftXTrigger(Threshold t, double threshold) {
return axisTrigger(t, threshold, this::getLeftX);
}
+ /**
+ * Returns a {@link Trigger} that fires based on the right-stick Y axis and the given threshold
+ * comparison.
+ *
+ * @param t the {@link Threshold} comparison type
+ * @param threshold the value to compare against
+ * @return trigger based on the right Y axis
+ */
public Trigger rightYTrigger(Threshold t, double threshold) {
return axisTrigger(t, threshold, this::getRightY);
}
+ /**
+ * Returns a {@link Trigger} that fires based on the right-stick X axis and the given threshold
+ * comparison.
+ *
+ * @param t the {@link Threshold} comparison type
+ * @param threshold the value to compare against
+ * @return trigger based on the right X axis
+ */
public Trigger rightXTrigger(Threshold t, double threshold) {
return axisTrigger(t, threshold, this::getRightX);
}
+ /**
+ * Returns a {@link Trigger} that fires when either right-stick axis exceeds the given absolute
+ * threshold.
+ *
+ * @param threshold minimum absolute axis value to activate the trigger
+ * @return trigger active when the right stick is deflected beyond the threshold
+ */
public Trigger rightStick(double threshold) {
return new Trigger(
() -> Math.abs(getRightX()) >= threshold || Math.abs(getRightY()) >= threshold);
}
+ /**
+ * Returns a {@link Trigger} that fires when either left-stick axis exceeds the given absolute
+ * threshold.
+ *
+ * @param threshold minimum absolute axis value to activate the trigger
+ * @return trigger active when the left stick is deflected beyond the threshold
+ */
public Trigger leftStick(double threshold) {
return new Trigger(
() -> Math.abs(getLeftX()) >= threshold || Math.abs(getLeftY()) >= threshold);
@@ -385,9 +615,16 @@ private Trigger axisTrigger(Threshold t, double threshold, DoubleSupplier v) {
});
}
+ /**
+ * Comparison type used by axis-based {@link Trigger} factories such as {@link
+ * #leftYTrigger(Threshold, double)}.
+ */
public enum Threshold {
+ /** Fires when the axis value is strictly greater than the threshold. */
GREATER,
+ /** Fires when the axis value is strictly less than the threshold. */
LESS,
+ /** Fires when the absolute axis value is greater than the threshold (deadband check). */
ABS_GREATER;
}
@@ -439,6 +676,11 @@ public Command rumbleCommand(Command command) {
return command.alongWith(rumbleCommand(1, 0.5)).withName(command.getName());
}
+ /**
+ * Returns whether the physical gamepad is currently connected to the DriverStation.
+ *
+ * @return {@code true} if the controller is attached and reports as connected
+ */
public boolean isConnected() {
if (config.attached) {
return this.getHID().isConnected();
@@ -447,6 +689,11 @@ public boolean isConnected() {
}
}
+ /**
+ * Returns the raw right-trigger axis value (0–1), or {@code 0.0} if not connected.
+ *
+ * @return right-trigger axis value
+ */
protected double getRightTriggerAxis() {
if (!isConnected()) {
return 0.0;
@@ -454,6 +701,11 @@ protected double getRightTriggerAxis() {
return xboxController.getRightTriggerAxis();
}
+ /**
+ * Returns the raw left-trigger axis value (0–1), or {@code 0.0} if not connected.
+ *
+ * @return left-trigger axis value
+ */
protected double getLeftTriggerAxis() {
if (!isConnected()) {
return 0.0;
@@ -461,6 +713,12 @@ protected double getLeftTriggerAxis() {
return xboxController.getLeftTriggerAxis();
}
+ /**
+ * Returns the differential trigger value ({@code rightTrigger - leftTrigger}), useful as a
+ * single "twist" axis for field-relative rotation commands.
+ *
+ * @return twist value in the range [-1, 1]
+ */
protected double getTwist() {
double right = getRightTriggerAxis();
double left = getLeftTriggerAxis();
@@ -468,6 +726,11 @@ protected double getTwist() {
return value;
}
+ /**
+ * Returns the left-stick X axis value, or {@code 0.0} if not connected.
+ *
+ * @return left X axis value in the range [-1, 1]
+ */
protected double getLeftX() {
if (!isConnected()) {
return 0.0;
@@ -475,6 +738,11 @@ protected double getLeftX() {
return xboxController.getLeftX();
}
+ /**
+ * Returns the left-stick Y axis value, or {@code 0.0} if not connected.
+ *
+ * @return left Y axis value in the range [-1, 1] (negative = up on most gamepads)
+ */
protected double getLeftY() {
if (!isConnected()) {
return 0.0;
@@ -482,6 +750,11 @@ protected double getLeftY() {
return xboxController.getLeftY();
}
+ /**
+ * Returns the right-stick X axis value, or {@code 0.0} if not connected.
+ *
+ * @return right X axis value in the range [-1, 1]
+ */
protected double getRightX() {
if (!isConnected()) {
return 0.0;
@@ -489,6 +762,11 @@ protected double getRightX() {
return xboxController.getRightX();
}
+ /**
+ * Returns the right-stick Y axis value, or {@code 0.0} if not connected.
+ *
+ * @return right Y axis value in the range [-1, 1] (negative = up on most gamepads)
+ */
protected double getRightY() {
if (!isConnected()) {
return 0.0;
@@ -496,6 +774,12 @@ protected double getRightY() {
return xboxController.getRightY();
}
+ /**
+ * Returns the underlying {@link GenericHID} for low-level access, or {@code null} if not
+ * attached.
+ *
+ * @return the raw HID device, or {@code null}
+ */
protected GenericHID getHID() {
if (!config.attached) {
return null;
@@ -503,6 +787,12 @@ protected GenericHID getHID() {
return xboxController.getHID();
}
+ /**
+ * Returns the underlying {@link GenericHID} for rumble output, or {@code null} if not
+ * connected.
+ *
+ * @return the raw HID device (only when connected), or {@code null}
+ */
protected GenericHID getRumbleHID() {
if (!isConnected()) {
return null;
@@ -510,6 +800,13 @@ protected GenericHID getRumbleHID() {
return xboxController.getHID();
}
+ /**
+ * Immediately sets the left and right rumble motor intensities. Use {@link
+ * #rumbleCommand(double, double, double)} for timed rumble sequences.
+ *
+ * @param leftIntensity left rumble motor intensity (0–1)
+ * @param rightIntensity right rumble motor intensity (0–1)
+ */
public void rumbleController(double leftIntensity, double rightIntensity) {
if (!isConnected()) {
return;
diff --git a/src/main/java/frc/spectrumLib/Rio.java b/src/main/java/frc/spectrumLib/hardware/Rio.java
similarity index 65%
rename from src/main/java/frc/spectrumLib/Rio.java
rename to src/main/java/frc/spectrumLib/hardware/Rio.java
index f08662b8..66845ad4 100644
--- a/src/main/java/frc/spectrumLib/Rio.java
+++ b/src/main/java/frc/spectrumLib/hardware/Rio.java
@@ -1,29 +1,31 @@
-package frc.spectrumLib;
+package frc.spectrumLib.hardware;
import edu.wpi.first.wpilibj.Alert;
import edu.wpi.first.wpilibj.Alert.AlertType;
import edu.wpi.first.wpilibj.RobotBase;
import edu.wpi.first.wpilibj.RobotController;
+import frc.spectrumLib.telemetry.Telemetry;
import java.util.HashMap;
import java.util.Map;
-/*
- * Represents a specific RoboRIO, as a key for configurations.
+/**
+ * Identifies the specific RoboRIO that is running, keyed by its serial number. Used to select
+ * robot-specific configurations at startup.
*
- * The serial numbers here can be found on the label on the back: add a leading zero.
+ * Serial numbers are printed on the label on the back of the RoboRIO — prefix with a leading
+ * zero if needed. Keep entries in lexical order. Note that the serial number may change after
+ * reflashing the RoboRIO.
*
- * Please keep the ID strings in lexical order.
- *
- * Note that the ID string may change when you reflash the RoboRIO.
- * Based on: https://github.com/Team100/all24/blob/2a109b28467cfddcafb93c7fc85ef60b56a628a2/lib/src/main/java/org/team100/lib/config/Identity.java
+ * Based on:
+ * https://github.com/Team100/all24/blob/2a109b28467cfddcafb93c7fc85ef60b56a628a2/lib/src/main/java/org/team100/lib/config/Identity.java
*/
-
public enum Rio {
// 2026 Robots
PHOTON2026("032B4BB3", true),
PM_2026("0329AD07", true),
- // FM_2026("", true),
+ FM_2026("", true),
+ OM_2026("", true),
// 2025 Robots
FM_2025("0329F2D1", true),
@@ -34,6 +36,7 @@ public enum Rio {
SIM("", true), // e.g. test default or simulation
UNKNOWN(null, true);
+ /** Map from serial-number string to the corresponding {@link Rio} enum constant. */
private static final Map Patterns are split into two categories:
+ *
+ * Multiple {@code SpectrumLEDs} instances can share a single physical {@link CANdle} device by
+ * passing the same {@link CANdle} reference in their {@link Config} objects and selecting
+ * non-overlapping {@code startIdx}/{@code numLeds} ranges.
+ *
+ * Patterns are applied via {@link #setPattern(CANdlePattern, int)}, which returns a {@link
+ * Command} that runs continuously and respects the priority system ({@link #checkPriority(int)}).
+ */
+public class SpectrumLEDs implements Subsystem {
+
+ // -------------------------------------------------------------------------
+ // CANdlePattern functional interface
+ // -------------------------------------------------------------------------
- // Example Animation List - https://github.com/Aircoookie/WLED/wiki/List-of-effects-and-palettes
+ /**
+ * Functional interface for a LED pattern that drives a segment of a {@link CANdle} strip.
+ *
+ * Implementations receive the live {@link CANdle} device, the first LED index in this
+ * instance's segment ({@code startIdx}), and the number of LEDs in the segment ({@code
+ * numLeds}). Hardware animation patterns call {@link CANdle#setControl}, software patterns call
+ * {@link CANdle#setControl(SolidColor)} (one-shot, resent each loop).
+ */
+ @FunctionalInterface
+ public interface CANdlePattern {
+ /**
+ * Apply this pattern to the given LED segment.
+ *
+ * @param candle the {@link CANdle} device to write to
+ * @param startIdx the first LED index (inclusive); {@code 0} includes the 8 onboard LEDs
+ * @param numLeds the number of LEDs in this segment
+ */
+ void applyTo(CANdle candle, int startIdx, int numLeds);
+ }
+
+ /**
+ * Internal marker wrapper — returned by animation factory methods so that {@link
+ * #setPattern(CANdlePattern, int)} can detect when a transition from animation to software
+ * pattern occurs and clear the animation slots.
+ */
+ private static final class HardwareAnimPattern implements CANdlePattern {
+ private final CANdlePattern impl;
+
+ HardwareAnimPattern(CANdlePattern impl) {
+ this.impl = impl;
+ }
+ @Override
+ public void applyTo(CANdle candle, int startIdx, int numLeds) {
+ impl.applyTo(candle, startIdx, numLeds);
+ }
+ }
+
+ /** Wraps a pattern lambda in a {@link HardwareAnimPattern} marker. */
+ private static CANdlePattern hardwareAnim(CANdlePattern p) {
+ return new HardwareAnimPattern(p);
+ }
+
+ // -------------------------------------------------------------------------
+ // Config
+ // -------------------------------------------------------------------------
+
+ /**
+ * Configuration for a {@link SpectrumLEDs} subsystem instance.
+ *
+ * Use {@link #Config(String, int, int, CANBus)} to create a standalone instance that owns
+ * and configures its {@link CANdle}, or {@link #Config(String, CANdle, int, int)} to share an
+ * already-configured device across multiple subsystems targeting different LED segments.
+ */
public static class Config {
+ /** Human-readable name used in telemetry. */
@Getter private String name;
+
+ /** Whether this LED strip is physically connected to the robot. */
@Getter @Setter private boolean attached = true;
- @Getter @Setter private AddressableLED led;
- @Getter @Setter private AddressableLEDBuffer buffer;
- @Getter @Setter private AddressableLEDBufferView view;
- @Getter @Setter private int startingIndex = 0;
- @Getter @Setter private int endingIndex = 0;
- @Getter @Setter private int port = 0;
- @Getter @Setter private int length;
- // LED strip density
- @Getter @Setter private Distance ledSpacing = Meters.of(1 / 120.0);
-
- public Config(String name, int length) {
+
+ /**
+ * Pre-built {@link CANdle} to reuse. When non-null, {@link #deviceId} and {@link #canBus}
+ * are ignored and no hardware configuration is applied by this instance.
+ */
+ @Getter @Setter private CANdle sharedCandle = null;
+
+ /** CAN device ID used when no {@link #sharedCandle} is provided. */
+ @Getter @Setter private int deviceId = 1;
+
+ /** CAN bus used when no {@link #sharedCandle} is provided. */
+ @Getter @Setter private CANBus canBus;
+
+ /**
+ * First LED index (inclusive) in the strip. Use {@code 0} to include the 8 onboard status
+ * LEDs on the CANdle board itself; use {@code 8} to skip them.
+ */
+ @Getter @Setter private int startIdx = 0;
+
+ /** Number of LEDs in the segment owned by this instance. */
+ @Getter @Setter private int numLeds;
+
+ /**
+ * CANdle hardware animation slot (0–7) used by this instance's animation patterns. Each
+ * {@link SpectrumLEDs} instance sharing a single {@link CANdle} must use a distinct slot,
+ * otherwise their animations overwrite each other.
+ */
+ @Getter @Setter private int animationSlot = 0;
+
+ /** LED strip type (RGB, RGBW, GRB, etc.). Ignored when {@link #sharedCandle} is set. */
+ @Getter @Setter private StripTypeValue stripType = StripTypeValue.RGB;
+
+ /**
+ * Overall brightness scalar applied in hardware (0.0–1.0). Ignored when {@link
+ * #sharedCandle} is set.
+ */
+ @Getter @Setter private double brightness = 1.0;
+
+ /**
+ * Behavior of the strip when CAN signal is lost. Ignored when {@link #sharedCandle} is set.
+ */
+ @Getter @Setter
+ private LossOfSignalBehaviorValue lossOfSignalBehavior =
+ LossOfSignalBehaviorValue.DisableLEDs;
+
+ /**
+ * Creates a configuration for a standalone LED instance that creates and owns its own
+ * {@link CANdle}. Hardware configuration (strip type, brightness, loss-of-signal) is
+ * applied automatically in the constructor.
+ *
+ * @param name human-readable name for telemetry
+ * @param deviceId CAN device ID of the CANdle
+ * @param numLeds number of LEDs on the external strip (not counting the 8 onboard LEDs)
+ * @param canBus CAN bus the CANdle is on
+ */
+ public Config(String name, int deviceId, int numLeds, CANBus canBus) {
this.name = name;
- this.length = length;
- this.startingIndex = 0;
- this.endingIndex = length - 1;
+ this.deviceId = deviceId;
+ this.numLeds = numLeds;
+ this.canBus = canBus;
}
- public Config(
- String name,
- AddressableLED l,
- AddressableLEDBuffer lb,
- int startingIndex,
- int endingIndex) {
+ /**
+ * Creates a configuration for a LED zone that shares an existing, already-configured {@link
+ * CANdle} device. No hardware configuration is applied.
+ *
+ * @param name human-readable name for telemetry
+ * @param sharedCandle the {@link CANdle} to reuse
+ * @param startIdx first LED index (inclusive) in the shared strip for this zone
+ * @param numLeds number of LEDs in this zone
+ */
+ public Config(String name, CANdle sharedCandle, int startIdx, int numLeds) {
this.name = name;
- this.led = l;
- this.buffer = lb;
- this.startingIndex = startingIndex;
- this.endingIndex = endingIndex;
+ this.sharedCandle = sharedCandle;
+ this.startIdx = startIdx;
+ this.numLeds = numLeds;
}
}
+ // -------------------------------------------------------------------------
+ // Fields
+ // -------------------------------------------------------------------------
+
+ /** Active configuration for this instance. */
@Getter private Config config;
- @Getter protected final AddressableLED led;
- @Getter protected final AddressableLEDBuffer ledBuffer;
- @Getter protected final AddressableLEDBufferView ledView;
- private boolean mainView = false;
+ /** The {@link CANdle} device (owned or shared). */
+ @Getter protected final CANdle candle;
- protected final LEDPattern defaultPattern = blink(Color.kOrange, 1);
+ /** {@code true} if the most recently applied pattern was a hardware animation. */
+ private boolean lastWasAnimation = false;
- @Getter
- protected Command defaultCommand =
- setPattern(defaultPattern, -1).withName("LEDs.defaultCommand");
+ /**
+ * Default pattern shown when no other command requires this subsystem (orange blink).
+ *
+ * Initialized in the constructor body (after {@link #config} is set) so that pattern
+ * factories can safely reference the config.
+ */
+ protected final CANdlePattern defaultPattern;
- public final Trigger defaultTrigger = new Trigger(() -> defaultCommand.isScheduled());
+ /**
+ * The default command built by this class (displays {@link #defaultPattern} at lowest
+ * priority). Installed via {@code setDefaultCommand} in the constructor; subclasses may install
+ * their own default command to replace it. Use {@code getDefaultCommand()} (from {@link
+ * Subsystem}) to query whichever default is currently installed.
+ */
+ protected final Command defaultCommand;
- @Getter @Setter private int commandPriority = 0;
+ /**
+ * Trigger that is active while the currently installed default command (whichever one that is)
+ * is the one running — i.e. no higher-priority pattern owns the subsystem.
+ */
+ public final Trigger defaultTrigger;
+
+ /**
+ * Priority level of the pattern command currently running. Higher values indicate higher
+ * priority; {@link #setPattern(CANdlePattern, int)} stores this while a command runs and resets
+ * it to {@code -1} when the command ends.
+ */
+ @Getter @Setter private int commandPriority = -1;
+ /** Spectrum purple color constant ({@code RGB 130, 103, 185}). */
public final Color purple = new Color(130, 103, 185);
+
+ /** Convenience alias for {@link Color#kWhite}. */
public final Color white = Color.kWhite;
+ // -------------------------------------------------------------------------
+ // Constructor
+ // -------------------------------------------------------------------------
+
+ /**
+ * Constructs the LED subsystem, configures the hardware (or reuses a shared device), and
+ * registers with the WPILib {@link CommandScheduler}.
+ *
+ * @param config the configuration describing the device, segment range, and strip type
+ */
public SpectrumLEDs(Config config) {
this.config = config;
- // Must be a PWM header, not MXP or DIO
- if (config.getLed() == null) {
- led = new AddressableLED(config.port);
- // Length is expensive to set, so only set it once, then just update data
- ledBuffer = new AddressableLEDBuffer(config.length);
- led.setLength(ledBuffer.getLength());
- mainView = true;
+ if (config.getSharedCandle() != null) {
+ candle = config.getSharedCandle();
} else {
- led = config.getLed();
- ledBuffer = config.buffer;
+ candle = new CANdle(config.getDeviceId(), config.getCanBus());
+ CANdleConfiguration candleConfig =
+ new CANdleConfiguration()
+ .withLED(
+ new LEDConfigs()
+ .withStripType(config.getStripType())
+ .withBrightnessScalar(config.getBrightness())
+ .withLossOfSignalBehavior(
+ config.getLossOfSignalBehavior()));
+ candle.getConfigurator().apply(candleConfig);
}
- ledView = ledBuffer.createView(config.startingIndex, config.endingIndex);
-
- // Set the data
- led.setData(ledBuffer);
- setPattern(defaultPattern);
- led.start();
+ // Pattern fields are initialized here (after config is set) so factory methods
+ // can safely read config values.
+ defaultPattern = blink(Color.kOrange, 1.0);
+ defaultCommand = setPattern(defaultPattern, -1).withName("LEDs.defaultCommand");
+ setDefaultCommand(defaultCommand);
+ defaultTrigger =
+ new Trigger(
+ () -> {
+ Command current = getCurrentCommand();
+ return current != null && current == getDefaultCommand();
+ });
- SpectrumRobot.add(this);
CommandScheduler.getInstance().registerSubsystem(this);
}
- @Override
- public void periodic() {
- // Set the LEDs only if this is the main view
- if (mainView) {
- led.setData(ledBuffer);
- }
- }
+ // -------------------------------------------------------------------------
+ // Subsystem API
+ // -------------------------------------------------------------------------
+ /**
+ * Returns whether this LED strip is physically connected to the robot.
+ *
+ * @return {@code true} if attached
+ */
public boolean isAttached() {
return config.isAttached();
}
+ /**
+ * Returns {@code true} if the most recently applied pattern was a CANdle hardware animation
+ * running in firmware, {@code false} if it was a software {@link SolidColor} pattern.
+ */
+ public boolean isAnimating() {
+ return lastWasAnimation;
+ }
+
+ /**
+ * Returns the currently running command that is applying a pattern to this subsystem, or {@code
+ * null} if no command is currently running.
+ *
+ * @return the currently running command, or {@code null} if none
+ */
+ public String getCurrentCommandName() {
+ Command cmd = getCurrentCommand();
+ return cmd != null ? cmd.getName() : "None";
+ }
+
+ /**
+ * Returns a {@link Trigger} that is active when the currently running command's priority is at
+ * or below the given value. Use to gate lower-priority commands from overriding higher-priority
+ * ones.
+ *
+ * @param priority the maximum priority level that allows the trigger to be active
+ * @return trigger active when {@link #commandPriority} ≤ priority
+ */
public Trigger checkPriority(int priority) {
return new Trigger(() -> commandPriority <= priority);
}
- public Command setPattern(LEDPattern pattern, int priority) {
+ /**
+ * Returns a command that continuously applies {@code pattern} to the LED segment and records
+ * the given {@code priority} while running. The command runs while the robot is disabled.
+ *
+ * When switching from a hardware animation to a software ({@link SolidColor}) pattern, all
+ * active animation slots are cleared automatically before the first software write.
+ *
+ * @param pattern the {@link CANdlePattern} to apply each loop cycle
+ * @param priority priority level stored in {@link #commandPriority} while this command runs
+ * @return a command that applies the pattern continuously
+ */
+ public Command setPattern(CANdlePattern pattern, int priority) {
return run(() -> {
commandPriority = priority;
- pattern.applyTo(ledView);
+ boolean isAnim = pattern instanceof HardwareAnimPattern;
+ // Clear this instance's animation slot once when transitioning to a software
+ // pattern. Only our own slot is cleared so other instances sharing the same
+ // CANdle keep their animations running.
+ if (lastWasAnimation && !isAnim) {
+ candle.setControl(new EmptyAnimation(config.getAnimationSlot()));
+ }
+ lastWasAnimation = isAnim;
+ pattern.applyTo(candle, config.getStartIdx(), config.getNumLeds());
})
+ .finallyDo(() -> commandPriority = -1)
.ignoringDisable(true)
.withName("LEDs.setPattern");
}
- public Command setPattern(LEDPattern pattern) {
+ /**
+ * Returns a command that continuously applies {@code pattern} to the LED segment at priority 0.
+ *
+ * @param pattern the {@link CANdlePattern} to apply
+ * @return a command that applies the pattern continuously at the default priority
+ */
+ public Command setPattern(CANdlePattern pattern) {
return setPattern(pattern, 0);
}
- @Override
- public void setupStates() {}
+ // -------------------------------------------------------------------------
+ // Internal helpers
+ // -------------------------------------------------------------------------
- @Override
- public void setupDefaultCommand() {
- setDefaultCommand(
- setPattern(solid(Color.kOrange), 0)
- .withName("SPECTRUM LED DEFAULT COMMAND SHOULD NOT BE RUNNING"));
+ /**
+ * Converts a WPILib {@link Color} (0.0–1.0 double components) to a {@link RGBWColor} with
+ * {@code W = 0}.
+ */
+ private static RGBWColor toRGBW(Color color) {
+ return new RGBWColor(
+ (int) (color.red * 255), (int) (color.green * 255), (int) (color.blue * 255), 0);
}
+ // -------------------------------------------------------------------------
+ // Hardware animation pattern factories
+ // -------------------------------------------------------------------------
+
/**
- * LED Pattern Stripe, takes in a double percent and sets the first length number of LEDs to one
- * color and the rest of the strip to another
+ * Blinking (strobe) pattern — alternates between {@code color} and off. Each half-cycle (on and
+ * off) lasts {@code onTimeSecs} seconds.
+ *
+ * Implemented using {@link StrobeAnimation}. Frame rate = {@code 1 / onTimeSecs} Hz.
+ *
+ * @param color the blink color
+ * @param onTimeSecs duration in seconds of each on (and off) half-cycle
+ * @return a hardware animation {@link CANdlePattern}
*/
- public LEDPattern stripe(double percent, Color color1, Color color2) {
- return LEDPattern.steps(Map.of(0.00, color1, percent, color2));
+ public CANdlePattern blink(Color color, double onTimeSecs) {
+ RGBWColor rgbw = toRGBW(color);
+ // Lazy: animation created on first applyTo call using the runtime startIdx/numLeds.
+ StrobeAnimation[] holder = new StrobeAnimation[1];
+ return hardwareAnim(
+ (candle, startIdx, numLeds) -> {
+ if (holder[0] == null) {
+ holder[0] =
+ new StrobeAnimation(startIdx, startIdx + numLeds - 1)
+ .withSlot(config.getAnimationSlot())
+ .withColor(rgbw)
+ .withFrameRate(Hertz.of(1.0 / onTimeSecs));
+ }
+ candle.setControl(holder[0]);
+ });
}
/**
- * Creates a solid LED pattern with the specified color.
+ * Breathing (sinusoidal fade-in/out) pattern — fades between the peak {@code color} and off.
+ *
+ * Implemented using {@link SingleFadeAnimation}. Each animation frame changes brightness by
+ * 1%, so frame rate = {@code 200 / periodSecs} Hz for a complete 0→100→0% cycle.
*
- * @param color the color to be used for the solid LED pattern
- * @return an LEDPattern object representing the solid color pattern
+ * @param color the peak color at full brightness
+ * @param periodSecs duration in seconds of one full breathe cycle
+ * @return a hardware animation {@link CANdlePattern}
*/
- public LEDPattern solid(Color color) {
- return LEDPattern.solid(color);
+ public CANdlePattern breathe(Color color, double periodSecs) {
+ RGBWColor rgbw = toRGBW(color);
+ SingleFadeAnimation[] holder = new SingleFadeAnimation[1];
+ return hardwareAnim(
+ (candle, startIdx, numLeds) -> {
+ if (holder[0] == null) {
+ holder[0] =
+ new SingleFadeAnimation(startIdx, startIdx + numLeds - 1)
+ .withSlot(config.getAnimationSlot())
+ .withColor(rgbw)
+ .withFrameRate(Hertz.of(200.0 / periodSecs));
+ }
+ candle.setControl(holder[0]);
+ });
}
/**
- * Creates an LED pattern that blinks with the specified on-time duration.
+ * Static rainbow — hue distributed evenly across the strip, advancing very slowly.
*
- * @param onTime the duration (in seconds) for which the LED stays on during each blink cycle
- * @return an LEDPattern that blinks with the specified on-time duration
+ * @return a hardware animation {@link CANdlePattern}
*/
- public LEDPattern blink(Color c, double onTime) {
- return solid(c).blink(Seconds.of(onTime));
+ public CANdlePattern rainbow() {
+ return rainbow(1.0);
}
/**
- * Creates a breathing LED pattern with the specified period.
+ * Static rainbow with configurable brightness.
*
- * @param period The period of the breathing effect in seconds.
- * @return An LEDPattern object representing the breathing effect.
+ * @param brightness brightness scalar (0.0–1.0)
+ * @return a hardware animation {@link CANdlePattern}
*/
- public LEDPattern breathe(Color c, double period) {
- return solid(c).breathe(Seconds.of(period));
+ public CANdlePattern rainbow(double brightness) {
+ RainbowAnimation[] holder = new RainbowAnimation[1];
+ return hardwareAnim(
+ (candle, startIdx, numLeds) -> {
+ if (holder[0] == null) {
+ holder[0] =
+ new RainbowAnimation(startIdx, startIdx + numLeds - 1)
+ .withSlot(config.getAnimationSlot())
+ .withBrightness(brightness)
+ .withFrameRate(Hertz.of(3));
+ }
+ candle.setControl(holder[0]);
+ });
}
/**
- * Creates and returns a rainbow LED pattern with specified brightness and saturation.
+ * Scrolling rainbow that advances quickly across the strip.
*
- * @return an LEDPattern object representing a rainbow pattern with maximum brightness (255) and
- * medium saturation (128).
+ * @return a hardware animation {@link CANdlePattern}
*/
- public LEDPattern rainbow() {
- return rainbow(255, 128);
+ public CANdlePattern scrollingRainbow() {
+ RainbowAnimation[] holder = new RainbowAnimation[1];
+ return hardwareAnim(
+ (candle, startIdx, numLeds) -> {
+ if (holder[0] == null) {
+ holder[0] =
+ new RainbowAnimation(startIdx, startIdx + numLeds - 1)
+ .withSlot(config.getAnimationSlot())
+ .withBrightness(1.0)
+ .withFrameRate(Hertz.of(60));
+ }
+ candle.setControl(holder[0]);
+ });
}
- public LEDPattern scrollingRainbow() {
- return rainbow().scrollAtAbsoluteSpeed(MetersPerSecond.of(0.25), config.getLedSpacing());
+ /**
+ * Chase / color-flow pattern — progressively lights LEDs one at a time across the strip and
+ * repeats.
+ *
+ * Implemented using {@link ColorFlowAnimation}. Frame rate = {@code numLeds × speed} Hz so
+ * that {@code speed} full cycles occur per second.
+ *
+ * @param color the chase color
+ * @param speed desired number of full strip cycles per second
+ * @return a hardware animation {@link CANdlePattern}
+ */
+ public CANdlePattern chase(Color color, double speed) {
+ RGBWColor rgbw = toRGBW(color);
+ ColorFlowAnimation[] holder = new ColorFlowAnimation[1];
+ return hardwareAnim(
+ (candle, startIdx, numLeds) -> {
+ if (holder[0] == null) {
+ holder[0] =
+ new ColorFlowAnimation(startIdx, startIdx + numLeds - 1)
+ .withSlot(config.getAnimationSlot())
+ .withColor(rgbw)
+ .withFrameRate(Hertz.of(numLeds * speed));
+ }
+ candle.setControl(holder[0]);
+ });
}
/**
- * Generates a rainbow LED pattern with the specified saturation and value.
+ * Bouncing dot pattern — a pocket of light travels back and forth across the strip.
+ *
+ * Implemented using {@link LarsonAnimation} with {@link LarsonBounceValue#Back}. Frame rate
+ * is computed so one back-and-forth cycle takes {@code durationSecs} seconds.
*
- * @param saturation the saturation level of the rainbow pattern (0-255)
- * @param value the brightness value of the rainbow pattern (0-255)
- * @return an LEDPattern object representing the rainbow pattern
+ * @param color the dot color
+ * @param durationSecs seconds per complete back-and-forth cycle
+ * @return a hardware animation {@link CANdlePattern}
*/
- public LEDPattern rainbow(int saturation, int value) {
- return LEDPattern.rainbow(saturation, value);
+ public CANdlePattern bounce(Color color, double durationSecs) {
+ RGBWColor rgbw = toRGBW(color);
+ LarsonAnimation[] holder = new LarsonAnimation[1];
+ return hardwareAnim(
+ (candle, startIdx, numLeds) -> {
+ if (holder[0] == null) {
+ // One full cycle = 2 * (numLeds - 1) LED-position advances.
+ double frameRate = 2.0 * Math.max(numLeds - 1, 1) / durationSecs;
+ holder[0] =
+ new LarsonAnimation(startIdx, startIdx + numLeds - 1)
+ .withSlot(config.getAnimationSlot())
+ .withColor(rgbw)
+ .withSize(3)
+ .withBounceMode(LarsonBounceValue.Back)
+ .withFrameRate(Hertz.of(frameRate));
+ }
+ candle.setControl(holder[0]);
+ });
}
/**
- * Creates a gradient LED pattern using the specified colors.
+ * Fire animation using the CANdle's built-in hardware animation engine.
*
- * @param colors The array of colors to be used in the gradient pattern.
- * @return An LEDPattern object representing the gradient pattern.
+ * @return a hardware animation {@link CANdlePattern}
*/
- public LEDPattern gradient(Color... colors) {
- return LEDPattern.gradient(GradientType.kContinuous, colors);
+ public CANdlePattern fire() {
+ FireAnimation[] holder = new FireAnimation[1];
+ return hardwareAnim(
+ (candle, startIdx, numLeds) -> {
+ if (holder[0] == null) {
+ holder[0] =
+ new FireAnimation(startIdx, startIdx + numLeds - 1)
+ .withSlot(config.getAnimationSlot())
+ .withFrameRate(Hertz.of(60));
+ }
+ candle.setControl(holder[0]);
+ });
}
/**
- * Scrolls the given LED pattern at the specified speed.
+ * RGB color-cycle animation using the CANdle's built-in hardware animation engine.
*
- * @param pattern the LEDPattern to be scrolled
- * @param speedMps the speed at which the pattern should scroll, in meters per second
- * @return a new LEDPattern that represents the scrolled pattern
+ * @return a hardware animation {@link CANdlePattern}
*/
- public LEDPattern scroll(LEDPattern pattern, double speedMps) {
- return pattern.scrollAtAbsoluteSpeed(MetersPerSecond.of(speedMps), config.getLedSpacing());
+ public CANdlePattern rgbCycle() {
+ RgbFadeAnimation[] holder = new RgbFadeAnimation[1];
+ return hardwareAnim(
+ (candle, startIdx, numLeds) -> {
+ if (holder[0] == null) {
+ holder[0] =
+ new RgbFadeAnimation(startIdx, startIdx + numLeds - 1)
+ .withSlot(config.getAnimationSlot())
+ .withFrameRate(Hertz.of(30));
+ }
+ candle.setControl(holder[0]);
+ });
}
+ // -------------------------------------------------------------------------
+ // Software pattern factories (SolidColor one-shot, resent each loop)
+ // -------------------------------------------------------------------------
+
/**
- * Creates an LED chase pattern with the specified color, percentage, and speed.
+ * Solid color pattern.
+ *
+ * Uses a single {@link SolidColor} control request (one-shot, resent each loop).
*
- * @param color1 The color to be used in the chase pattern.
- * @param percent The percentage of the pattern that will be the specified color.
- * @param speed The speed at which the pattern will scroll, in Hertz.
- * @return An LEDPattern object representing the chase pattern.
+ * @param color the color to display
+ * @return a software {@link CANdlePattern} showing a constant solid color
*/
- public LEDPattern chase(Color color1, double percent, double speed) {
- return LEDPattern.steps(Map.of(0.00, color1, percent, Color.kBlack))
- .scrollAtRelativeSpeed(Frequency.ofBaseUnits(speed, Hertz));
+ public CANdlePattern solid(Color color) {
+ RGBWColor rgbw = toRGBW(color);
+ SolidColor[] holder = new SolidColor[1];
+ return (candle, startIdx, numLeds) -> {
+ if (holder[0] == null) {
+ holder[0] = new SolidColor(startIdx, startIdx + numLeds - 1).withColor(rgbw);
+ }
+ candle.setControl(holder[0]);
+ };
}
/**
- * Creates a bouncing LED pattern with the specified color and duration.
- *
- * @param c the color of the bouncing LED
- * @param durationInSeconds the duration of one complete bounce cycle in seconds
- * @return an LEDPattern that applies the bouncing effect to the LEDs
- */
- public LEDPattern bounce(Color c, double durationInSeconds) {
- return new LEDPattern() {
- @Override
- public void applyTo(LEDReader reader, LEDWriter writer) {
- int bufLen = reader.getLength();
- long currentTime = System.currentTimeMillis();
- double cycleTime =
- durationInSeconds
- * 1000; // Convert time to milliseconds for the entire cycle
- double phase =
- (currentTime % cycleTime) / cycleTime; // Phase of the cycle from 0 to 1
-
- // Determine direction and position based on the phase
- boolean backwards = phase > 0.5;
- double position = backwards ? 2 * (1 - phase) : 2 * phase;
- int ledPosition = (int) (position * bufLen);
-
- for (int tempLed = 0; tempLed < bufLen; tempLed++) {
- if (tempLed == ledPosition) {
- writer.setLED(tempLed, c);
- } else if (tempLed == ledPosition - 1 || tempLed == ledPosition + 1) {
- writer.setLED(
- tempLed, new Color(c.red * 0.66, c.green * 0.66, c.blue * 0.66));
- } else if (tempLed == ledPosition - 2 || tempLed == ledPosition + 2) {
- writer.setLED(
- tempLed, new Color(c.red * 0.33, c.green * 0.33, c.blue * 0.33));
- } else {
- writer.setLED(tempLed, Color.kBlack);
- }
- }
+ * Two-color stripe: the first {@code percent} fraction of LEDs shows {@code color1}, the
+ * remainder shows {@code color2}.
+ *
+ * Uses two {@link SolidColor} controls (one-shot each, resent each loop).
+ *
+ * @param percent fraction of the strip (0.0–1.0) assigned to {@code color1}
+ * @param color1 color for the leading segment
+ * @param color2 color for the trailing segment
+ * @return a software {@link CANdlePattern} showing the two-color stripe
+ */
+ public CANdlePattern stripe(double percent, Color color1, Color color2) {
+ RGBWColor rgbw1 = toRGBW(color1);
+ RGBWColor rgbw2 = toRGBW(color2);
+ SolidColor[][] holder = new SolidColor[1][];
+ return (candle, startIdx, numLeds) -> {
+ if (holder[0] == null) {
+ int split = Math.max(0, Math.min((int) Math.round(numLeds * percent), numLeds));
+ holder[0] = new SolidColor[2];
+ // Segment 1 (may be empty if split == 0)
+ holder[0][0] =
+ (split > 0)
+ ? new SolidColor(startIdx, startIdx + split - 1).withColor(rgbw1)
+ : null;
+ // Segment 2 (may be empty if split == numLeds)
+ holder[0][1] =
+ (split < numLeds)
+ ? new SolidColor(startIdx + split, startIdx + numLeds - 1)
+ .withColor(rgbw2)
+ : null;
+ }
+ for (SolidColor req : holder[0]) {
+ if (req != null) candle.setControl(req);
}
};
}
/**
- * Creates an ombre LED pattern that transitions smoothly between two colors.
- *
- * @param startColor The starting color of the ombre effect.
- * @param endColor The ending color of the ombre effect.
- * @return An LEDPattern that applies the ombre effect to the LED strip.
- */
- public LEDPattern ombre(Color startColor, Color endColor) {
- return new LEDPattern() {
- @Override
- public void applyTo(LEDReader reader, LEDWriter writer) {
- int bufLen = reader.getLength();
- long currentTime = System.currentTimeMillis();
- // The speed factor here determines how quickly the ombre moves along the strip
- double phaseShift =
- (currentTime / 1000.0)
- * 0.58 // this is the speed, the higher the number the faster the
- // ombre moves
- % 1.0; // Modulo 1 to keep the phase within [0, 1]
- for (int tempLed = 0; tempLed < bufLen; tempLed++) {
- // Adjust ratio to include the phaseShift, causing the ombre to move
- double ratio =
- ((tempLed + (bufLen * phaseShift))
- / bufLen
- % 1.0); // Modulo 1 to ensure the ratio loops within [0,
- // 1]
-
- // Interpolate the red, green, and blue components separately using double
- // precision
- double red = (startColor.red * (1 - ratio)) + (endColor.red * ratio);
- double green = (startColor.green * (1 - ratio)) + (endColor.green * ratio);
- double blue = (startColor.blue * (1 - ratio)) + (endColor.blue * ratio);
-
- // Create a new color for the current LED
- Color currentColor = new Color(red, green, blue);
-
- // Set the color of the current LED
- writer.setLED(tempLed, currentColor);
+ * Linear gradient between two colors across the strip. Colors are pre-computed at first use.
+ *
+ * Uses N {@link SolidColor} controls (one per LED, one-shot, resent each loop).
+ *
+ * @param color1 color at the start (index 0) of the segment
+ * @param color2 color at the end of the segment
+ * @return a software {@link CANdlePattern} showing the two-color gradient
+ */
+ public CANdlePattern gradient(Color color1, Color color2) {
+ SolidColor[][] holder = new SolidColor[1][];
+ return (candle, startIdx, numLeds) -> {
+ if (holder[0] == null) {
+ holder[0] = new SolidColor[numLeds];
+ for (int i = 0; i < numLeds; i++) {
+ double ratio = (numLeds <= 1) ? 0.0 : (double) i / (numLeds - 1);
+ int r = (int) (color1.red * 255 * (1 - ratio) + color2.red * 255 * ratio);
+ int g = (int) (color1.green * 255 * (1 - ratio) + color2.green * 255 * ratio);
+ int b = (int) (color1.blue * 255 * (1 - ratio) + color2.blue * 255 * ratio);
+ holder[0][i] =
+ new SolidColor(startIdx + i, startIdx + i)
+ .withColor(new RGBWColor(r, g, b, 0));
}
}
+ for (SolidColor req : holder[0]) {
+ candle.setControl(req);
+ }
};
}
/**
- * Creates a wave LED pattern that transitions between two colors over a specified cycle length
- * of LEDs and duration.
- *
- * @param c1 The first color in the wave pattern.
- * @param c2 The second color in the wave pattern.
- * @param cycleLength The length of the wave cycle in terms of LEDs.
- * @param durationSecs The duration of the entire wave pattern in seconds.
- * @return An LEDPattern that applies the wave effect to the LEDs.
- */
- public LEDPattern wave(Color c1, Color c2, double cycleLength, double durationSecs) {
- return new LEDPattern() {
- @Override
- public void applyTo(LEDReader reader, LEDWriter writer) {
- int bufLen = reader.getLength();
- double currentTime = Timer.getFPGATimestamp();
- double phase = (currentTime % durationSecs) / durationSecs;
- double x = (1 - phase) * 2.0 * Math.PI;
- double xDiffPerLed = (2.0 * Math.PI) / cycleLength;
- double waveExponent = 0.4;
-
- for (int tempLed = 0; tempLed < bufLen; tempLed++) {
- x += xDiffPerLed;
-
- double ratio = (Math.pow(Math.sin(x), waveExponent) + 1.0) / 2.0;
- if (Double.isNaN(ratio)) {
- ratio = (-Math.pow(Math.sin(x + Math.PI), waveExponent) + 1.0) / 2.0;
- }
- if (Double.isNaN(ratio)) {
- ratio = 0.5;
- }
- double red = (c1.red * (1 - ratio)) + (c2.red * ratio);
- double green = (c1.green * (1 - ratio)) + (c2.green * ratio);
- double blue = (c1.blue * (1 - ratio)) + (c2.blue * ratio);
- writer.setLED(tempLed, new Color(red, green, blue));
+ * Edge-highlight pattern — lights the first and last {@code length} LEDs with {@code color} and
+ * turns off the center LEDs.
+ *
+ * Uses two or three {@link SolidColor} controls (one-shot, resent each loop).
+ *
+ * @param color the color to apply to the edge LEDs
+ * @param length the number of LEDs to illuminate at each end of the strip
+ * @return a software {@link CANdlePattern} showing lit edges and a dark center
+ */
+ public CANdlePattern edges(Color color, int length) {
+ RGBWColor rgbw = toRGBW(color);
+ SolidColor[][] holder = new SolidColor[1][];
+ return (candle, startIdx, numLeds) -> {
+ if (holder[0] == null) {
+ int clampedLen = Math.min(length, numLeds / 2);
+ int centerStart = startIdx + clampedLen;
+ int centerEnd = startIdx + numLeds - clampedLen - 1;
+ // left edge, right edge, (optional) center black
+ holder[0] = (centerStart <= centerEnd) ? new SolidColor[3] : new SolidColor[2];
+ holder[0][0] = new SolidColor(startIdx, startIdx + clampedLen - 1).withColor(rgbw);
+ holder[0][1] =
+ new SolidColor(startIdx + numLeds - clampedLen, startIdx + numLeds - 1)
+ .withColor(rgbw);
+ if (holder[0].length == 3) {
+ holder[0][2] =
+ new SolidColor(centerStart, centerEnd)
+ .withColor(new RGBWColor(0, 0, 0, 0));
}
}
+ for (SolidColor req : holder[0]) {
+ candle.setControl(req);
+ }
};
}
/**
- * Creates an LEDPattern that represents a countdown effect. The countdown starts from a
- * specified time and lasts for a given duration. During the countdown, the LEDs transition from
- * yellow to red, and progressively turn off from the end of the strip towards the beginning.
- *
- * @param countStartTimeSec A DoubleSupplier that provides the start time of the countdown in
- * seconds.
- * @param durationInSeconds The total duration of the countdown in seconds.
- * @return An LEDPattern that applies the countdown effect to the LEDs.
- */
- public LEDPattern countdown(DoubleSupplier countStartTimeSec, double durationInSeconds) {
- double startTime = countStartTimeSec.getAsDouble();
- return new LEDPattern() {
- @Override
- public void applyTo(LEDReader reader, LEDWriter writer) {
- int bufLen = reader.getLength();
- double currentTimeSecs = Timer.getFPGATimestamp();
- double elapsedTimeInSeconds = currentTimeSecs - startTime;
-
- // Calculate the progress of the countdown
- double progress = elapsedTimeInSeconds / durationInSeconds;
-
- // Calculate the number of LEDs to turn off based on the progress
- int ledsToTurnOff = (int) (bufLen * progress);
-
- // Calculate the color transition from yellow to red based on the progress
- // Yellow (255, 255, 0) to Red (255, 0, 0)
- int red = 255; // Red component stays at 255
- int green = (int) (255 * (1 - progress)); // Green component decreases to 0
- Color countdownColor = new Color(red, green, 0);
-
- // Update the LEDs from the end of the strip towards the beginning
- for (int tempLed = bufLen - 1; tempLed >= 0; tempLed--) {
- if (bufLen - tempLed <= ledsToTurnOff) {
- // Turn off the LEDs progressively
- writer.setLED(tempLed, Color.kBlack);
- } else {
- // Set the remaining LEDs to the countdown color
- writer.setLED(tempLed, countdownColor);
- }
- }
-
- // If the countdown is complete, ensure all LEDs are turned off
- if (progress >= 1.0) {
- for (int i = 0; i < bufLen; i++) {
- writer.setLED(i, Color.kBlack);
- }
+ * Animated ombre — transitions smoothly between two colors across the strip and scrolls the
+ * blend point over time.
+ *
+ * Uses N {@link SolidColor} controls (one per LED, one-shot, resent each loop with updated
+ * colors). {@link RGBWColor} objects are created each loop since the type is immutable.
+ *
+ * @param startColor the leading color
+ * @param endColor the trailing color
+ * @return a software {@link CANdlePattern} showing the animated ombre
+ */
+ public CANdlePattern ombre(Color startColor, Color endColor) {
+ SolidColor[][] holder = new SolidColor[1][];
+ return (candle, startIdx, numLeds) -> {
+ if (holder[0] == null) {
+ holder[0] = new SolidColor[numLeds];
+ for (int i = 0; i < numLeds; i++) {
+ holder[0][i] = new SolidColor(startIdx + i, startIdx + i);
}
}
+ // Speed: 0.58 strip-lengths per second
+ double phaseShift = (System.currentTimeMillis() / 1000.0) * 0.58 % 1.0;
+ for (int i = 0; i < numLeds; i++) {
+ double ratio = ((i + numLeds * phaseShift) / numLeds) % 1.0;
+ int r = (int) (startColor.red * 255 * (1 - ratio) + endColor.red * 255 * ratio);
+ int g = (int) (startColor.green * 255 * (1 - ratio) + endColor.green * 255 * ratio);
+ int b = (int) (startColor.blue * 255 * (1 - ratio) + endColor.blue * 255 * ratio);
+ holder[0][i].Color = new RGBWColor(r, g, b, 0);
+ candle.setControl(holder[0][i]);
+ }
};
}
- public LEDPattern switchCountdown(Color startingColor) {
-
- return new LEDPattern() {
- @Override
- public void applyTo(LEDReader reader, LEDWriter writer) {
-
- int[] times = {
- 10, // surely there's a more efficient way to do this
- 25, 25, 25, 25, 30,
- };
-
- int bufLen = reader.getLength();
- double currentTimeSecs = Timer.getMatchTime();
-
- double elapsedTimeInSeconds = 140 - currentTimeSecs;
-
- // Calculate the progress of the countdown
- int shiftTime = 0;
- int cumulativeTime = 0;
- Color color = Color.kBlack;
-
- for (int i = 0; i < times.length; i++) {
- cumulativeTime += times[i];
- if (cumulativeTime > elapsedTimeInSeconds) {
- shiftTime = times[i];
- switch (i) {
- case 0, 5: // this is also kinda a mess but it works wtv
- color = Color.kPurple;
- break;
- case 1, 3:
- color = startingColor;
- break;
- case 2, 4:
- if (startingColor == Color.kRed) color = Color.kBlue;
- else color = Color.kRed;
- break;
- }
- break;
- }
+ /**
+ * Sinusoidal wave pattern blending between two colors.
+ *
+ * Uses N {@link SolidColor} controls (one per LED, one-shot, resent each loop).
+ *
+ * @param c1 first wave color
+ * @param c2 second wave color
+ * @param cycleLength number of LEDs per wave period
+ * @param durationSecs period of the time-based animation in seconds
+ * @return a software {@link CANdlePattern} showing the wave
+ */
+ public CANdlePattern wave(Color c1, Color c2, double cycleLength, double durationSecs) {
+ SolidColor[][] holder = new SolidColor[1][];
+ return (candle, startIdx, numLeds) -> {
+ if (holder[0] == null) {
+ holder[0] = new SolidColor[numLeds];
+ for (int i = 0; i < numLeds; i++) {
+ holder[0][i] = new SolidColor(startIdx + i, startIdx + i);
}
- double progress = 1 - (cumulativeTime - elapsedTimeInSeconds) / shiftTime;
-
- // Calculate the number of LEDs to turn off based on the progress
- int ledsToTurnOff = (int) (bufLen * progress);
-
- // Update the LEDs from the end of the strip towards the beginning
- for (int tempLed = bufLen - 1; tempLed >= 0; tempLed--) {
- if (bufLen - tempLed <= ledsToTurnOff) {
- // Turn off the LEDs progressively
- writer.setLED(tempLed, Color.kBlack);
- } else {
- // Set the remaining LEDs to the countdown color
- writer.setLED(tempLed, color);
- }
+ }
+ double currentTime = Timer.getFPGATimestamp();
+ double phase = (currentTime % durationSecs) / durationSecs;
+ double x = (1 - phase) * 2.0 * Math.PI;
+ double xDiffPerLed = (2.0 * Math.PI) / cycleLength;
+ double waveExponent = 0.4;
+ for (int i = 0; i < numLeds; i++) {
+ x += xDiffPerLed;
+ double ratio = (Math.pow(Math.sin(x), waveExponent) + 1.0) / 2.0;
+ if (Double.isNaN(ratio)) {
+ ratio = (-Math.pow(Math.sin(x + Math.PI), waveExponent) + 1.0) / 2.0;
}
+ if (Double.isNaN(ratio)) ratio = 0.5;
+ int r = (int) (c1.red * 255 * (1 - ratio) + c2.red * 255 * ratio);
+ int g = (int) (c1.green * 255 * (1 - ratio) + c2.green * 255 * ratio);
+ int b = (int) (c1.blue * 255 * (1 - ratio) + c2.blue * 255 * ratio);
+ holder[0][i].Color = new RGBWColor(r, g, b, 0);
+ candle.setControl(holder[0][i]);
+ }
+ };
+ }
- // If the countdown is complete, ensure all LEDs are turned off
- if (progress >= 1.0) {
- for (int i = 0; i < bufLen; i++) {
- writer.setLED(i, Color.kBlack);
- }
+ /**
+ * Countdown pattern — LEDs transition from yellow to red and progressively turn off from the
+ * end of the segment toward the beginning as time elapses.
+ *
+ * Uses N {@link SolidColor} controls (one per LED, one-shot, resent each loop).
+ *
+ * @param countStartTimeSec supplies the FPGA timestamp (seconds) when the countdown began
+ * @param durationInSeconds total countdown duration in seconds
+ * @return a software {@link CANdlePattern} showing the countdown
+ */
+ public CANdlePattern countdown(DoubleSupplier countStartTimeSec, double durationInSeconds) {
+ SolidColor[][] holder = new SolidColor[1][];
+ return (candle, startIdx, numLeds) -> {
+ if (holder[0] == null) {
+ holder[0] = new SolidColor[numLeds];
+ for (int i = 0; i < numLeds; i++) {
+ holder[0][i] = new SolidColor(startIdx + i, startIdx + i);
}
}
+ // Read the supplier each loop (not at factory time) so patterns built at binding
+ // time still measure from the correct start when the command eventually runs.
+ double elapsed = Timer.getFPGATimestamp() - countStartTimeSec.getAsDouble();
+ double progress = Math.min(elapsed / durationInSeconds, 1.0);
+ int ledsOff = (int) (numLeds * progress);
+ int green = (int) (255 * (1 - progress));
+ for (int i = numLeds - 1; i >= 0; i--) {
+ holder[0][i].Color =
+ (numLeds - i <= ledsOff)
+ ? new RGBWColor(0, 0, 0, 0)
+ : new RGBWColor(255, green, 0, 0);
+ candle.setControl(holder[0][i]);
+ }
};
}
- public LEDPattern edges(Color c, double length) {
- return new LEDPattern() {
- public void applyTo(LEDReader reader, LEDWriter writer) {
- int bufLen = reader.getLength();
- for (int i = 0; i < bufLen; i++) {
- if (i < length || i > bufLen - length - 1) {
- writer.setLED(i, c);
- } else {
- writer.setLED(i, Color.kBlack);
+ /**
+ * Alliance switch countdown — cycles through alliance colors (and purple) on a hard-coded
+ * match-time schedule, progressively turning off LEDs within each segment as time elapses.
+ *
+ * Segment schedule (seconds remaining → color):
+ *
+ * Uses N {@link SolidColor} controls (one per LED, one-shot, resent each loop).
+ *
+ * @param startingColor the alliance color displayed during this robot's segments
+ * @return a software {@link CANdlePattern} reflecting the current switch-countdown state
+ */
+ public CANdlePattern switchCountdown(Color startingColor) {
+ SolidColor[][] holder = new SolidColor[1][];
+ return (candle, startIdx, numLeds) -> {
+ if (holder[0] == null) {
+ holder[0] = new SolidColor[numLeds];
+ for (int i = 0; i < numLeds; i++) {
+ holder[0][i] = new SolidColor(startIdx + i, startIdx + i);
+ }
+ }
+
+ int[] times = {10, 25, 25, 25, 25, 30};
+ double elapsed = 140 - Timer.getMatchTime();
+
+ int shiftTime = 0;
+ int cumulativeTime = 0;
+ Color color = Color.kBlack;
+
+ for (int i = 0; i < times.length; i++) {
+ cumulativeTime += times[i];
+ if (cumulativeTime > elapsed) {
+ shiftTime = times[i];
+ switch (i) {
+ case 0, 5 -> color = Color.kPurple;
+ case 1, 3 -> color = startingColor;
+ case 2, 4 -> color =
+ Color.kRed.equals(startingColor) ? Color.kBlue : Color.kRed;
+ default -> color = Color.kBlack;
}
+ break;
}
}
+
+ double progress = 1.0 - (cumulativeTime - elapsed) / Math.max(shiftTime, 1);
+ int ledsOff = (int) (numLeds * Math.min(progress, 1.0));
+ RGBWColor segColor = toRGBW(color);
+
+ for (int i = numLeds - 1; i >= 0; i--) {
+ holder[0][i].Color =
+ (numLeds - i <= ledsOff) ? new RGBWColor(0, 0, 0, 0) : segColor;
+ candle.setControl(holder[0][i]);
+ }
};
}
-
- // LEDPattern Methods
- // reversed()
- // offsetBy(int offset)
- // scrollAtAbsoluteSpeed(Distance speed, Distance spacing)
- // scrollAtRelativeSpeed(Frequency velocity)
- // blink(Time onTime, Time offTime)
- // synchronizedBlink(BooleanSupplier signal)
- // breathe(Time period)
- // overlayOn(LEDPattern base)
- // blend(LEDPattern other)
- // mask(LEDPattern mask)
- // atBrightness(Dimensionless relativeBrightness)
- // progressMaskLayer(DoubleSupplier progressSupplier)
- // steps(Map steps)
}
diff --git a/src/main/java/frc/spectrumLib/mechanism/Mechanism.java b/src/main/java/frc/spectrumLib/mechanism/Mechanism.java
index 2df2ef33..e87293a5 100644
--- a/src/main/java/frc/spectrumLib/mechanism/Mechanism.java
+++ b/src/main/java/frc/spectrumLib/mechanism/Mechanism.java
@@ -25,12 +25,11 @@
import edu.wpi.first.wpilibj.DriverStation;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.Commands;
+import edu.wpi.first.wpilibj2.command.Subsystem;
import edu.wpi.first.wpilibj2.command.button.Trigger;
import frc.robot.Robot;
-import frc.spectrumLib.CachedDouble;
-import frc.spectrumLib.SpectrumRobot;
-import frc.spectrumLib.SpectrumSubsystem;
-import frc.spectrumLib.talonFX.TalonFXFactory;
+import frc.spectrumLib.hardware.TalonFXFactory;
+import frc.spectrumLib.util.CachedDouble;
import frc.spectrumLib.util.CanDeviceId;
import frc.spectrumLib.util.Conversions;
import java.util.function.DoubleSupplier;
@@ -71,14 +70,29 @@
* Note: This class assumes CTRE Phoenix 6 units (rotations, rotations/sec, etc.) and uses
* {@code Config.talonConfig.Feedback.SensorToMechanismRatio} as the mechanism gearing ratio.
*/
-public abstract class Mechanism implements SpectrumSubsystem {
+public abstract class Mechanism implements Subsystem {
+
+ // ── Fields ─────────────────────────────────────────────────────────────────
+
+ /** The primary (leader) TalonFX motor controller. */
@Getter protected TalonFX motor;
+
+ /** Optional follower TalonFX motor controllers that mirror the leader. */
@Getter protected TalonFX[] followerMotors;
+
+ /** Configuration object holding motor IDs, Talon settings, and mechanism parameters. */
public Config config;
+ /** Alert displayed when an unexpected current reading is detected during diagnostics. */
Alert currentAlert = new Alert("", AlertType.kWarning);
+
+ /** The last closed-loop position setpoint (in rotations) sent to the motor. */
private double target = 0;
+ /** The last closed-loop velocity setpoint (in rotations per second) sent to the motor. */
+ private double velocityTarget = 0;
+
+ // Cached sensor readings — updated lazily each loop via CachedDouble
private final CachedDouble cachedRotations;
private final CachedDouble cachedPercentage;
private final CachedDouble cachedVoltage;
@@ -88,20 +102,30 @@ public abstract class Mechanism implements SpectrumSubsystem {
private final CachedDouble cachedSupplyCurrent;
private final CachedDouble cachedTemp;
+ // ── Constructors ───────────────────────────────────────────────────────────
+
+ /**
+ * Creates a Mechanism and, if {@link Config#isAttached()} is {@code true}, initializes the
+ * leader TalonFX and any configured follower motors. Sensor caches are always initialized so
+ * safe defaults ({@code 0}) are returned even when unattached.
+ *
+ * @param config the mechanism configuration (motor IDs, Talon settings, follower config, etc.)
+ */
protected Mechanism(Config config) {
this.config = config;
if (isAttached()) {
motor = TalonFXFactory.createConfigTalon(config.id, config.talonConfig);
BaseStatusSignal.setUpdateFrequencyForAll(
- 100,
+ 250,
motor.getDutyCycle(),
motor.getMotorVoltage(),
motor.getTorqueCurrent(),
motor.getStatorCurrent(),
motor.getSupplyCurrent(),
motor.getPosition(),
- motor.getVelocity());
+ motor.getVelocity(),
+ motor.getDeviceTemp());
motor.optimizeBusUtilization();
followerMotors = new TalonFX[config.followerConfigs.length];
@@ -111,6 +135,16 @@ protected Mechanism(Config config) {
config.followerConfigs[i].id,
motor,
config.followerConfigs[i].opposeLeader);
+ BaseStatusSignal.setUpdateFrequencyForAll(
+ 250,
+ followerMotors[i].getDutyCycle(),
+ followerMotors[i].getMotorVoltage(),
+ followerMotors[i].getTorqueCurrent(),
+ followerMotors[i].getStatorCurrent(),
+ followerMotors[i].getSupplyCurrent(),
+ followerMotors[i].getPosition(),
+ followerMotors[i].getVelocity(),
+ followerMotors[i].getDeviceTemp());
followerMotors[i].optimizeBusUtilization();
}
}
@@ -124,54 +158,99 @@ protected Mechanism(Config config) {
cachedVelocity = new CachedDouble(this::updateVelocityRPM);
cachedTemp = new CachedDouble(this::updateTemp);
- SpectrumRobot.add(this);
this.register();
}
+ /**
+ * Creates a Mechanism and explicitly overrides the {@code attached} flag in the config.
+ *
+ * @param config the mechanism configuration
+ * @param attached {@code true} to enable hardware; {@code false} to run in software-only mode
+ */
protected Mechanism(Config config, boolean attached) {
- this(config);
+ // The override must be applied BEFORE the delegated constructor runs, since that
+ // constructor decides whether to create the motor hardware based on the flag.
+ this(applyAttachedOverride(config, attached));
+ }
+
+ private static Config applyAttachedOverride(Config config, boolean attached) {
config.attached = attached;
+ return config;
}
+ // ── Subsystem Overrides ────────────────────────────────────────────────────
+
+ /**
+ * Called once per scheduler loop. Concrete subclasses should override to implement their
+ * periodic state-machine logic, telemetry, and sensor updates.
+ */
@Override
public void periodic() {}
+ /**
+ * Called once per simulation loop. Concrete subclasses should override to update simulation
+ * state (e.g., physics model inputs).
+ */
@Override
public void simulationPeriodic() {}
+ /**
+ * Returns the human-readable name of this mechanism, as defined in its {@link Config}.
+ *
+ * @return the mechanism name
+ */
@Override
public String getName() {
return config.getName();
}
+ // ── Utility ────────────────────────────────────────────────────────────────
+
+ /**
+ * Returns {@code true} if physical hardware is attached and motor commands should be sent.
+ *
+ * @return {@code true} when hardware is available
+ */
public boolean isAttached() {
return config.isAttached();
}
+ /**
+ * Reports the combined supply current draw of the leader motor and all followers to the battery
+ * logger. Does nothing if the mechanism is not attached.
+ */
public void logBatteryUsage() {
if (isAttached()) {
- // Get all motor currents
double motorCurrent = motor.getSupplyCurrent().getValueAsDouble();
double followersCurrent = 0;
for (TalonFX follower : followerMotors) {
followersCurrent += follower.getSupplyCurrent().getValueAsDouble();
}
-
- // Report to battery logger
Robot.getBatteryLogger()
.reportCurrentUsage("Mechanisms/" + getName(), motorCurrent + followersCurrent);
}
}
+ /**
+ * Returns the name of the command currently scheduled on this subsystem, or {@code "none"} if
+ * no command is running.
+ *
+ * @return the current command name
+ */
protected String getCurrentCommandName() {
Command currentCommand = this.getCurrentCommand();
if (currentCommand != null) {
return currentCommand.getName();
}
-
return "none";
}
+ /**
+ * Returns a {@link Trigger} that is active whenever this subsystem's current command is its
+ * default command.
+ *
+ * @return trigger that is {@code true} while the default command is running
+ */
public Trigger runningDefaultCommand() {
return new Trigger(this::isRunningDefaultCommand);
}
@@ -180,19 +259,57 @@ private boolean isRunningDefaultCommand() {
return this.getCurrentCommand() == this.getDefaultCommand();
}
- // Return the closed loop target we have sent to the motor.
+ /**
+ * Returns the last closed-loop setpoint (in rotations) sent to the motor by this class.
+ *
+ * @return the most recent target position in rotations
+ */
public double getTarget() {
return target;
}
+ /**
+ * Returns the last closed-loop velocity setpoint (in rotations per second) sent to the motor by
+ * this class.
+ *
+ * @return the most recent target velocity in rotations per second
+ */
+ public double getVelocityTargetRPS() {
+ return velocityTarget;
+ }
+
+ // ── Triggers ───────────────────────────────────────────────────────────────
+
+ /**
+ * Returns a {@link Trigger} that is active when the motor is within {@code tolerance} rotations
+ * of the last commanded target position.
+ *
+ * @param tolerance maximum allowable error in rotations
+ * @return trigger that is {@code true} when position error is within tolerance
+ */
public Trigger atTargetPosition(DoubleSupplier tolerance) {
return new Trigger(() -> isAtTargetPosition(tolerance));
}
- private boolean isAtTargetPosition(DoubleSupplier tolerance) {
+ /**
+ * Returns {@code true} when the motor is within {@code tolerance} rotations of the last
+ * commanded target position.
+ *
+ * @param tolerance maximum allowable error in rotations
+ * @return {@code true} when position error is within tolerance
+ */
+ public boolean isAtTargetPosition(DoubleSupplier tolerance) {
return Math.abs(cachedRotations.getAsDouble() - target) < tolerance.getAsDouble();
}
+ /**
+ * Returns a {@link Trigger} that is active when the motor position is within {@code tolerance}
+ * of {@code target} (both in rotations).
+ *
+ * @param target the desired position in rotations
+ * @param tolerance maximum allowable error in rotations
+ * @return trigger that is {@code true} when position is within tolerance of target
+ */
public Trigger atRotations(DoubleSupplier target, DoubleSupplier tolerance) {
return new Trigger(
() ->
@@ -200,16 +317,52 @@ public Trigger atRotations(DoubleSupplier target, DoubleSupplier tolerance) {
< tolerance.getAsDouble());
}
+ /**
+ * Returns {@code true} when the motor position is within {@code tolerance} of {@code target}
+ * (both in rotations).
+ *
+ * @param target the desired position in rotations
+ * @param tolerance maximum allowable error in rotations
+ * @return {@code true} when position is within tolerance of target
+ */
+ public boolean isAtRotations(DoubleSupplier target, DoubleSupplier tolerance) {
+ return Math.abs(getPositionRotations() - target.getAsDouble()) < tolerance.getAsDouble();
+ }
+
+ /**
+ * Returns a {@link Trigger} that is active when the motor position is below {@code target +
+ * tolerance} rotations.
+ *
+ * @param target reference position in rotations
+ * @param tolerance offset added to target to form the upper bound
+ * @return trigger that is {@code true} when position is below the threshold
+ */
public Trigger belowRotations(DoubleSupplier target, DoubleSupplier tolerance) {
return new Trigger(
() -> getPositionRotations() < (target.getAsDouble() + tolerance.getAsDouble()));
}
+ /**
+ * Returns a {@link Trigger} that is active when the motor position is above {@code target -
+ * tolerance} rotations.
+ *
+ * @param target reference position in rotations
+ * @param tolerance offset subtracted from target to form the lower bound
+ * @return trigger that is {@code true} when position is above the threshold
+ */
public Trigger aboveRotations(DoubleSupplier target, DoubleSupplier tolerance) {
return new Trigger(
() -> getPositionRotations() > (target.getAsDouble() - tolerance.getAsDouble()));
}
+ /**
+ * Returns a {@link Trigger} that is active when the motor position is within {@code tolerance}
+ * of {@code target} (both as a percentage of max rotations).
+ *
+ * @param target the desired position as a percentage of max rotations
+ * @param tolerance maximum allowable error as a percentage
+ * @return trigger that is {@code true} when position is within tolerance of target
+ */
public Trigger atPercentage(DoubleSupplier target, DoubleSupplier tolerance) {
return new Trigger(
() ->
@@ -217,16 +370,40 @@ public Trigger atPercentage(DoubleSupplier target, DoubleSupplier tolerance) {
< tolerance.getAsDouble());
}
+ /**
+ * Returns a {@link Trigger} that is active when the motor position is below {@code target +
+ * tolerance} (as a percentage of max rotations).
+ *
+ * @param target reference position as a percentage
+ * @param tolerance offset added to target to form the upper bound
+ * @return trigger that is {@code true} when position is below the threshold
+ */
public Trigger belowPercentage(DoubleSupplier target, DoubleSupplier tolerance) {
return new Trigger(
() -> getPositionPercentage() < (target.getAsDouble() + tolerance.getAsDouble()));
}
+ /**
+ * Returns a {@link Trigger} that is active when the motor position is above {@code target -
+ * tolerance} (as a percentage of max rotations).
+ *
+ * @param target reference position as a percentage
+ * @param tolerance offset subtracted from target to form the lower bound
+ * @return trigger that is {@code true} when position is above the threshold
+ */
public Trigger abovePercentage(DoubleSupplier target, DoubleSupplier tolerance) {
return new Trigger(
() -> getPositionPercentage() > (target.getAsDouble() - tolerance.getAsDouble()));
}
+ /**
+ * Returns a {@link Trigger} that is active when the motor position is within {@code tolerance}
+ * of {@code target} (both in degrees).
+ *
+ * @param target the desired position in degrees
+ * @param tolerance maximum allowable error in degrees
+ * @return trigger that is {@code true} when position is within tolerance of target
+ */
public Trigger atDegrees(DoubleSupplier target, DoubleSupplier tolerance) {
return new Trigger(
() ->
@@ -234,31 +411,79 @@ public Trigger atDegrees(DoubleSupplier target, DoubleSupplier tolerance) {
< tolerance.getAsDouble());
}
+ /**
+ * Returns a {@link Trigger} that is active when the motor position is below {@code target +
+ * tolerance} degrees.
+ *
+ * @param target reference position in degrees
+ * @param tolerance offset added to target to form the upper bound
+ * @return trigger that is {@code true} when position is below the threshold
+ */
public Trigger belowDegrees(DoubleSupplier target, DoubleSupplier tolerance) {
return new Trigger(
() -> getPositionDegrees() < (target.getAsDouble() + tolerance.getAsDouble()));
}
+ /**
+ * Returns a {@link Trigger} that is active when the motor position is above {@code target -
+ * tolerance} degrees.
+ *
+ * @param target reference position in degrees
+ * @param tolerance offset subtracted from target to form the lower bound
+ * @return trigger that is {@code true} when position is above the threshold
+ */
public Trigger aboveDegrees(DoubleSupplier target, DoubleSupplier tolerance) {
return new Trigger(
() -> getPositionDegrees() > (target.getAsDouble() - tolerance.getAsDouble()));
}
+ /**
+ * Returns a {@link Trigger} that is active when the motor velocity is within {@code tolerance}
+ * RPM of {@code target}.
+ *
+ * @param target the desired velocity in RPM
+ * @param tolerance maximum allowable error in RPM
+ * @return trigger that is {@code true} when velocity is within tolerance of target
+ */
public Trigger atVelocityRPM(DoubleSupplier target, DoubleSupplier tolerance) {
return new Trigger(
() -> Math.abs(getVelocityRPM() - target.getAsDouble()) < tolerance.getAsDouble());
}
+ /**
+ * Returns a {@link Trigger} that is active when the motor velocity is below {@code target +
+ * tolerance} RPM.
+ *
+ * @param target reference velocity in RPM
+ * @param tolerance offset added to target to form the upper bound
+ * @return trigger that is {@code true} when velocity is below the threshold
+ */
public Trigger belowVelocityRPM(DoubleSupplier target, DoubleSupplier tolerance) {
return new Trigger(
() -> getVelocityRPM() < (target.getAsDouble() + tolerance.getAsDouble()));
}
+ /**
+ * Returns a {@link Trigger} that is active when the motor velocity is above {@code target -
+ * tolerance} RPM.
+ *
+ * @param target reference velocity in RPM
+ * @param tolerance offset subtracted from target to form the lower bound
+ * @return trigger that is {@code true} when velocity is above the threshold
+ */
public Trigger aboveVelocityRPM(DoubleSupplier target, DoubleSupplier tolerance) {
return new Trigger(
() -> getVelocityRPM() > (target.getAsDouble() - tolerance.getAsDouble()));
}
+ /**
+ * Returns a {@link Trigger} that is active when the motor stator current is within {@code
+ * tolerance} amps of {@code target}.
+ *
+ * @param target the desired stator current in amps
+ * @param tolerance maximum allowable error in amps
+ * @return trigger that is {@code true} when stator current is within tolerance of target
+ */
public Trigger atCurrent(DoubleSupplier target, DoubleSupplier tolerance) {
return new Trigger(
() ->
@@ -266,20 +491,38 @@ public Trigger atCurrent(DoubleSupplier target, DoubleSupplier tolerance) {
< tolerance.getAsDouble());
}
+ /**
+ * Returns a {@link Trigger} that is active when the motor stator current is below {@code target
+ * + tolerance} amps.
+ *
+ * @param target reference current in amps
+ * @param tolerance offset added to target to form the upper bound
+ * @return trigger that is {@code true} when stator current is below the threshold
+ */
public Trigger belowCurrent(DoubleSupplier target, DoubleSupplier tolerance) {
return new Trigger(
() -> getStatorCurrent() < (target.getAsDouble() + tolerance.getAsDouble()));
}
+ /**
+ * Returns a {@link Trigger} that is active when the motor stator current is above {@code target
+ * - tolerance} amps.
+ *
+ * @param target reference current in amps
+ * @param tolerance offset subtracted from target to form the lower bound
+ * @return trigger that is {@code true} when stator current is above the threshold
+ */
public Trigger aboveCurrent(DoubleSupplier target, DoubleSupplier tolerance) {
return new Trigger(
() -> getStatorCurrent() > (target.getAsDouble() - tolerance.getAsDouble()));
}
+ // ── Sensor Readings ────────────────────────────────────────────────────────
+
/**
- * Update the value of the stator current for the motor
+ * Reads the stator current directly from the motor hardware.
*
- * @return motor stator current in amps
+ * @return motor stator current in amps, or {@code 0} if not attached
*/
public double updateStatorCurrent() {
if (config.attached) {
@@ -289,7 +532,7 @@ public double updateStatorCurrent() {
}
/**
- * Gets the stator current of the motor
+ * Returns the cached stator current of the motor.
*
* @return motor stator current in amps
*/
@@ -298,9 +541,9 @@ public double getStatorCurrent() {
}
/**
- * Update the value of the supply current for the motor
+ * Reads the supply current directly from the motor hardware.
*
- * @return motor supply current in amps
+ * @return motor supply current in amps, or {@code 0} if not attached
*/
public double updateSupplyCurrent() {
if (config.attached) {
@@ -310,7 +553,7 @@ public double updateSupplyCurrent() {
}
/**
- * Gets the supply current of the motor
+ * Returns the cached supply current of the motor.
*
* @return motor supply current in amps
*/
@@ -319,9 +562,9 @@ public double getSupplyCurrent() {
}
/**
- * Update the value of the voltage for the motor
+ * Reads the motor voltage directly from hardware.
*
- * @return motor voltage in volts
+ * @return motor voltage in volts, or {@code 0} if not attached
*/
public double updateVoltage() {
if (config.attached) {
@@ -331,7 +574,7 @@ public double updateVoltage() {
}
/**
- * Gets the voltage of the motor
+ * Returns the cached voltage of the motor.
*
* @return motor voltage in volts
*/
@@ -340,9 +583,9 @@ public double getVoltage() {
}
/**
- * Update the value of the temperature for the motor
+ * Reads the motor temperature directly from hardware.
*
- * @return motor temperature in Celsius
+ * @return motor temperature in Celsius, or {@code 0} if not attached
*/
public double updateTemp() {
if (config.attached) {
@@ -352,7 +595,7 @@ public double updateTemp() {
}
/**
- * Gets the temperature of the motor
+ * Returns the cached temperature of the motor.
*
* @return motor temperature in Celsius
*/
@@ -360,46 +603,52 @@ public double getTemp() {
return cachedTemp.getAsDouble();
}
+ // ── Unit Conversions ───────────────────────────────────────────────────────
+
/**
- * Percentage to Rotations
+ * Converts a percentage of the mechanism's maximum range into an absolute rotation count.
*
- * @return rotations based on percentage of max rotations
+ * @param percent position as a percentage of max rotations (0–100)
+ * @return the equivalent position in rotations
*/
public double percentToRotations(DoubleSupplier percent) {
return (percent.getAsDouble() / 100) * config.maxRotations;
}
/**
- * Rotations to Percentage
+ * Converts an absolute rotation count into a percentage of the mechanism's maximum range.
*
- * @param rotations
- * @return percentage of max rotations
+ * @param rotations position in rotations
+ * @return the equivalent percentage of max rotations (0–100)
*/
public double rotationsToPercent(DoubleSupplier rotations) {
return (rotations.getAsDouble() / config.maxRotations) * 100;
}
/**
- * Degrees to Rotations
+ * Converts degrees to rotations (1 rotation = 360 degrees).
*
- * @return rotations
+ * @param degrees angle in degrees
+ * @return the equivalent position in rotations
*/
public double degreesToRotations(DoubleSupplier degrees) {
return (degrees.getAsDouble() / 360);
}
/**
- * Rotations to Degrees
+ * Converts rotations to degrees (1 rotation = 360 degrees).
*
- * @param rotations
- * @return degrees
+ * @param rotations position in rotations
+ * @return the equivalent angle in degrees
*/
public double rotationsToDegrees(DoubleSupplier rotations) {
return 360 * rotations.getAsDouble();
}
+ // ── Position & Velocity ────────────────────────────────────────────────────
+
/**
- * Gets the position of the motor in rotations
+ * Returns the cached motor position in rotations.
*
* @return motor position in rotations
*/
@@ -408,9 +657,9 @@ public double getPositionRotations() {
}
/**
- * Updates the position of the motor in rotations
+ * Reads the motor position directly from hardware.
*
- * @return motor position in rotations
+ * @return motor position in rotations, or {@code 0} if not attached
*/
private double updatePositionRotations() {
if (config.attached) {
@@ -420,16 +669,16 @@ private double updatePositionRotations() {
}
/**
- * Gets the position of the motor in percentage of max rotations
+ * Returns the cached motor position as a percentage of max rotations.
*
- * @return motor position in percentage of max rotations
+ * @return motor position in percentage of max rotations (0–100)
*/
public double getPositionPercentage() {
return cachedPercentage.getAsDouble();
}
/**
- * Updates the position of the motor and converts it to percentage of max rotations
+ * Computes the motor position as a percentage of max rotations using the cached rotation value.
*
* @return motor position in percentage of max rotations
*/
@@ -438,7 +687,7 @@ private double updatePositionPercentage() {
}
/**
- * Gets the position of the motor in degrees
+ * Returns the cached motor position in degrees.
*
* @return motor position in degrees
*/
@@ -447,7 +696,7 @@ public double getPositionDegrees() {
}
/**
- * Updates the position of the motor and converts it to degrees
+ * Computes the motor position in degrees using the cached rotation value.
*
* @return motor position in degrees
*/
@@ -456,9 +705,9 @@ private double updatePositionDegrees() {
}
/**
- * Updates the velocity of the motor
+ * Reads the motor velocity directly from hardware in rotations per second (CTRE native units).
*
- * @return motor velocity in rotations/sec which are the CTRE native units
+ * @return motor velocity in rotations per second, or {@code 0} if not attached
*/
private double updateVelocityRPS() {
if (config.attached) {
@@ -468,28 +717,31 @@ private double updateVelocityRPS() {
}
/**
- * Gets the velocity of the motor in RPM
+ * Returns the cached motor velocity in RPM.
*
- * @return motor velocity in RPM
+ * @return motor velocity in revolutions per minute
*/
public double getVelocityRPM() {
return cachedVelocity.getAsDouble();
}
/**
- * Updates the velocity of the motor and converts it to RPM
+ * Computes the motor velocity in RPM from the raw RPS sensor reading.
*
- * @return motor velocity in RPM
+ * @return motor velocity in revolutions per minute
*/
private double updateVelocityRPM() {
return Conversions.RPStoRPM(updateVelocityRPS());
}
- /* Commands: see method in lambda for more information */
+ // ── Command Factories ──────────────────────────────────────────────────────
+
/**
- * Runs the Mechanism at a given velocity
+ * Returns a {@link Command} that continuously drives the mechanism at the specified velocity
+ * using closed-loop voltage control.
*
- * @param velocityRPM in revolutions per minute
+ * @param velocityRPM the target velocity in revolutions per minute
+ * @return a command that runs the mechanism at the given velocity
*/
public Command runVelocity(DoubleSupplier velocityRPM) {
return run(() -> setVelocity(() -> Conversions.RPMtoRPS(velocityRPM)))
@@ -497,11 +749,11 @@ public Command runVelocity(DoubleSupplier velocityRPM) {
}
/**
- * Run the mechanism at given velocity rpm in TorqueCurrentFOC mode
+ * Returns a {@link Command} that continuously drives the mechanism at the specified velocity
+ * using closed-loop Torque Current FOC control (requires Phoenix Pro).
*
- * @param velocityRPM rotations per minute
- * @return A Command that runs the mechanism at the given velocity in rpm using torque current
- * FOC control.
+ * @param velocityRPM the target velocity in revolutions per minute
+ * @return a command that runs the mechanism at the given velocity using torque current FOC
*/
public Command runVelocityTcFocRPM(DoubleSupplier velocityRPM) {
return run(() -> setVelocityTorqueCurrentFOC(() -> Conversions.RPMtoRPS(velocityRPM)))
@@ -509,32 +761,33 @@ public Command runVelocityTcFocRPM(DoubleSupplier velocityRPM) {
}
/**
- * Open-loop percent output control with voltage compensation
+ * Returns a {@link Command} that continuously applies an open-loop percent output to the
+ * mechanism using voltage compensation.
*
- * @param percent A fractional units between -1 and +1
- * @return A Command that runs the mechanism at the given percentage output.
+ * @param percent fractional output between -1 and +1
+ * @return a command that runs the mechanism at the given percent output
*/
public Command runPercentage(DoubleSupplier percent) {
return run(() -> setPercentOutput(percent)).withName(getName() + ".runPercentage");
}
/**
- * Apply a voltage output to the mechanism, bypassing any closed-loop control.
+ * Returns a {@link Command} that continuously applies the specified voltage to the mechanism,
+ * bypassing any closed-loop control.
*
- * @param voltage volts
- * @return A Command that applies the specified voltage output to the mechanism.
+ * @param voltage the desired voltage in volts
+ * @return a command that applies the given voltage output
*/
public Command runVoltage(DoubleSupplier voltage) {
return run(() -> setVoltageOutput(voltage)).withName(getName() + ".runVoltage");
}
/**
- * Apply a voltage output to the mechanism, bypassing any closed-loop control and ignoring
- * software limits.
+ * Returns a {@link Command} that continuously applies the specified voltage to the mechanism,
+ * bypassing closed-loop control and ignoring software limit switches.
*
- * @param voltage volts
- * @return A Command that applies the specified voltage output to the mechanism, ignoring
- * software limits.
+ * @param voltage the desired voltage in volts
+ * @return a command that applies the given voltage output, ignoring software limits
*/
public Command runVoltageNoSoftLimit(DoubleSupplier voltage) {
return run(() -> setVoltageOutputNoSoftLimit(voltage))
@@ -542,32 +795,33 @@ public Command runVoltageNoSoftLimit(DoubleSupplier voltage) {
}
/**
- * Run the mechanism at the specified torque current in FOC control.
+ * Returns a {@link Command} that continuously drives the mechanism at the specified torque
+ * current using FOC control (requires Phoenix Pro).
*
- * @param current torque current
- * @return A Command that runs the mechanism at the given torque current in FOC control.
+ * @param current the desired torque current in amps
+ * @return a command that runs the mechanism at the given torque current
*/
public Command runTorqueCurrentFoc(DoubleSupplier current) {
return run(() -> setTorqueCurrentFoc(current)).withName(getName() + ".runTorqueCurrentFoc");
}
/**
- * Run to the specified position.
+ * Returns a {@link Command} that continuously moves the mechanism to the specified position
+ * using Motion Magic Torque Current FOC control (requires Phoenix Pro).
*
- * @param rotations position in revolutions
- * @return A Command that runs the mechanism to the specified position in revolutions using FOC
- * control.
+ * @param rotations the target position in rotations
+ * @return a command that moves the mechanism to the given position
*/
public Command moveToRotations(DoubleSupplier rotations) {
return run(() -> setMMPositionFoc(rotations)).withName(getName() + ".runPoseRevolutions");
}
/**
- * Move to the specified position.
+ * Returns a {@link Command} that continuously moves the mechanism to the specified position
+ * using Motion Magic Torque Current FOC control (requires Phoenix Pro).
*
- * @param percent position in percentage of max revolutions
- * @return A Command that runs the mechanism to the specified position in percentage of max
- * revolutions using FOC control.
+ * @param percent the target position as a percentage of max rotations (0–100)
+ * @return a command that moves the mechanism to the given percentage position
*/
public Command moveToPercentage(DoubleSupplier percent) {
return run(() -> setMMPositionFoc(() -> percentToRotations(percent)))
@@ -575,11 +829,11 @@ public Command moveToPercentage(DoubleSupplier percent) {
}
/**
- * Move to the specified position.
+ * Returns a {@link Command} that continuously moves the mechanism to the specified angular
+ * position using Motion Magic Torque Current FOC control (requires Phoenix Pro).
*
- * @param degrees position in degrees
- * @return A Command that runs the mechanism to the specified position in degrees using FOC
- * control.
+ * @param degrees the target position in degrees
+ * @return a command that moves the mechanism to the given position in degrees
*/
public Command moveToDegrees(DoubleSupplier degrees) {
return run(() -> setMMPositionFoc(() -> degreesToRotations(degrees)))
@@ -587,31 +841,32 @@ public Command moveToDegrees(DoubleSupplier degrees) {
}
/**
- * Runs to the specified Motion Magic position using FOC control. Will require different PID and
- * feedforward configs
+ * Returns a {@link Command} that continuously moves the mechanism to the specified position
+ * using Motion Magic Torque Current FOC control (requires Phoenix Pro).
+ *
+ * Equivalent to {@link #moveToRotations(DoubleSupplier)} — prefer that method for clarity.
*
- * @param rotations position in revolutions
- * @return A Command that runs the mechanism to the specified position in revolutions using
- * Motion Magic FOC control.
+ * @param rotations the target position in rotations
+ * @return a command that moves the mechanism to the given position
*/
public Command runFocRotations(DoubleSupplier rotations) {
return run(() -> setMMPositionFoc(rotations)).withName(getName() + ".runFOCPosition");
}
/**
- * Stops the mechanism.
+ * Returns a {@link Command} that stops the mechanism and holds it stopped for its duration.
*
- * @return A Command that stops the mechanism.
+ * @return a command that stops the mechanism
*/
public Command runStop() {
return run(this::stop).withName(getName() + ".runStop");
}
/**
- * Temporarily sets the mechanism to coast mode. The configuration is applied when the command
- * is started and reverted when the command is ended.
+ * Returns a {@link Command} that sets the mechanism to coast mode while active, then reverts to
+ * brake mode when the command ends. Safe to run while the robot is disabled.
*
- * @return A Command that sets the mechanism to coast mode.
+ * @return a command that temporarily enables coast mode
*/
public Command coastMode() {
return startEnd(() -> setBrakeMode(false), () -> setBrakeMode(true))
@@ -620,10 +875,10 @@ public Command coastMode() {
}
/**
- * Sets the motor to brake mode if it is in coast mode.
+ * Returns a {@link Command} that sets the mechanism to brake mode if it is currently in coast
+ * mode. Safe to run while the robot is disabled.
*
- * @return A Command that sets the mechanism to brake mode if it is in coast mode, ignoring
- * disable.
+ * @return a command that ensures brake mode is active
*/
public Command ensureBrakeMode() {
return runOnce(() -> setBrakeMode(true))
@@ -636,21 +891,40 @@ public Command ensureBrakeMode() {
.withName(getName() + ".ensureBrakeMode");
}
+ /**
+ * Returns a {@link Command} that applies new supply and stator current limits to the mechanism.
+ *
+ * @param supplyLimit the new supply current limit in amps
+ * @param statorLimit the new stator current limit in amps
+ * @return a command that updates the current limits
+ */
protected Command runCurrentLimits(DoubleSupplier supplyLimit, DoubleSupplier statorLimit) {
return Commands.runOnce(() -> setCurrentLimits(supplyLimit, statorLimit));
}
+ // ── Motor Control (Protected) ──────────────────────────────────────────────
+
+ /**
+ * Immediately applies new supply and stator current limits to the motor configuration.
+ *
+ * @param supplyLimit the new supply current limit in amps
+ * @param statorLimit the new stator current limit in amps
+ */
protected void setCurrentLimits(DoubleSupplier supplyLimit, DoubleSupplier statorLimit) {
applyCurrentLimit(supplyLimit, statorLimit);
}
+ /** Stops the motor output. Does nothing if the mechanism is not attached. */
protected void stop() {
if (isAttached()) {
motor.stopMotor();
}
}
- /** Sets the mechanism position of the motor to 0 */
+ /**
+ * Sets the mechanism's reported position to zero (tares the motor encoder). Does nothing if the
+ * mechanism is not attached.
+ */
protected void tareMotor() {
if (isAttached()) {
setMotorPosition(() -> 0);
@@ -658,9 +932,9 @@ protected void tareMotor() {
}
/**
- * Sets the mechanism position of the motor
+ * Sets the motor's internal position register to the specified value without moving the motor.
*
- * @param rotations rotations
+ * @param rotations the position to write to the motor in rotations
*/
protected void setMotorPosition(DoubleSupplier rotations) {
if (isAttached()) {
@@ -669,61 +943,67 @@ protected void setMotorPosition(DoubleSupplier rotations) {
}
/**
- * Closed-loop Velocity Motion Magic with torque control (requires Pro)
+ * Closed-loop velocity control using Motion Magic with Torque Current FOC (requires Phoenix
+ * Pro).
*
- * @param velocityRPS rotations per second
+ * @param velocityRPS the target velocity in rotations per second
*/
protected void setMMVelocityFOC(DoubleSupplier velocityRPS) {
if (isAttached()) {
- target = velocityRPS.getAsDouble();
- MotionMagicVelocityTorqueCurrentFOC mm = config.mmVelocityFOC.withVelocity(target);
+ velocityTarget = velocityRPS.getAsDouble();
+ MotionMagicVelocityTorqueCurrentFOC mm =
+ config.mmVelocityFOC.withVelocity(velocityTarget);
motor.setControl(mm);
}
}
/**
- * Closed-loop Velocity with torque control (requires Pro)
+ * Closed-loop velocity control using Torque Current FOC (requires Phoenix Pro).
*
- * @param velocityRPS rotations per second
+ * @param velocityRPS the target velocity in rotations per second
*/
protected void setVelocityTorqueCurrentFOC(DoubleSupplier velocityRPS) {
if (isAttached()) {
- target = velocityRPS.getAsDouble();
- VelocityTorqueCurrentFOC output = config.velocityTorqueCurrentFOC.withVelocity(target);
+ velocityTarget = velocityRPS.getAsDouble();
+ VelocityTorqueCurrentFOC output =
+ config.velocityTorqueCurrentFOC.withVelocity(velocityTarget);
motor.setControl(output);
}
}
/**
- * Closed-loop Velocity with torque control (requires Pro)
+ * Closed-loop velocity control using Torque Current FOC with an RPM input (requires Phoenix
+ * Pro). The RPM value is converted to RPS internally before being sent to the motor.
*
- * @param velocityRPS rotations per second
+ * @param velocityRPM the target velocity in revolutions per minute
*/
- protected void setVelocityTCFOCrpm(DoubleSupplier velocityRPS) {
+ protected void setVelocityTCFOCrpm(DoubleSupplier velocityRPM) {
if (isAttached()) {
- target = Conversions.RPMtoRPS(velocityRPS.getAsDouble());
- VelocityTorqueCurrentFOC output = config.velocityTorqueCurrentFOC.withVelocity(target);
+ velocityTarget = Conversions.RPMtoRPS(velocityRPM.getAsDouble());
+ VelocityTorqueCurrentFOC output =
+ config.velocityTorqueCurrentFOC.withVelocity(velocityTarget);
motor.setControl(output);
}
}
/**
- * Closed-loop velocity control with voltage compensation
+ * Closed-loop velocity control with voltage compensation.
*
- * @param velocityRPS rotations per second
+ * @param velocityRPS the target velocity in rotations per second
*/
protected void setVelocity(DoubleSupplier velocityRPS) {
if (isAttached()) {
- target = velocityRPS.getAsDouble();
- VelocityVoltage output = config.velocityControl.withVelocity(target);
+ velocityTarget = velocityRPS.getAsDouble();
+ VelocityVoltage output = config.velocityControl.withVelocity(velocityTarget);
motor.setControl(output);
}
}
/**
- * Closed-loop Position Motion Magic with torque control (requires Pro)
+ * Closed-loop position control using Motion Magic with Torque Current FOC (requires Phoenix
+ * Pro).
*
- * @param rotations rotations
+ * @param rotations the target position in rotations
*/
protected void setMMPositionFoc(DoubleSupplier rotations) {
if (isAttached()) {
@@ -734,13 +1014,13 @@ protected void setMMPositionFoc(DoubleSupplier rotations) {
}
/**
- * Closed-loop Position Motion Magic with torque control (requires Pro). Dynamic allows you to
- * set velocity, acceleration, and jerk during the command.
+ * Closed-loop position control using Dynamic Motion Magic with Torque Current FOC (requires
+ * Phoenix Pro). Trajectory parameters can be changed every loop cycle.
*
- * @param rotations The target position in rotations.
- * @param velocity The maximum velocity in rotations per second.
- * @param acceleration The maximum acceleration in rotations per second squared.
- * @param jerk The maximum jerk in rotations per second cubed.
+ * @param rotations the target position in rotations
+ * @param velocity the cruise velocity in rotations per second
+ * @param acceleration the acceleration in rotations per second squared
+ * @param jerk the jerk in rotations per second cubed
*/
protected void setDynMMPositionFoc(
DoubleSupplier rotations,
@@ -760,13 +1040,13 @@ protected void setDynMMPositionFoc(
}
/**
- * Closed-loop Position Motion Magic with voltage control. Dynamic allows you to set velocity,
- * acceleration, and jerk during the command.
+ * Closed-loop position control using Dynamic Motion Magic with voltage compensation. Trajectory
+ * parameters can be changed every loop cycle.
*
- * @param rotations The target position in rotations.
- * @param velocity The maximum velocity in rotations per second.
- * @param acceleration The maximum acceleration in rotations per second squared.
- * @param jerk The maximum jerk in rotations per second cubed.
+ * @param rotations the target position in rotations
+ * @param velocity the cruise velocity in rotations per second
+ * @param acceleration the acceleration in rotations per second squared
+ * @param jerk the jerk in rotations per second cubed
*/
protected void setDynMMPositionVoltage(
DoubleSupplier rotations,
@@ -786,19 +1066,20 @@ protected void setDynMMPositionVoltage(
}
/**
- * Closed-loop Position Motion Magic
+ * Closed-loop position control using Motion Magic with voltage compensation (slot 0).
*
- * @param rotations rotations
+ * @param rotations the target position in rotations
*/
protected void setMMPosition(DoubleSupplier rotations) {
setMMPosition(rotations, 0);
}
/**
- * Closed-loop Position Motion Magic using a slot other than 0
+ * Closed-loop position control using Motion Magic with voltage compensation and an explicit
+ * PID/FF gain slot.
*
- * @param rotations rotations
- * @param slot gains slot
+ * @param rotations the target position in rotations
+ * @param slot the gain slot to use (0, 1, or 2)
*/
public void setMMPosition(DoubleSupplier rotations, int slot) {
if (isAttached()) {
@@ -809,10 +1090,13 @@ public void setMMPosition(DoubleSupplier rotations, int slot) {
}
}
+ // ── Motor Control (Public) ─────────────────────────────────────────────────
+
/**
- * Open-loop Percent output control with voltage compensation
+ * Open-loop percent output control with voltage compensation. The output voltage is {@code
+ * percent × voltageCompSaturation}.
*
- * @param percent fractional units between -1 and +1
+ * @param percent fractional output between -1 and +1
*/
public void setPercentOutput(DoubleSupplier percent) {
if (isAttached()) {
@@ -824,9 +1108,10 @@ public void setPercentOutput(DoubleSupplier percent) {
}
/**
- * Open-loop voltage control
+ * Open-loop voltage control — applies the requested voltage directly without compensation
+ * scaling.
*
- * @param voltage volts
+ * @param voltage the desired voltage in volts
*/
public void setVoltageOutput(DoubleSupplier voltage) {
if (isAttached()) {
@@ -836,9 +1121,10 @@ public void setVoltageOutput(DoubleSupplier voltage) {
}
/**
- * Open-loop voltage control that ignores software limits
+ * Open-loop voltage control that ignores software limit switches. Use with caution — this can
+ * drive the mechanism past its configured travel limits.
*
- * @param voltage volts
+ * @param voltage the desired voltage in volts
*/
public void setVoltageOutputNoSoftLimit(DoubleSupplier voltage) {
if (isAttached()) {
@@ -850,6 +1136,11 @@ public void setVoltageOutputNoSoftLimit(DoubleSupplier voltage) {
}
}
+ /**
+ * Applies a torque current setpoint using FOC control (requires Phoenix Pro).
+ *
+ * @param current the desired torque current in amps
+ */
public void setTorqueCurrentFoc(DoubleSupplier current) {
if (isAttached()) {
TorqueCurrentFOC output = config.torqueCurrentFOC.withOutput(current.getAsDouble());
@@ -857,6 +1148,14 @@ public void setTorqueCurrentFoc(DoubleSupplier current) {
}
}
+ // ── Hardware Configuration ─────────────────────────────────────────────────
+
+ /**
+ * Sets the motor's neutral mode to brake or coast and immediately applies the change to
+ * hardware.
+ *
+ * @param isInBrake {@code true} to set brake mode; {@code false} to set coast mode
+ */
public void setBrakeMode(boolean isInBrake) {
if (isAttached()) {
config.configNeutralBrakeMode(isInBrake);
@@ -864,6 +1163,12 @@ public void setBrakeMode(boolean isInBrake) {
}
}
+ /**
+ * Enables or disables the reverse software limit switch and immediately applies the change. The
+ * threshold is read from the current configuration.
+ *
+ * @param enabled {@code true} to enable the reverse soft limit; {@code false} to disable it
+ */
public void toggleReverseSoftLimit(boolean enabled) {
if (isAttached()) {
double threshold = config.talonConfig.SoftwareLimitSwitch.ReverseSoftLimitThreshold;
@@ -872,6 +1177,13 @@ public void toggleReverseSoftLimit(boolean enabled) {
}
}
+ /**
+ * Enables or disables a forward/reverse torque current limit and immediately applies the
+ * change. When disabled, the peak torque current is reset to ±300 A (effectively unlimited).
+ *
+ * @param enabledLimit the torque current limit in amps when {@code enabled} is {@code true}
+ * @param enabled {@code true} to apply the limit; {@code false} to remove it
+ */
public void toggleTorqueCurrentLimit(DoubleSupplier enabledLimit, boolean enabled) {
if (isAttached()) {
if (enabled) {
@@ -887,6 +1199,12 @@ public void toggleTorqueCurrentLimit(DoubleSupplier enabledLimit, boolean enable
}
}
+ /**
+ * Enables or disables the supply current limit and immediately applies the change.
+ *
+ * @param enabledLimit the supply current limit in amps
+ * @param enabled {@code true} to enable the limit; {@code false} to disable it
+ */
public void toggleSupplyCurrentLimit(DoubleSupplier enabledLimit, boolean enabled) {
if (isAttached()) {
if (enabled) {
@@ -899,10 +1217,17 @@ public void toggleSupplyCurrentLimit(DoubleSupplier enabledLimit, boolean enable
}
}
+ /**
+ * Applies new supply and stator current limits if the requested values differ from the
+ * currently configured limits. The update is retried up to 10 times on failure.
+ *
+ * @param supplyLimit the new supply current limit in amps
+ * @param statorLimit the new stator current limit in amps
+ */
public void applyCurrentLimit(DoubleSupplier supplyLimit, DoubleSupplier statorLimit) {
if (isAttached()) {
if (config.talonConfig.CurrentLimits.StatorCurrentLimit != statorLimit.getAsDouble()
- && config.talonConfig.CurrentLimits.SupplyCurrentLimit
+ || config.talonConfig.CurrentLimits.SupplyCurrentLimit
!= supplyLimit.getAsDouble()) {
config.configSupplyCurrentLimit(Math.abs(supplyLimit.getAsDouble()), true);
config.configStatorCurrentLimit(Math.abs(statorLimit.getAsDouble()), true);
@@ -923,6 +1248,17 @@ public void applyCurrentLimit(DoubleSupplier supplyLimit, DoubleSupplier statorL
}
}
+ // ── Diagnostic Commands ────────────────────────────────────────────────────
+
+ /**
+ * Returns a {@link Command} that measures the average stator current over its runtime and fires
+ * a warning {@link Alert} if the average deviates from {@code expectedCurrent} by more than
+ * {@code tolerance}.
+ *
+ * @param expectedCurrent the expected average stator current in amps
+ * @param tolerance the maximum acceptable deviation in amps
+ * @return a diagnostic command that checks average current
+ */
public Command checkAvgCurrent(DoubleSupplier expectedCurrent, DoubleSupplier tolerance) {
return new Command() {
double totalCurrent = 0;
@@ -958,6 +1294,13 @@ public void end(boolean interrupted) {
};
}
+ /**
+ * Returns a {@link Command} that tracks the peak stator current over its runtime and fires a
+ * warning {@link Alert} if the peak exceeds {@code expectedCurrent}.
+ *
+ * @param expectedCurrent the maximum acceptable peak stator current in amps
+ * @return a diagnostic command that checks peak current
+ */
public Command checkMaxCurrent(DoubleSupplier expectedCurrent) {
return new Command() {
double maxCurrent = 0;
@@ -991,6 +1334,14 @@ public void end(boolean interrupted) {
};
}
+ /**
+ * Returns a {@link Command} that tracks the peak stator current over its runtime and fires a
+ * warning {@link Alert} if the peak never reaches {@code expectedCurrent}. Use this to verify
+ * that a mechanism drew at least the expected minimum load.
+ *
+ * @param expectedCurrent the minimum acceptable peak stator current in amps
+ * @return a diagnostic command that checks whether a minimum current threshold was reached
+ */
public Command checkMinThresholdCurrent(DoubleSupplier expectedCurrent) {
return new Command() {
double maxCurrent = 0;
@@ -1024,12 +1375,40 @@ public void end(boolean interrupted) {
};
}
+ // ── Nested Classes ─────────────────────────────────────────────────────────
+
+ /**
+ * Configuration for a TalonFX follower motor that mirrors the leader.
+ *
+ * A follower automatically copies the leader's output. Set {@code opposeLeader} to {@link
+ * MotorAlignmentValue#Opposed} when the physical motor is mounted in the opposite direction and
+ * must spin in reverse to produce the same mechanism motion.
+ */
public static class FollowerConfig {
+
+ /** Human-readable name for this follower motor (used in alerts and logging). */
@Getter private String name;
+
+ /** CAN bus device ID and bus name for this follower motor. */
@Getter private CanDeviceId id;
+
+ /** Whether hardware is attached for this follower. */
@Getter private boolean attached = true;
+
+ /**
+ * Alignment of the follower relative to the leader. Use {@link MotorAlignmentValue#Opposed}
+ * when the follower is physically mounted in the opposite direction.
+ */
@Getter private MotorAlignmentValue opposeLeader = MotorAlignmentValue.Aligned;
+ /**
+ * Creates a follower motor configuration.
+ *
+ * @param name human-readable name for this follower
+ * @param id CAN device ID
+ * @param canbus CAN bus name (e.g., {@code "rio"} or {@code "canivore"})
+ * @param opposeLeader alignment relative to the leader motor
+ */
public FollowerConfig(
String name, int id, String canbus, MotorAlignmentValue opposeLeader) {
this.name = name;
@@ -1038,18 +1417,52 @@ public FollowerConfig(
}
}
+ /**
+ * Configuration for a {@link Mechanism}, encapsulating the TalonFX hardware configuration,
+ * control request objects, and mechanism-level parameters (gear ratio, soft limits, PID/FF
+ * gains, Motion Magic profile, current limits, etc.).
+ *
+ * Subclass this in each concrete mechanism and call the {@code config*()} helpers in the
+ * constructor to set mechanism-specific parameters before passing the config to the {@link
+ * Mechanism} constructor.
+ */
public static class Config {
+
+ /** Human-readable name for this mechanism (used in logging and alerts). */
@Getter private String name;
+
+ /**
+ * Whether physical hardware is attached. Set to {@code false} to run in simulation-only
+ * mode.
+ */
@Getter @Setter private boolean attached = true;
+
+ /** CAN bus device ID and bus name for the leader motor. */
@Getter private CanDeviceId id;
+
+ /** Full TalonFX hardware configuration applied to the leader motor on startup. */
@Getter @Setter protected TalonFXConfiguration talonConfig;
+
+ /** Total number of motors (leader + followers). */
@Getter private int numMotors = 1;
- @Getter private double voltageCompSaturation = 12.0; // 12V by default
+
+ /**
+ * Voltage compensation saturation value used by {@link Mechanism#setPercentOutput}.
+ * Defaults to 12 V.
+ */
+ @Getter private double voltageCompSaturation = 12.0;
+
+ /** Minimum mechanism position in rotations (used for range calculations). */
@Getter private double minRotations = 0;
+
+ /** Maximum mechanism position in rotations (used for range calculations). */
@Getter private double maxRotations = 1;
+ /** Configurations for follower motors. Empty by default (no followers). */
@Getter private FollowerConfig[] followerConfigs = new FollowerConfig[0];
+ // Pre-built control request objects — reused each loop to avoid GC pressure.
+
@Getter
private MotionMagicVelocityTorqueCurrentFOC mmVelocityFOC =
new MotionMagicVelocityTorqueCurrentFOC(0);
@@ -1081,9 +1494,19 @@ public static class Config {
@Getter private TorqueCurrentFOC torqueCurrentFOC = new TorqueCurrentFOC(0);
- // Percent Output control using percentage of supply voltage. Should normally use VoltageOut
+ /** Percent (duty-cycle) output control — prefer {@link #voltageControl} in most cases. */
@Getter private DutyCycleOut percentOutput = new DutyCycleOut(0);
+ // ── Constructor ───────────────────────────────────────────────────────
+
+ /**
+ * Creates a base mechanism configuration with default Talon settings. Hardware limit
+ * switches are disabled by default.
+ *
+ * @param name human-readable name for this mechanism
+ * @param id CAN device ID of the leader motor
+ * @param canbus CAN bus name (e.g., {@code "rio"} or {@code "canivore"})
+ */
public Config(String name, int id, String canbus) {
this.name = name;
this.id = new CanDeviceId(id, canbus);
@@ -1094,6 +1517,14 @@ public Config(String name, int id, String canbus) {
talonConfig.HardwareLimitSwitch.ReverseLimitEnable = false;
}
+ // ── Config Helpers ────────────────────────────────────────────────────
+
+ /**
+ * Applies the current {@link TalonFXConfiguration} to the given motor and reports a warning
+ * to the DriverStation if the apply fails.
+ *
+ * @param talon the TalonFX motor to configure
+ */
public void applyTalonConfig(TalonFX talon) {
StatusCode result = talon.getConfigurator().apply(talonConfig);
if (!result.isOK()) {
@@ -1102,30 +1533,62 @@ public void applyTalonConfig(TalonFX talon) {
}
}
+ /**
+ * Sets the follower motor configurations.
+ *
+ * @param followers one or more {@link FollowerConfig} objects describing follower motors
+ */
public void setFollowerConfigs(FollowerConfig... followers) {
followerConfigs = followers;
}
+ /**
+ * Sets the voltage compensation saturation voltage used by percent-output control.
+ *
+ * @param voltageCompSaturation the saturation voltage in volts (typically 12.0)
+ */
public void configVoltageCompensation(double voltageCompSaturation) {
this.voltageCompSaturation = voltageCompSaturation;
}
+ /**
+ * Configures the motor output as counter-clockwise positive (default for most mechanisms
+ * when viewed from the shaft end).
+ */
public void configCounterClockwise_Positive() {
talonConfig.MotorOutput.Inverted = InvertedValue.CounterClockwise_Positive;
}
+ /** Configures the motor output as clockwise positive (inverted relative to the default). */
public void configClockwise_Positive() {
talonConfig.MotorOutput.Inverted = InvertedValue.Clockwise_Positive;
}
+ /**
+ * Sets the peak forward output voltage.
+ *
+ * @param voltageLimit maximum forward voltage in volts
+ */
public void configForwardVoltageLimit(double voltageLimit) {
talonConfig.Voltage.PeakForwardVoltage = voltageLimit;
}
+ /**
+ * Sets the peak reverse output voltage.
+ *
+ * @param voltageLimit maximum reverse voltage in volts
+ */
public void configReverseVoltageLimit(double voltageLimit) {
talonConfig.Voltage.PeakReverseVoltage = voltageLimit;
}
+ /**
+ * Configures the supply current limit. The absolute value of {@code supplyLimit} is used,
+ * so negative values are automatically corrected.
+ *
+ * @param supplyLimit the supply current limit in amps
+ * @param enabled {@code true} to enable the limit
+ */
public void configSupplyCurrentLimit(double supplyLimit, boolean enabled) {
if (supplyLimit < 0) {
supplyLimit = -supplyLimit;
@@ -1134,6 +1597,13 @@ public void configSupplyCurrentLimit(double supplyLimit, boolean enabled) {
talonConfig.CurrentLimits.SupplyCurrentLimitEnable = enabled;
}
+ /**
+ * Configures the stator current limit. The absolute value of {@code statorLimit} is used,
+ * so negative values are automatically corrected.
+ *
+ * @param statorLimit the stator current limit in amps
+ * @param enabled {@code true} to enable the limit
+ */
public void configStatorCurrentLimit(double statorLimit, boolean enabled) {
if (statorLimit < 0) {
statorLimit = -statorLimit;
@@ -1142,6 +1612,12 @@ public void configStatorCurrentLimit(double statorLimit, boolean enabled) {
talonConfig.CurrentLimits.StatorCurrentLimitEnable = enabled;
}
+ /**
+ * Sets the peak forward torque current limit. The absolute value is used so negative inputs
+ * are corrected automatically.
+ *
+ * @param currentLimit peak forward torque current in amps
+ */
public void configForwardTorqueCurrentLimit(double currentLimit) {
if (currentLimit < 0) {
currentLimit = -currentLimit;
@@ -1149,6 +1625,12 @@ public void configForwardTorqueCurrentLimit(double currentLimit) {
talonConfig.TorqueCurrent.PeakForwardTorqueCurrent = currentLimit;
}
+ /**
+ * Sets the peak reverse torque current limit. The value is forced negative so positive
+ * inputs are corrected automatically.
+ *
+ * @param currentLimit peak reverse torque current in amps (sign is corrected if positive)
+ */
public void configReverseTorqueCurrentLimit(double currentLimit) {
if (currentLimit > 0) {
currentLimit = -currentLimit;
@@ -1156,38 +1638,84 @@ public void configReverseTorqueCurrentLimit(double currentLimit) {
talonConfig.TorqueCurrent.PeakReverseTorqueCurrent = currentLimit;
}
+ /**
+ * Sets the lower supply current limit, used to reduce dissipation after the upper limit has
+ * been triggered.
+ *
+ * @param currentLimit the lower supply current limit in amps
+ */
public void configLowerSupplyCurrentLimit(double currentLimit) {
talonConfig.CurrentLimits.SupplyCurrentLowerLimit = currentLimit;
}
+ /**
+ * Sets the time window for the lower supply current limit.
+ *
+ * @param time the time in seconds
+ */
public void configLowerSupplyCurrentTime(double time) {
talonConfig.CurrentLimits.SupplyCurrentLowerTime = time;
}
+ /**
+ * Sets the duty-cycle neutral deadband. Outputs below this magnitude are treated as zero.
+ *
+ * @param deadband deadband as a fraction of full output (e.g., {@code 0.001})
+ */
public void configNeutralDeadband(double deadband) {
talonConfig.MotorOutput.DutyCycleNeutralDeadband = deadband;
}
+ /**
+ * Sets the peak forward and reverse duty-cycle output limits.
+ *
+ * @param forward maximum forward output (0 to 1)
+ * @param reverse maximum reverse output (-1 to 0)
+ */
public void configPeakOutput(double forward, double reverse) {
talonConfig.MotorOutput.PeakForwardDutyCycle = forward;
talonConfig.MotorOutput.PeakReverseDutyCycle = reverse;
}
+ /**
+ * Configures the forward software limit switch.
+ *
+ * @param threshold the position threshold in rotations
+ * @param enabled {@code true} to enable the limit
+ */
public void configForwardSoftLimit(double threshold, boolean enabled) {
talonConfig.SoftwareLimitSwitch.ForwardSoftLimitThreshold = threshold;
talonConfig.SoftwareLimitSwitch.ForwardSoftLimitEnable = enabled;
}
+ /**
+ * Configures the reverse software limit switch.
+ *
+ * @param threshold the position threshold in rotations
+ * @param enabled {@code true} to enable the limit
+ */
public void configReverseSoftLimit(double threshold, boolean enabled) {
talonConfig.SoftwareLimitSwitch.ReverseSoftLimitThreshold = threshold;
talonConfig.SoftwareLimitSwitch.ReverseSoftLimitEnable = enabled;
}
+ /**
+ * Enables or disables continuous position wrap-around for closed-loop control. Useful for
+ * mechanisms that rotate continuously (e.g., swerve azimuth).
+ *
+ * @param enabled {@code true} to enable continuous wrap
+ */
public void configContinuousWrap(boolean enabled) {
talonConfig.ClosedLoopGeneral.ContinuousWrap = enabled;
}
- // Configure optional motion magic velocity parameters
+ /**
+ * Configures optional Motion Magic velocity parameters (acceleration and feed-forward) for
+ * both FOC and voltage velocity control requests.
+ *
+ * @param acceleration the velocity acceleration in rotations per second squared
+ * @param feedforward the feed-forward term applied during velocity control
+ */
public void configMotionMagicVelocity(double acceleration, double feedforward) {
mmVelocityFOC =
mmVelocityFOC.withAcceleration(acceleration).withFeedForward(feedforward);
@@ -1195,28 +1723,54 @@ public void configMotionMagicVelocity(double acceleration, double feedforward) {
mmVelocityVoltage.withAcceleration(acceleration).withFeedForward(feedforward);
}
- // Configure optional motion magic position parameters
+ /**
+ * Configures the feed-forward term for Motion Magic position control requests.
+ *
+ * @param feedforward the feed-forward term to apply during position control
+ */
public void configMotionMagicPosition(double feedforward) {
mmPositionFOC = mmPositionFOC.withFeedForward(feedforward);
mmPositionVoltage = mmPositionVoltage.withFeedForward(feedforward);
}
+ /**
+ * Configures the Motion Magic cruise velocity, acceleration, and jerk limits.
+ *
+ * @param cruiseVelocity maximum cruise velocity in rotations per second
+ * @param acceleration maximum acceleration in rotations per second squared
+ * @param jerk maximum jerk in rotations per second cubed
+ */
public void configMotionMagic(double cruiseVelocity, double acceleration, double jerk) {
talonConfig.MotionMagic.MotionMagicCruiseVelocity = cruiseVelocity;
talonConfig.MotionMagic.MotionMagicAcceleration = acceleration;
talonConfig.MotionMagic.MotionMagicJerk = jerk;
}
- // This is the ratio of rotor rotations to the mechanism's output.
- // If a remote sensor is used this a ratio of sensor rotations to the mechanism's output.
+ /**
+ * Configures the sensor-to-mechanism gear ratio. This is the ratio of rotor rotations to
+ * one full mechanism output rotation (or sensor rotations if a remote sensor is used).
+ *
+ * @param gearRatio the gear ratio (e.g., {@code 11.25} means 11.25 rotor turns per output
+ * rotation)
+ */
public void configGearRatio(double gearRatio) {
talonConfig.Feedback.SensorToMechanismRatio = gearRatio;
}
+ /**
+ * Returns the currently configured sensor-to-mechanism gear ratio.
+ *
+ * @return the gear ratio
+ */
public double getGearRatio() {
return talonConfig.Feedback.SensorToMechanismRatio;
}
+ /**
+ * Sets the motor neutral mode to brake or coast.
+ *
+ * @param isInBrake {@code true} for brake mode; {@code false} for coast mode
+ */
public void configNeutralBrakeMode(boolean isInBrake) {
if (isInBrake) {
talonConfig.MotorOutput.NeutralMode = NeutralModeValue.Brake;
@@ -1226,54 +1780,90 @@ public void configNeutralBrakeMode(boolean isInBrake) {
}
/**
- * Defaults to slot 0
+ * Configures PID gains in slot 0.
*
- * @param kP
- * @param kI
- * @param kD
+ * @param kP proportional gain
+ * @param kI integral gain
+ * @param kD derivative gain
*/
public void configPIDGains(double kP, double kI, double kD) {
configPIDGains(0, kP, kI, kD);
}
+ /**
+ * Configures PID gains in the specified slot.
+ *
+ * @param slot the gain slot (0, 1, or 2)
+ * @param kP proportional gain
+ * @param kI integral gain
+ * @param kD derivative gain
+ */
public void configPIDGains(int slot, double kP, double kI, double kD) {
talonConfigFeedbackPID(slot, kP, kI, kD);
}
/**
- * Defaults to slot 0
+ * Configures feed-forward gains in slot 0.
*
- * @param kS
- * @param kV
- * @param kA
- * @param kG
+ * @param kS static friction compensation (volts or amps)
+ * @param kV velocity feed-forward gain
+ * @param kA acceleration feed-forward gain
+ * @param kG gravity/load compensation gain
*/
public void configFeedForwardGains(double kS, double kV, double kA, double kG) {
configFeedForwardGains(0, kS, kV, kA, kG);
}
+ /**
+ * Configures feed-forward gains in the specified slot.
+ *
+ * @param slot the gain slot (0, 1, or 2)
+ * @param kS static friction compensation (volts or amps)
+ * @param kV velocity feed-forward gain
+ * @param kA acceleration feed-forward gain
+ * @param kG gravity/load compensation gain
+ */
public void configFeedForwardGains(int slot, double kS, double kV, double kA, double kG) {
talonConfigFeedForward(slot, kV, kA, kS, kG);
}
+ /**
+ * Configures the feedback sensor source using a rotorOffset of {@code 0}.
+ *
+ * @param source the feedback sensor source (e.g., remote CANcoder)
+ */
public void configFeedbackSensorSource(FeedbackSensorSourceValue source) {
configFeedbackSensorSource(source, 0);
}
+ /**
+ * Configures the feedback sensor source and its rotational offset.
+ *
+ * @param source the feedback sensor source
+ * @param offset the feedback rotor offset in rotations
+ */
public void configFeedbackSensorSource(FeedbackSensorSourceValue source, double offset) {
talonConfig.Feedback.FeedbackSensorSource = source;
talonConfig.Feedback.FeedbackRotorOffset = offset;
}
/**
- * Defaults to slot 0
+ * Configures the gravity compensation type in slot 0.
*
- * @param isArm
+ * @param isArm {@code true} for {@link GravityTypeValue#Arm_Cosine} (rotating arm); {@code
+ * false} for {@link GravityTypeValue#Elevator_Static} (elevator)
*/
public void configGravityType(boolean isArm) {
configGravityType(0, isArm);
}
+ /**
+ * Configures the gravity compensation type in the specified slot.
+ *
+ * @param slot the gain slot (0, 1, or 2)
+ * @param isArm {@code true} for {@link GravityTypeValue#Arm_Cosine} (rotating arm); {@code
+ * false} for {@link GravityTypeValue#Elevator_Static} (elevator)
+ */
public void configGravityType(int slot, boolean isArm) {
GravityTypeValue gravityType =
isArm ? GravityTypeValue.Arm_Cosine : GravityTypeValue.Elevator_Static;
@@ -1288,7 +1878,29 @@ public void configGravityType(int slot, boolean isArm) {
}
}
- // Configure the TalonFXConfiguration feed forward gains
+ /**
+ * Sets the minimum and maximum rotation limits for the mechanism. These bounds are used by
+ * unit-conversion helpers such as {@link Mechanism#percentToRotations}.
+ *
+ * @param minRotation the minimum position in rotations
+ * @param maxRotation the maximum position in rotations
+ */
+ protected void configMinMaxRotations(double minRotation, double maxRotation) {
+ this.minRotations = minRotation;
+ this.maxRotations = maxRotation;
+ }
+
+ // ── Private Helpers ───────────────────────────────────────────────────
+
+ /**
+ * Applies feed-forward gains (kV, kA, kS, kG) to the specified TalonFX slot.
+ *
+ * @param slot the gain slot (0, 1, or 2)
+ * @param kV velocity feed-forward
+ * @param kA acceleration feed-forward
+ * @param kS static friction compensation
+ * @param kG gravity compensation
+ */
private void talonConfigFeedForward(int slot, double kV, double kA, double kS, double kG) {
if (slot == 0) {
talonConfig.Slot0.kV = kV;
@@ -1310,6 +1922,14 @@ private void talonConfigFeedForward(int slot, double kV, double kA, double kS, d
}
}
+ /**
+ * Applies PID gains (kP, kI, kD) to the specified TalonFX slot.
+ *
+ * @param slot the gain slot (0, 1, or 2)
+ * @param kP proportional gain
+ * @param kI integral gain
+ * @param kD derivative gain
+ */
private void talonConfigFeedbackPID(int slot, double kP, double kI, double kD) {
if (slot == 0) {
talonConfig.Slot0.kP = kP;
@@ -1327,16 +1947,5 @@ private void talonConfigFeedbackPID(int slot, double kP, double kI, double kD) {
DriverStation.reportWarning("MechConfig: Invalid Feedback slot", false);
}
}
-
- /**
- * Sets the minimum and maximum motor rotations
- *
- * @param minRotation
- * @param maxRotation
- */
- protected void configMinMaxRotations(double minRotation, double maxRotation) {
- this.minRotations = minRotation;
- this.maxRotations = maxRotation;
- }
}
}
diff --git a/src/main/java/frc/spectrumLib/sim/ArmConfig.java b/src/main/java/frc/spectrumLib/sim/ArmConfig.java
index 7ee1809a..377fcf1c 100644
--- a/src/main/java/frc/spectrumLib/sim/ArmConfig.java
+++ b/src/main/java/frc/spectrumLib/sim/ArmConfig.java
@@ -5,33 +5,71 @@
import lombok.Getter;
import lombok.Setter;
+/**
+ * Configuration data for an arm simulation. Stores physical properties, display settings, and
+ * optional mount attachment used by {@link ArmSim}.
+ */
public class ArmConfig {
+ /** Number of Kraken X60 motors driving the arm. */
@Getter @Setter private int numMotors = 1;
+ /** Initial X position of the arm pivot in the Mechanism2d canvas (metres). */
@Getter @Setter private double initialX = 0.7;
+ /** Initial Y position of the arm pivot in the Mechanism2d canvas (metres). */
@Getter @Setter private double initialY = 0.3;
+ /** Current X position of the arm pivot used during simulation updates (metres). */
@Getter @Setter private double pivotX = 0.7;
+ /** Current Y position of the arm pivot used during simulation updates (metres). */
@Getter @Setter private double pivotY = 0.3;
-
- @Getter @Setter
- private double ratio =
- 50; // the number of rotations it takes for the mechanism to do one revolution
-
+ /** Motor rotations required for one full revolution of the arm mechanism. */
+ @Getter @Setter private double ratio = 50;
+ /** Visual length of the arm ligament in the Mechanism2d canvas (metres). */
@Getter @Setter private double length = 0.5;
+ /** Moment of inertia used by the physics simulation (kg·m²). */
@Getter @Setter private double simMOI = 1.2;
+ /**
+ * Distance from the pivot to the arm's centre of gravity used by the physics simulation
+ * (metres).
+ */
@Getter @Setter private double simCGLength = 0.2;
+ /** Minimum allowable arm angle (radians). */
@Getter @Setter private double minAngle = Math.toRadians(-60);
+ /** Maximum allowable arm angle (radians). */
@Getter @Setter private double maxAngle = Math.toRadians(90);
+ /** Arm angle at the start of the simulation (radians). */
@Getter @Setter private double startingAngle = Math.toRadians(90);
+ /** Whether the physics simulation should apply gravitational force to the arm. */
@Getter @Setter private boolean simulateGravity = true;
+ /** Whether this arm is attached to a parent {@link Mount}. */
@Getter private boolean mounted = false;
+ /** The parent mount this arm is attached to, or {@code null} if not mounted. */
@Getter private Mount mount;
+ /** X position of the mount at simulation start (metres). */
@Getter private double initMountX;
+ /** Y position of the mount at simulation start (metres). */
@Getter private double initMountY;
+ /** Angle of the mount at simulation start (radians). */
@Getter private double initMountAngle;
+ /**
+ * When {@code true} the arm's visual angle is expressed in absolute robot-frame degrees; when
+ * {@code false} it is relative to the parent mount's current angle.
+ */
@Getter private boolean absAngle;
+ /** Color used to draw the arm ligament in the Mechanism2d canvas. */
@Getter private Color8Bit color = new Color8Bit(Color.kBlue);
+ /**
+ * Creates an ArmConfig with the required physical and display parameters. Angle arguments are
+ * specified in degrees and stored internally as radians.
+ *
+ * @param initialX initial X position of the pivot in the Mechanism2d canvas (metres)
+ * @param initialY initial Y position of the pivot in the Mechanism2d canvas (metres)
+ * @param ratio motor rotations per one full arm revolution
+ * @param length visual arm length in the Mechanism2d canvas (metres)
+ * @param minAngleDegrees minimum allowable arm angle in degrees
+ * @param maxAngleDegrees maximum allowable arm angle in degrees
+ * @param startingAngleDegrees initial arm angle in degrees
+ */
public ArmConfig(
double initialX,
double initialY,
@@ -51,11 +89,31 @@ public ArmConfig(
this.pivotY = initialY;
}
+ /**
+ * Sets the arm's display color in the Mechanism2d canvas.
+ *
+ * @param color the color to use
+ * @return this config for chaining
+ */
public ArmConfig setColor(Color8Bit color) {
this.color = color;
return this;
}
+ public ArmConfig setSimulatedGravity(boolean simulateGravity) {
+ this.simulateGravity = simulateGravity;
+ return this;
+ }
+
+ /**
+ * Attaches this arm to a {@link LinearSim} mount so its pivot tracks the linear stage's
+ * position.
+ *
+ * @param sim the linear stage to mount onto, or {@code null} to leave unmounted
+ * @param fixedAngle when {@code true} the arm angle is treated as absolute; when {@code false}
+ * it is relative to the mount's current angle
+ * @return this config for chaining
+ */
public ArmConfig setMount(LinearSim sim, boolean fixedAngle) {
if (sim != null) {
mounted = true;
@@ -68,6 +126,15 @@ public ArmConfig setMount(LinearSim sim, boolean fixedAngle) {
return this;
}
+ /**
+ * Attaches this arm to a parent {@link ArmSim} mount so its pivot tracks the parent arm's tip
+ * position.
+ *
+ * @param sim the parent arm to mount onto, or {@code null} to leave unmounted
+ * @param absAngle when {@code true} the arm angle is expressed in the absolute robot frame;
+ * when {@code false} it is relative to the parent arm's current angle
+ * @return this config for chaining
+ */
public ArmConfig setMount(ArmSim sim, boolean absAngle) {
if (sim != null) {
mounted = true;
diff --git a/src/main/java/frc/spectrumLib/sim/ArmSim.java b/src/main/java/frc/spectrumLib/sim/ArmSim.java
index cd836a20..8b6b11b9 100644
--- a/src/main/java/frc/spectrumLib/sim/ArmSim.java
+++ b/src/main/java/frc/spectrumLib/sim/ArmSim.java
@@ -10,16 +10,32 @@
import edu.wpi.first.wpilibj.smartdashboard.MechanismRoot2d;
import lombok.Getter;
+/**
+ * WPILib-backed simulation of a single-jointed arm driven by one or more Kraken X60 motors. Updates
+ * the TalonFX sim state each robot period and animates the arm in a {@link Mechanism2d} canvas.
+ * Implements {@link Mount} so other mechanisms can be attached to this arm's tip, and implements
+ * {@link Mountable} so this arm can itself be attached to a parent mount.
+ */
public class ArmSim implements Mount, Mountable {
private SingleJointedArmSim armSim;
+ /** Configuration containing physical properties and display settings for this arm. */
@Getter private ArmConfig config;
private MechanismRoot2d armPivot;
private MechanismLigament2d armMech2d;
private TalonFXSimState armMotorSim;
+ /** Always {@link MountType#ARM}; used by child mechanisms to determine positioning logic. */
@Getter private final MountType mountType = MountType.ARM;
+ /**
+ * Creates and registers an arm simulation.
+ *
+ * @param config physical and display configuration for the arm
+ * @param mech the Mechanism2d canvas to draw the arm on
+ * @param armMotorSim the TalonFX sim state of the motor driving the arm
+ * @param name unique name prefix used for Mechanism2d element labels
+ */
public ArmSim(ArmConfig config, Mechanism2d mech, TalonFXSimState armMotorSim, String name) {
this.config = config;
this.armMotorSim = armMotorSim;
@@ -31,7 +47,7 @@ public ArmSim(ArmConfig config, Mechanism2d mech, TalonFXSimState armMotorSim, S
config.getSimCGLength(),
config.getMinAngle(),
config.getMaxAngle(),
- false, // Simulate gravity (change back to true)
+ config.isSimulateGravity(),
config.getStartingAngle());
armPivot = mech.getRoot(name + " Arm Pivot", config.getPivotX(), config.getPivotY());
@@ -45,6 +61,10 @@ public ArmSim(ArmConfig config, Mechanism2d mech, TalonFXSimState armMotorSim, S
config.getColor()));
}
+ /**
+ * Advances the arm physics simulation by one robot period, updates the TalonFX rotor position
+ * and velocity, and refreshes the Mechanism2d visualization.
+ */
public void simulationPeriodic() {
// armMotorSim.setSupplyVoltage(RobotController.getBatteryVoltage());
armSim.setInput(armMotorSim.getMotorVoltage());
@@ -82,18 +102,39 @@ public void simulationPeriodic() {
armPivot.setPosition(config.getPivotX(), config.getPivotY());
}
+ /**
+ * Returns the current arm angle from the WPILib physics simulation.
+ *
+ * @return arm angle in radians
+ */
public double getAngleRads() {
return armSim.getAngleRads();
}
+ /**
+ * Returns how far the pivot has moved horizontally from its initial position.
+ *
+ * @return horizontal displacement in metres
+ */
public double getDisplacementX() {
return config.getPivotX() - config.getInitialX();
}
+ /**
+ * Returns how far the pivot has moved vertically from its initial position.
+ *
+ * @return vertical displacement in metres
+ */
public double getDisplacementY() {
return config.getPivotY() - config.getInitialY();
}
+ /**
+ * Returns the effective arm angle accounting for the parent mount's angle when mounted and not
+ * using an absolute angle reference.
+ *
+ * @return effective arm angle in radians
+ */
public double getAngle() {
if (config.isMounted()) {
if (config.isAbsAngle()) {
@@ -105,10 +146,20 @@ public double getAngle() {
return getAngleRads();
}
+ /**
+ * Returns the X coordinate of the arm pivot, used by child mechanisms as their mount point.
+ *
+ * @return pivot X position in metres
+ */
public double getMountX() {
return config.getPivotX();
}
+ /**
+ * Returns the Y coordinate of the arm pivot, used by child mechanisms as their mount point.
+ *
+ * @return pivot Y position in metres
+ */
public double getMountY() {
return config.getPivotY();
}
diff --git a/src/main/java/frc/spectrumLib/sim/Circle.java b/src/main/java/frc/spectrumLib/sim/Circle.java
index cff55b53..5318a378 100644
--- a/src/main/java/frc/spectrumLib/sim/Circle.java
+++ b/src/main/java/frc/spectrumLib/sim/Circle.java
@@ -9,6 +9,10 @@
import lombok.Getter;
import lombok.Setter;
+/**
+ * Renders a filled circle in a {@link Mechanism2d} canvas using evenly-spaced radial ligaments.
+ * Used by {@link RollerSim} to visualise the roller's spin state and color.
+ */
public class Circle {
private MechanismRoot2d rollerAxle;
@@ -16,13 +20,27 @@ public class Circle {
@SuppressWarnings("unused")
private MechanismLigament2d rollerViz;
+ /** Array of radial ligaments that together form the circle outline. */
@Getter private MechanismLigament2d[] circleBackground;
+ /** Number of radial lines used to approximate the circle. */
@Getter private int backgroundLines;
+
private double diameterInches;
private MechanismRoot2d root;
+ /** Color applied to all background lines when the circle is drawn. */
@Setter private Color8Bit color = new Color8Bit(Color.kBlack);
+ /** Label prefix used when naming Mechanism2d elements. */
@Setter private String name;
+ /**
+ * Creates a Circle and immediately draws the radial background lines.
+ *
+ * @param backgroundLines number of evenly-spaced radial lines used to approximate the circle
+ * @param diameterInches diameter of the circle in inches
+ * @param name label prefix for Mechanism2d element names
+ * @param root the Mechanism2d root the circle is attached to
+ * @param mech the parent Mechanism2d canvas
+ */
public Circle(
int backgroundLines,
double diameterInches,
@@ -38,6 +56,16 @@ public Circle(
drawCircle();
}
+ /**
+ * Creates a Circle with a specified color and immediately draws the radial background lines.
+ *
+ * @param mech the parent Mechanism2d canvas
+ * @param backgroundLines number of evenly-spaced radial lines used to approximate the circle
+ * @param diameterInches diameter of the circle in inches
+ * @param name label prefix for Mechanism2d element names
+ * @param root the Mechanism2d root the circle is attached to
+ * @param color the initial color applied to every background line
+ */
public Circle(
Mechanism2d mech,
int backgroundLines,
@@ -49,6 +77,10 @@ public Circle(
this.color = color;
}
+ /**
+ * Creates and attaches all radial background ligaments that form the circle, distributing them
+ * evenly around 360 degrees.
+ */
public void drawCircle() {
for (int i = 0; i < backgroundLines; i++) {
circleBackground[i] =
@@ -62,6 +94,10 @@ public void drawCircle() {
}
}
+ /**
+ * Appends a short white indicator line to the roller axle so rotation direction is visible in
+ * the Mechanism2d canvas.
+ */
public void drawViz() {
rollerViz =
rollerAxle.append(
@@ -73,12 +109,24 @@ public void drawViz() {
new Color8Bit(Color.kWhite)));
}
+ /**
+ * Sets all background radial lines to the same color.
+ *
+ * @param color the color to apply to every background line
+ */
public void setBackgroundColor(Color8Bit color) {
for (int i = 0; i < backgroundLines; i++) {
circleBackground[i].setColor(color);
}
}
+ /**
+ * Alternates two colors across the background lines, giving the circle a two-tone appearance
+ * (useful for indicating reverse spin direction).
+ *
+ * @param color8Bit color applied to even-indexed background lines
+ * @param color8Bit2 color applied to odd-indexed background lines
+ */
public void setHalfBackground(Color8Bit color8Bit, Color8Bit color8Bit2) {
for (int i = 0; i < backgroundLines; i++) {
if (i % 2 == 0) {
diff --git a/src/main/java/frc/spectrumLib/sim/LinearConfig.java b/src/main/java/frc/spectrumLib/sim/LinearConfig.java
index e1018b08..9db74af2 100644
--- a/src/main/java/frc/spectrumLib/sim/LinearConfig.java
+++ b/src/main/java/frc/spectrumLib/sim/LinearConfig.java
@@ -6,32 +6,69 @@
import lombok.Getter;
import lombok.Setter;
+/**
+ * Configuration data for a linear (elevator-style) mechanism simulation. Stores physical
+ * properties, Mechanism2d display settings, and optional mount attachment used by {@link
+ * LinearSim}.
+ */
public class LinearConfig {
+ /** Number of Kraken X60 motors driving the linear stage. */
@Getter private int numMotors = 1;
+ /** Gear ratio between the motor and the elevator drum. */
@Getter private double elevatorGearing = 5;
+ /** Mass of the moving carriage in kilograms, used by the physics simulation. */
@Getter private double carriageMassKg = 1;
+ /** Radius of the elevator drum in metres, used to convert rotations to linear position. */
@Getter private double drumRadius = Units.inchesToMeters(0.955 / 2);
+ /** Minimum travel height of the mechanism in metres. */
@Getter private double minHeight = 0;
-
+ /** Maximum travel height of the mechanism in metres. */
@Getter
private double maxHeight = 10000; // Units.inchesToMeters(Robot.config.elevator.maxHeight);
// Display Config
+ /**
+ * Angle of the linear stage in the Mechanism2d canvas (degrees; 0 = horizontal, 90 = vertical,
+ * CCW positive).
+ */
@Getter private double angle = 90; // O is horizontal, 90 is vertical, CCW is positive
+ /** Color of the moving stage ligament in the Mechanism2d canvas. */
@Getter private Color8Bit color = new Color8Bit(Color.kPurple);
+ /** Stroke width of the stage ligaments in the Mechanism2d canvas. */
@Getter private double lineWidth = 10;
+ /** Initial X position of the static root in the Mechanism2d canvas (metres). */
@Getter private double initialX = 0.5;
+ /** Initial Y position of the static root in the Mechanism2d canvas (metres). */
@Getter private double initialY = 0;
+ /** Current X position of the static root, updated when mounted (metres). */
@Getter @Setter private double staticRootX = 0.5;
+ /** Current Y position of the static root, updated when mounted (metres). */
@Getter @Setter private double staticRootY = 0;
+ /**
+ * Visual length of the static (non-moving) stage ligament in the Mechanism2d canvas (metres).
+ */
@Getter private double staticLength = 20;
+ /** Visual length of the moving stage ligament in the Mechanism2d canvas (metres). */
@Getter private double movingLength = 20;
+ /** Whether this linear stage is attached to a parent {@link Mount}. */
@Getter private boolean mounted = false;
+ /** The parent mount this linear stage is attached to, or {@code null} if not mounted. */
@Getter private Mount mount;
+ /** X position of the mount at simulation start (metres). */
@Getter private double initMountX;
+ /** Y position of the mount at simulation start (metres). */
@Getter private double initMountY;
+ /** Angle of the mount at simulation start (radians). */
@Getter private double initMountAngle;
+ /**
+ * Creates a LinearConfig with the minimum required positioning and mechanical parameters.
+ *
+ * @param x initial X position of the stage root in the Mechanism2d canvas (metres)
+ * @param y initial Y position of the stage root in the Mechanism2d canvas (metres)
+ * @param gearing gear ratio between the motor and the elevator drum
+ * @param drumRadius radius of the elevator drum in metres
+ */
public LinearConfig(double x, double y, double gearing, double drumRadius) {
this.initialX = x;
this.initialY = y;
@@ -41,47 +78,102 @@ public LinearConfig(double x, double y, double gearing, double drumRadius) {
this.drumRadius = drumRadius;
}
+ /**
+ * Sets the number of motors driving this linear stage.
+ *
+ * @param numMotors number of Kraken X60 motors
+ * @return this config for chaining
+ */
public LinearConfig setNumMotors(int numMotors) {
this.numMotors = numMotors;
return this;
}
+ /**
+ * Sets the carriage mass used by the physics simulation.
+ *
+ * @param carriageMassKg mass of the moving carriage in kilograms
+ * @return this config for chaining
+ */
public LinearConfig setCarriageMass(double carriageMassKg) {
this.carriageMassKg = carriageMassKg;
return this;
}
+ /**
+ * Sets the orientation angle of the linear stage in the Mechanism2d canvas.
+ *
+ * @param angle angle in degrees (0 = horizontal, 90 = vertical, CCW positive)
+ * @return this config for chaining
+ */
public LinearConfig setAngle(double angle) {
this.angle = angle;
return this;
}
+ /**
+ * Sets the color of the moving stage ligament in the Mechanism2d canvas.
+ *
+ * @param color the display color
+ * @return this config for chaining
+ */
public LinearConfig setColor(Color8Bit color) {
this.color = color;
return this;
}
+ /**
+ * Sets the stroke width of the stage ligaments in the Mechanism2d canvas.
+ *
+ * @param lineWidth stroke width in pixels
+ * @return this config for chaining
+ */
public LinearConfig setLineWidth(double lineWidth) {
this.lineWidth = lineWidth;
return this;
}
+ /**
+ * Sets the visual length of the static (non-moving) stage ligament.
+ *
+ * @param lengthInches length in inches; stored internally as metres
+ * @return this config for chaining
+ */
public LinearConfig setStaticLength(double lengthInches) {
this.staticLength = Units.inchesToMeters(lengthInches);
;
return this;
}
+ /**
+ * Sets the visual length of the moving stage ligament.
+ *
+ * @param lengthInches length in inches; stored internally as metres
+ * @return this config for chaining
+ */
public LinearConfig setMovingLength(double lengthInches) {
this.movingLength = Units.inchesToMeters(lengthInches);
return this;
}
+ /**
+ * Sets the maximum travel height of the mechanism.
+ *
+ * @param lengthInches maximum height in inches; stored internally as metres
+ * @return this config for chaining
+ */
public LinearConfig setMaxHeight(double lengthInches) {
this.maxHeight = Units.inchesToMeters(lengthInches);
return this;
}
+ /**
+ * Attaches this linear stage to a parent {@link LinearSim} mount so its root tracks the parent
+ * stage's position.
+ *
+ * @param sim the parent linear stage to mount onto, or {@code null} to leave unmounted
+ * @return this config for chaining
+ */
public LinearConfig setMount(LinearSim sim) {
if (sim != null) {
mounted = true;
@@ -94,6 +186,13 @@ public LinearConfig setMount(LinearSim sim) {
return this;
}
+ /**
+ * Attaches this linear stage to a parent {@link ArmSim} mount so its root tracks the arm tip's
+ * position.
+ *
+ * @param sim the parent arm to mount onto, or {@code null} to leave unmounted
+ * @return this config for chaining
+ */
public LinearConfig setMount(ArmSim sim) {
if (sim != null) {
mounted = true;
diff --git a/src/main/java/frc/spectrumLib/sim/LinearSim.java b/src/main/java/frc/spectrumLib/sim/LinearSim.java
index 3b3e7401..24d4176b 100644
--- a/src/main/java/frc/spectrumLib/sim/LinearSim.java
+++ b/src/main/java/frc/spectrumLib/sim/LinearSim.java
@@ -11,6 +11,13 @@
import edu.wpi.first.wpilibj.util.Color8Bit;
import lombok.Getter;
+/**
+ * WPILib-backed simulation of a linear (elevator-style) mechanism driven by one or more Kraken X60
+ * motors. Updates the TalonFX sim state each robot period and animates both a static backing
+ * ligament and a moving stage ligament in a {@link Mechanism2d} canvas. Implements {@link Mount} so
+ * other mechanisms can be attached to the moving stage, and implements {@link Mountable} so this
+ * stage can itself be attached to a parent mount.
+ */
public class LinearSim implements Mount, Mountable {
private ElevatorSim elevatorSim;
@@ -18,11 +25,22 @@ public class LinearSim implements Mount, Mountable {
private final MechanismRoot2d root;
private final MechanismLigament2d staticMech2d;
private final MechanismLigament2d m_elevatorMech2d;
+ /** Configuration containing physical properties and display settings for this linear stage. */
@Getter private LinearConfig config;
+
private TalonFXSimState linearMotorSim;
+ /** Always {@link MountType#LINEAR}; used by child mechanisms to determine positioning logic. */
@Getter private final MountType mountType = MountType.LINEAR;
+ /**
+ * Creates and registers a linear mechanism simulation.
+ *
+ * @param config physical and display configuration for the linear stage
+ * @param mech the Mechanism2d canvas to draw the stage on
+ * @param linearMotorSim the TalonFX sim state of the motor driving the stage
+ * @param name unique name prefix used for Mechanism2d element labels
+ */
public LinearSim(
LinearConfig config, Mechanism2d mech, TalonFXSimState linearMotorSim, String name) {
this.config = config;
@@ -61,6 +79,12 @@ public LinearSim(
new Color8Bit(Color.kBlack)));
}
+ /**
+ * Returns the moving stage {@link MechanismLigament2d}, useful for attaching additional visual
+ * elements.
+ *
+ * @return the elevator moving-stage ligament
+ */
public MechanismLigament2d getElevatorMech2d() {
return m_elevatorMech2d;
}
@@ -75,6 +99,10 @@ private double getRotations() {
* config.getElevatorGearing();
}
+ /**
+ * Advances the elevator physics simulation by one robot period, updates the TalonFX rotor
+ * position and velocity, and refreshes both the static and moving Mechanism2d ligaments.
+ */
public void simulationPeriodic() {
elevatorSim.setInput(linearMotorSim.getMotorVoltage());
elevatorSim.update(TimedRobot.kDefaultPeriod);
@@ -118,6 +146,12 @@ public void simulationPeriodic() {
}
}
+ /**
+ * Returns the horizontal component of the stage's current displacement from its initial
+ * position, accounting for the parent mount's angle when mounted.
+ *
+ * @return horizontal displacement in metres
+ */
public double getDisplacementX() {
double angle;
@@ -138,6 +172,12 @@ public double getDisplacementX() {
+ (config.getStaticRootX() - config.getInitialX());
}
+ /**
+ * Returns the vertical component of the stage's current displacement from its initial position,
+ * accounting for the parent mount's angle when mounted.
+ *
+ * @return vertical displacement in metres
+ */
public double getDisplacementY() {
double angle;
@@ -158,6 +198,12 @@ public double getDisplacementY() {
+ (config.getStaticRootY() - config.getInitialY());
}
+ /**
+ * Returns the effective absolute angle of the linear stage in radians, adding the parent
+ * mount's angle when mounted.
+ *
+ * @return effective stage angle in radians
+ */
public double getAngle() {
if (config.isMounted()) {
return config.getMount().getAngle() + Math.toRadians(config.getAngle());
@@ -166,10 +212,22 @@ public double getAngle() {
}
}
+ /**
+ * Returns the X coordinate of the static root of this stage, used by child mechanisms as their
+ * mount point.
+ *
+ * @return static root X position in metres
+ */
public double getMountX() {
return config.getStaticRootX();
}
+ /**
+ * Returns the Y coordinate of the static root of this stage, used by child mechanisms as their
+ * mount point.
+ *
+ * @return static root Y position in metres
+ */
public double getMountY() {
return config.getStaticRootY();
}
diff --git a/src/main/java/frc/spectrumLib/sim/Mount.java b/src/main/java/frc/spectrumLib/sim/Mount.java
index f3583838..813f9901 100644
--- a/src/main/java/frc/spectrumLib/sim/Mount.java
+++ b/src/main/java/frc/spectrumLib/sim/Mount.java
@@ -1,21 +1,62 @@
package frc.spectrumLib.sim;
+/**
+ * Represents a simulation component that other mechanisms can be attached to. Implementations
+ * expose their current position and angle so child mechanisms can update their own position each
+ * simulation period.
+ */
public interface Mount {
+ /**
+ * Discriminates between the two supported mount types so child mechanisms can apply the correct
+ * positioning logic.
+ */
public enum MountType {
+ /** A linear (elevator-style) stage mount. */
LINEAR,
+ /** A single-jointed arm mount. */
ARM,
}
+ /**
+ * Returns the type of this mount, used by child mechanisms to select positioning logic.
+ *
+ * @return this mount's {@link MountType}
+ */
MountType getMountType();
+ /**
+ * Returns the horizontal displacement of this mount from its initial position (metres).
+ *
+ * @return horizontal displacement in metres
+ */
double getDisplacementX();
+ /**
+ * Returns the vertical displacement of this mount from its initial position (metres).
+ *
+ * @return vertical displacement in metres
+ */
double getDisplacementY();
+ /**
+ * Returns the current absolute angle of this mount in radians.
+ *
+ * @return angle in radians
+ */
double getAngle();
+ /**
+ * Returns the X coordinate that child mechanisms should use as their attachment point (metres).
+ *
+ * @return mount X position in metres
+ */
double getMountX();
+ /**
+ * Returns the Y coordinate that child mechanisms should use as their attachment point (metres).
+ *
+ * @return mount Y position in metres
+ */
double getMountY();
}
diff --git a/src/main/java/frc/spectrumLib/sim/Mountable.java b/src/main/java/frc/spectrumLib/sim/Mountable.java
index 9ca5fabd..ed0e050e 100644
--- a/src/main/java/frc/spectrumLib/sim/Mountable.java
+++ b/src/main/java/frc/spectrumLib/sim/Mountable.java
@@ -2,8 +2,31 @@
import frc.spectrumLib.sim.Mount.MountType;
+/**
+ * Mixin interface for simulation components that can be attached to a {@link Mount}. Provides
+ * default geometry helpers that compute the component's updated canvas position each simulation
+ * period, taking the parent mount's type, current position, and current angle into account.
+ */
public interface Mountable {
+ /**
+ * Computes the updated X position of a mounted component given full explicit geometry
+ * parameters.
+ *
+ * @param mountType type of the parent mount (ARM or LINEAR)
+ * @param initialX component's initial X position on the canvas (metres)
+ * @param initialY component's initial Y position on the canvas (metres)
+ * @param initMountX mount's X position at simulation start (metres)
+ * @param initMountY mount's Y position at simulation start (metres)
+ * @param initMountAngle mount's angle at simulation start (radians)
+ * @param mountX mount's current X position (metres)
+ * @param mountY mount's current Y position (metres)
+ * @param displacementX mount's current horizontal displacement from its initial position
+ * (metres)
+ * @param displacementY mount's current vertical displacement from its initial position (metres)
+ * @param mountAngle mount's current angle (radians)
+ * @return updated X position of the component on the canvas (metres)
+ */
default double getUpdatedX(
MountType mountType,
double initialX,
@@ -36,6 +59,24 @@ default double getUpdatedX(
}
}
+ /**
+ * Computes the updated Y position of a mounted component given full explicit geometry
+ * parameters.
+ *
+ * @param mountType type of the parent mount (ARM or LINEAR)
+ * @param initialX component's initial X position on the canvas (metres)
+ * @param initialY component's initial Y position on the canvas (metres)
+ * @param initMountX mount's X position at simulation start (metres)
+ * @param initMountY mount's Y position at simulation start (metres)
+ * @param initMountAngle mount's angle at simulation start (radians)
+ * @param mountX mount's current X position (metres)
+ * @param mountY mount's current Y position (metres)
+ * @param displacementX mount's current horizontal displacement from its initial position
+ * (metres)
+ * @param displacementY mount's current vertical displacement from its initial position (metres)
+ * @param mountAngle mount's current angle (radians)
+ * @return updated Y position of the component on the canvas (metres)
+ */
default double getUpdatedY(
MountType mountType,
double initialX,
@@ -68,6 +109,12 @@ default double getUpdatedY(
}
}
+ /**
+ * Convenience overload that derives all geometry parameters from a {@link RollerConfig}.
+ *
+ * @param config the roller configuration carrying mount and initial-position data
+ * @return updated X position on the canvas (metres)
+ */
default double getUpdatedX(RollerConfig config) {
Mount mount = config.getMount();
return getUpdatedX(
@@ -84,6 +131,12 @@ default double getUpdatedX(RollerConfig config) {
mount.getAngle());
}
+ /**
+ * Convenience overload that derives all geometry parameters from an {@link ArmConfig}.
+ *
+ * @param config the arm configuration carrying mount and initial-position data
+ * @return updated X position on the canvas (metres)
+ */
default double getUpdatedX(ArmConfig config) {
Mount mount = config.getMount();
return getUpdatedX(
@@ -100,6 +153,12 @@ default double getUpdatedX(ArmConfig config) {
mount.getAngle());
}
+ /**
+ * Convenience overload that derives all geometry parameters from a {@link LinearConfig}.
+ *
+ * @param config the linear stage configuration carrying mount and initial-position data
+ * @return updated X position on the canvas (metres)
+ */
default double getUpdatedX(LinearConfig config) {
Mount mount = config.getMount();
return getUpdatedX(
@@ -116,6 +175,12 @@ default double getUpdatedX(LinearConfig config) {
mount.getAngle());
}
+ /**
+ * Convenience overload that derives all geometry parameters from a {@link RollerConfig}.
+ *
+ * @param config the roller configuration carrying mount and initial-position data
+ * @return updated Y position on the canvas (metres)
+ */
default double getUpdatedY(RollerConfig config) {
Mount mount = config.getMount();
return getUpdatedY(
@@ -132,6 +197,12 @@ default double getUpdatedY(RollerConfig config) {
mount.getAngle());
}
+ /**
+ * Convenience overload that derives all geometry parameters from an {@link ArmConfig}.
+ *
+ * @param config the arm configuration carrying mount and initial-position data
+ * @return updated Y position on the canvas (metres)
+ */
default double getUpdatedY(ArmConfig config) {
Mount mount = config.getMount();
return getUpdatedY(
@@ -148,6 +219,12 @@ default double getUpdatedY(ArmConfig config) {
mount.getAngle());
}
+ /**
+ * Convenience overload that derives all geometry parameters from a {@link LinearConfig}.
+ *
+ * @param config the linear stage configuration carrying mount and initial-position data
+ * @return updated Y position on the canvas (metres)
+ */
default double getUpdatedY(LinearConfig config) {
Mount mount = config.getMount();
return getUpdatedY(
@@ -180,14 +257,41 @@ static double getAngleOffset(
}
}
+ /**
+ * Computes the straight-line distance between two points on the canvas.
+ *
+ * @param x1 X coordinate of the first point (metres)
+ * @param y1 Y coordinate of the first point (metres)
+ * @param x2 X coordinate of the second point (metres)
+ * @param y2 Y coordinate of the second point (metres)
+ * @return Euclidean distance in metres
+ */
static double getDistance(double x1, double y1, double x2, double y2) {
return Math.sqrt(Math.pow(x1 - x2, 2) + Math.pow(y1 - y2, 2));
}
+ /**
+ * Computes the X coordinate of a point at {@code radius} distance from {@code displacementX} in
+ * the direction of {@code angle}.
+ *
+ * @param radius distance from the reference point (metres)
+ * @param angle direction angle in radians
+ * @param displacementX reference X coordinate (metres)
+ * @return resulting X coordinate (metres)
+ */
static double getXWithAngle(double radius, double angle, double displacementX) {
return radius * Math.cos(angle) + displacementX;
}
+ /**
+ * Computes the Y coordinate of a point at {@code radius} distance from {@code displacementY} in
+ * the direction of {@code angle}.
+ *
+ * @param radius distance from the reference point (metres)
+ * @param angle direction angle in radians
+ * @param displacementY reference Y coordinate (metres)
+ * @return resulting Y coordinate (metres)
+ */
static double getYWithAngle(double radius, double angle, double displacementY) {
return radius * Math.sin(angle) + displacementY;
}
diff --git a/src/main/java/frc/spectrumLib/sim/RollerConfig.java b/src/main/java/frc/spectrumLib/sim/RollerConfig.java
index 170fe895..2247069e 100644
--- a/src/main/java/frc/spectrumLib/sim/RollerConfig.java
+++ b/src/main/java/frc/spectrumLib/sim/RollerConfig.java
@@ -4,42 +4,91 @@
import edu.wpi.first.wpilibj.util.Color8Bit;
import lombok.Getter;
+/**
+ * Configuration data for a roller mechanism simulation. Stores physical properties, display colors,
+ * canvas position, and optional mount attachment used by {@link RollerSim}.
+ */
public class RollerConfig {
+ /** Outer diameter of the roller in inches, used for physics and visual scaling. */
@Getter private double rollerDiameterInches = 2;
+ /** Number of radial lines used to draw the roller circle in the Mechanism2d canvas. */
@Getter private int backgroundLines = 36;
+ /** Gear ratio between the motor and the roller output shaft. */
@Getter private double gearRatio = 5;
+ /** Moment of inertia of the roller used by the flywheel physics simulation (kg·m²). */
@Getter private double simMOI = 0.01;
+ /** Color displayed when the roller is stationary or below the velocity threshold. */
@Getter private Color8Bit offColor = new Color8Bit(Color.kBlack);
+ /** Color displayed when the roller is spinning in the forward direction. */
@Getter private Color8Bit fwdColor = new Color8Bit(Color.kGreen);
+ /** Color displayed when the roller is spinning in the reverse direction. */
@Getter private Color8Bit revColor = new Color8Bit(Color.kRed);
+ /** Initial X position of the roller axle in the Mechanism2d canvas (metres). */
@Getter private double initialX = 0;
+ /** Initial Y position of the roller axle in the Mechanism2d canvas (metres). */
@Getter private double initialY = 0;
+ /** Whether this roller is attached to a parent {@link Mount}. */
@Getter private boolean mounted = false;
+ /** The parent mount this roller is attached to, or {@code null} if not mounted. */
@Getter private Mount mount;
+ /** X position of the mount at simulation start (metres). */
@Getter private double initMountX;
+ /** Y position of the mount at simulation start (metres). */
@Getter private double initMountY;
+ /** Angle of the mount at simulation start (radians). */
@Getter private double initMountAngle;
+ /**
+ * Creates a RollerConfig for a roller with the given diameter.
+ *
+ * @param diameterInches outer diameter of the roller in inches
+ */
public RollerConfig(double diameterInches) {
rollerDiameterInches = diameterInches;
}
+ /**
+ * Sets the gear ratio between the motor and the roller output shaft.
+ *
+ * @param ratio gear ratio (motor rotations per roller rotation)
+ * @return this config for chaining
+ */
public RollerConfig setGearRatio(double ratio) {
gearRatio = ratio;
return this;
}
+ /**
+ * Sets the moment of inertia used by the flywheel physics simulation.
+ *
+ * @param moi moment of inertia in kg·m²
+ * @return this config for chaining
+ */
public RollerConfig setSimMOI(double moi) {
simMOI = moi;
return this;
}
+ /**
+ * Sets the initial position of the roller axle in the Mechanism2d canvas.
+ *
+ * @param x initial X position in metres
+ * @param y initial Y position in metres
+ * @return this config for chaining
+ */
public RollerConfig setPosition(double x, double y) {
initialX = x;
initialY = y;
return this;
}
+ /**
+ * Attaches this roller to a {@link LinearSim} mount so its axle tracks the linear stage's
+ * position.
+ *
+ * @param sim the linear stage to mount onto, or {@code null} to leave unmounted
+ * @return this config for chaining
+ */
public RollerConfig setMount(LinearSim sim) {
if (sim != null) {
mounted = true;
@@ -52,6 +101,12 @@ public RollerConfig setMount(LinearSim sim) {
return this;
}
+ /**
+ * Attaches this roller to an {@link ArmSim} mount so its axle tracks the arm tip's position.
+ *
+ * @param sim the parent arm to mount onto, or {@code null} to leave unmounted
+ * @return this config for chaining
+ */
public RollerConfig setMount(ArmSim sim) {
if (sim != null) {
mounted = true;
diff --git a/src/main/java/frc/spectrumLib/sim/RollerSim.java b/src/main/java/frc/spectrumLib/sim/RollerSim.java
index 50808d06..96ef9757 100644
--- a/src/main/java/frc/spectrumLib/sim/RollerSim.java
+++ b/src/main/java/frc/spectrumLib/sim/RollerSim.java
@@ -14,6 +14,12 @@
import edu.wpi.first.wpilibj.util.Color;
import edu.wpi.first.wpilibj.util.Color8Bit;
+/**
+ * WPILib-backed simulation of a roller (flywheel) mechanism driven by a single Kraken X60 motor.
+ * Updates the TalonFX sim state each robot period and animates the roller — including spin-color
+ * feedback — in a {@link Mechanism2d} canvas. Implements {@link Mountable} so the roller axle can
+ * follow a parent {@link Mount}.
+ */
public class RollerSim implements Mountable {
private MechanismRoot2d rollerAxle;
@@ -24,6 +30,14 @@ public class RollerSim implements Mountable {
private RollerConfig config;
private Circle roller;
+ /**
+ * Creates and registers a roller simulation.
+ *
+ * @param config physical and display configuration for the roller
+ * @param mech the Mechanism2d canvas to draw the roller on
+ * @param rollerMotorSim the TalonFX sim state of the motor driving the roller
+ * @param name unique name prefix used for Mechanism2d element labels
+ */
public RollerSim(
RollerConfig config, Mechanism2d mech, TalonFXSimState rollerMotorSim, String name) {
this.config = config;
@@ -54,6 +68,11 @@ public RollerSim(
mech);
}
+ /**
+ * Advances the flywheel physics simulation by one robot period, updates the TalonFX rotor
+ * velocity and position, moves the axle to its current mount position, and updates the
+ * Mechanism2d color to reflect the roller's spin direction.
+ */
public void simulationPeriodic() { // double x, double y) {
// ------ Update sim based on motor output
rollerSim.setInput(rollerMotorSim.getMotorVoltage());
@@ -64,9 +83,11 @@ public void simulationPeriodic() { // double x, double y) {
// Subtracting out the starting angle is necessary so the simulation can't "cheat" and use
// the
// sim as an absolute encoder.
- double rotationsPerSecond = rollerSim.getAngularVelocityRadPerSec() / (2.0 * Math.PI);
- rollerMotorSim.setRotorVelocity(rotationsPerSecond);
- rollerMotorSim.addRotorPosition(rotationsPerSecond * TimedRobot.kDefaultPeriod);
+ // FlywheelSim reports mechanism-side velocity; the rotor spins gearRatio times faster.
+ double rotorRotationsPerSecond =
+ rollerSim.getAngularVelocityRadPerSec() / (2.0 * Math.PI) * config.getGearRatio();
+ rollerMotorSim.setRotorVelocity(rotorRotationsPerSecond);
+ rollerMotorSim.addRotorPosition(rotorRotationsPerSecond * TimedRobot.kDefaultPeriod);
// Update the axle as the robot moves
if (config.isMounted()) {
diff --git a/src/main/java/frc/spectrumLib/swerve/MapleSimSwerveDrivetrain.java b/src/main/java/frc/spectrumLib/swerve/MapleSimSwerveDrivetrain.java
index f82df409..ade436b6 100644
--- a/src/main/java/frc/spectrumLib/swerve/MapleSimSwerveDrivetrain.java
+++ b/src/main/java/frc/spectrumLib/swerve/MapleSimSwerveDrivetrain.java
@@ -47,8 +47,13 @@
* It replaces the {@link com.ctre.phoenix6.swerve.SimSwerveDrivetrain} class.
*/
public class MapleSimSwerveDrivetrain {
+ /** Simulation state of the Pigeon2 gyro, used to inject computed yaw and angular velocity. */
private final Pigeon2SimState pigeonSim;
+
+ /** Simulated representations of each swerve module, indexed FL/FR/BL/BR. */
private final SimSwerveModule[] simModules;
+
+ /** The underlying Maple-Sim drive simulation providing physics and odometry. */
public final SwerveDriveSimulation mapleSimDrive;
/**
@@ -148,11 +153,22 @@ public void update() {
* Construct one instance per robot, passing the {@link Swerve} subsystem. Then bind {@link
+ * #sysIdQuasistatic} and {@link #sysIdDynamic} to test-mode triggers. Change {@link
+ * #RoutineToApply} (by editing the source) to select which routine runs.
+ */
public class SysID {
// private Swerve swerve;
+
+ /** SysId routine that characterizes linear translation drive gains. */
@Getter private final SysIdRoutine SysIdRoutineTranslation;
+
+ /** SysId routine that characterizes rotational drive gains. */
@Getter private final SysIdRoutine SysIdRoutineRotation;
+
+ /** SysId routine that characterizes steer-module gains. */
@Getter private final SysIdRoutine SysIdRoutineSteer;
+
+ /** The routine currently selected for quasistatic and dynamic test commands. */
private final SysIdRoutine RoutineToApply;
+ /** CTRE swerve request used during the translation characterization routine. */
private final SwerveRequest.SysIdSwerveTranslation TranslationCharacterization =
new SwerveRequest.SysIdSwerveTranslation();
+
+ /** CTRE swerve request used during the rotation characterization routine. */
private final SwerveRequest.SysIdSwerveRotation RotationCharacterization =
new SwerveRequest.SysIdSwerveRotation();
+
+ /** CTRE swerve request used during the steer-gains characterization routine. */
private final SwerveRequest.SysIdSwerveSteerGains SteerCharacterization =
new SwerveRequest.SysIdSwerveSteerGains();
+ /**
+ * Constructs all three SysId routines and selects {@link #SysIdRoutineTranslation} as the
+ * active routine. Change {@link #RoutineToApply} at the bottom of this constructor to switch
+ * which routine is exercised by {@link #sysIdQuasistatic} and {@link #sysIdDynamic}.
+ *
+ * @param swerve the {@link Swerve} subsystem that will be commanded during characterization
+ */
public SysID(Swerve swerve) {
// this.swerve = swerve;
@@ -79,10 +107,24 @@ public SysID(Swerve swerve) {
* Both the sysid commands are specific to one particular sysid routine, change
* which one you're trying to characterize
*/
+
+ /**
+ * Returns a quasistatic (slow ramp) characterization command for the active routine.
+ *
+ * @param direction the direction ({@link SysIdRoutine.Direction#kForward} or {@link
+ * SysIdRoutine.Direction#kReverse}) in which to ramp the output
+ * @return a command that slowly ramps the output and logs data for system identification
+ */
public Command sysIdQuasistatic(SysIdRoutine.Direction direction) {
return RoutineToApply.quasistatic(direction);
}
+ /**
+ * Returns a dynamic (step) characterization command for the active routine.
+ *
+ * @param direction the direction in which to apply the voltage step
+ * @return a command that applies a step voltage and logs data for system identification
+ */
public Command sysIdDynamic(SysIdRoutine.Direction direction) {
return RoutineToApply.dynamic(direction);
}
diff --git a/src/main/java/frc/spectrumLib/BatteryLogger.java b/src/main/java/frc/spectrumLib/telemetry/BatteryLogger.java
similarity index 66%
rename from src/main/java/frc/spectrumLib/BatteryLogger.java
rename to src/main/java/frc/spectrumLib/telemetry/BatteryLogger.java
index ccfb7576..ce3f52f7 100644
--- a/src/main/java/frc/spectrumLib/BatteryLogger.java
+++ b/src/main/java/frc/spectrumLib/telemetry/BatteryLogger.java
@@ -5,28 +5,48 @@
// license that can be found in the LICENSE file at
// the root directory of this project.
-package frc.spectrumLib;
+package frc.spectrumLib.telemetry;
import java.util.HashMap;
import java.util.Map;
import lombok.Getter;
import lombok.Setter;
-/** Class for logging current, power, and energy usage. */
+/**
+ * Tracks and logs current draw, power, and cumulative energy consumption across subsystems each
+ * robot loop cycle.
+ */
public class BatteryLogger {
+ /** Duration of one robot loop in seconds, used to convert power (W) to energy (J). */
private final double loopPeriodSecs = 0.02;
+ /** When {@code false} all methods are no-ops, allowing the logger to be disabled at runtime. */
@Setter private boolean enabled = false;
+ /**
+ * Running total of current draw accumulated since the last {@link #logPower()} call, in amps.
+ */
@Getter private double totalCurrent = 0.0;
+ /** Running total of power accumulated since the last {@link #logPower()} call, in watts. */
@Getter private double totalPower = 0.0;
+ /** Cumulative energy consumed over the entire enabled session, in joules. */
@Getter private double totalEnergy = 0.0;
+ /** Battery terminal voltage used to convert current to power, in volts. */
@Setter private double batteryVoltage = 12.6;
+ /** Estimated current drawn by the RoboRIO itself, in amps. */
@Setter private double rioCurrent = 0.0;
private Map All methods are safe to call when the camera is not attached ({@link #isAttached()} returns
+ * {@code false}); they return zero / false / empty values in that case.
+ *
+ * Two pose estimation flavors are supported:
+ *
+ * The Lombok {@code @Accessors(chain = true)} annotation allows fluent setter calls: {@code
+ * config.setName("limelight").setAttached(true)}.
+ */
@Accessors(chain = true)
public static class LimelightConfig {
/** Must match to the name given in LL dashboard */
@Getter @Setter private String name;
+ /** Whether this camera is physically connected to the robot. */
@Getter @Setter private boolean attached = true;
+ /**
+ * Whether pose measurements from this camera are currently being fused into the estimator.
+ */
@Getter @Setter private boolean isIntegrating;
+
/** Physical Config */
+ /**
+ * Forward offset of the camera from the robot center in meters (positive = toward front).
+ */
@Getter private double forward, right, up; // meters
+ /** Orientation of the camera in degrees (roll/pitch/yaw relative to robot frame). */
@Getter private double roll, pitch, yaw; // degrees
+ /**
+ * Creates a configuration for the named Limelight camera.
+ *
+ * @param name the network-table name assigned to the camera in the LL dashboard
+ */
public LimelightConfig(String name) {
this.name = name;
}
@@ -66,33 +102,76 @@ public LimelightConfig withRotation(double roll, double pitch, double yaw) {
}
/* Debug */
+ /** Formatter used for printing pose coordinates to SmartDashboard. */
private final DecimalFormat df = new DecimalFormat();
+
+ /** Active configuration for this Limelight instance. */
private LimelightConfig config;
+
+ /** Whether pose measurements from this camera are currently being integrated. */
@Getter @Setter private boolean isIntegrating = false;
+
+ /** Network-table name of this camera (mirrors {@link LimelightConfig#getName()}). */
@Getter private String cameraName = "default";
+
+ /** Human-readable string describing the current integration status, logged for diagnostics. */
@Getter @Setter private String logStatus = "";
+
+ /** Human-readable string describing the currently visible tag(s), logged for diagnostics. */
@Getter @Setter private String tagStatus = "";
+ /**
+ * Constructs a Limelight wrapper from a fully populated {@link LimelightConfig}.
+ *
+ * @param config the camera configuration
+ */
public Limelight(LimelightConfig config) {
this.config = config;
+ cameraName = config.getName();
}
+ /**
+ * Constructs a Limelight wrapper with a default configuration for the given camera name.
+ *
+ * @param name the network-table name of the camera
+ */
public Limelight(String name) {
cameraName = name;
config = new LimelightConfig(name);
}
+ /**
+ * Constructs a Limelight wrapper, explicitly setting whether the camera is attached.
+ *
+ * @param name the network-table name of the camera
+ * @param attached {@code true} if the camera is physically present on the robot
+ */
public Limelight(String name, boolean attached) {
cameraName = name;
config = new LimelightConfig(name).setAttached(attached);
}
+ /**
+ * Constructs a Limelight wrapper and immediately sets its active pipeline.
+ *
+ * @param name the network-table name of the camera
+ * @param pipeline the pipeline index to activate (see {@link
+ * frc.robot.subsystems.vision.Vision.VisionConfig})
+ */
public Limelight(String name, int pipeline) {
this(name);
cameraName = name;
setLimelightPipeline(pipeline);
}
+ /**
+ * Constructs a Limelight wrapper with an explicit configuration and immediately sets its active
+ * pipeline.
+ *
+ * @param name the network-table name of the camera
+ * @param pipeline the pipeline index to activate
+ * @param config the fully populated {@link LimelightConfig} to use
+ */
public Limelight(String name, int pipeline, LimelightConfig config) {
this(name);
cameraName = name;
@@ -100,10 +179,20 @@ public Limelight(String name, int pipeline, LimelightConfig config) {
setLimelightPipeline(pipeline);
}
+ /**
+ * Returns the network-table name of this camera.
+ *
+ * @return the camera name as configured in the LL dashboard
+ */
public String getName() {
return config.getName();
}
+ /**
+ * Returns whether this camera is physically connected to the robot.
+ *
+ * @return {@code true} if attached
+ */
public boolean isAttached() {
return config.isAttached();
}
@@ -159,11 +248,21 @@ public boolean multipleTagsInView() {
return getTagCountInView() > 1;
}
+ /**
+ * Returns the number of AprilTags included in the current MegaTag1 pose estimate.
+ *
+ * @return number of tags contributing to the current pose estimate, or {@code 0} if not
+ * attached or no estimate is available
+ */
public double getTagCountInView() {
if (!isAttached()) {
return 0;
}
- return LimelightHelpers.getBotPoseEstimate_wpiBlue(config.getName()).tagCount;
+ PoseEstimate est = LimelightHelpers.getBotPoseEstimate_wpiBlue(config.getName());
+ if (est == null) {
+ return 0;
+ }
+ return est.tagCount;
// if (retrieveJSON() == null) return 0;
@@ -183,6 +282,11 @@ public double getClosestTagID() {
return LimelightHelpers.getFiducialID(config.getName());
}
+ /**
+ * Returns the area of the primary target as a percentage of the camera image (0–100).
+ *
+ * @return target area percentage, or {@code 0} if not attached
+ */
public double getTargetSize() {
if (!isAttached()) {
return 0;
@@ -225,6 +329,12 @@ public Pose2d getMegaTag2_Pose2d() {
return poseEstimate.pose;
}
+ /**
+ * Returns the full MegaTag1 {@link PoseEstimate}, including timestamp, tag count, and raw
+ * fiducials. Returns an empty estimate when not attached or when no estimate is available.
+ *
+ * @return the MegaTag1 pose estimate in the WPILib Blue origin frame
+ */
public PoseEstimate getMegaTag1_PoseEstimate() {
if (!isAttached()) {
return new PoseEstimate();
@@ -237,6 +347,12 @@ public PoseEstimate getMegaTag1_PoseEstimate() {
return poseEstimate;
}
+ /**
+ * Returns the full MegaTag2 {@link PoseEstimate} (heading-fused), including timestamp and tag
+ * count. Returns an empty estimate when not attached or when no estimate is available.
+ *
+ * @return the MegaTag2 pose estimate in the WPILib Blue origin frame
+ */
public PoseEstimate getMegaTag2_PoseEstimate() {
if (!isAttached()) {
return new PoseEstimate();
@@ -250,6 +366,12 @@ public PoseEstimate getMegaTag2_PoseEstimate() {
return poseEstimate;
}
+ /**
+ * Returns {@code true} when the pose estimate is considered accurate — i.e., multiple tags are
+ * visible and the combined target area exceeds a minimum threshold.
+ *
+ * @return {@code true} if the pose estimate meets the accuracy criteria
+ */
public boolean hasAccuratePose() {
if (!isAttached()) {
return false;
@@ -271,8 +393,18 @@ public double getDistanceToTagFromCamera() {
return Math.sqrt(Math.pow(x, 2) + Math.pow(y, 2));
}
+ /**
+ * Returns the raw fiducial data for all currently detected AprilTags.
+ *
+ * @return array of {@link RawFiducial} entries from the MegaTag1 estimate; empty array if not
+ * attached or no estimate is available
+ */
public RawFiducial[] getRawFiducial() {
- return LimelightHelpers.getBotPoseEstimate_wpiBlue(config.name).rawFiducials;
+ PoseEstimate est = LimelightHelpers.getBotPoseEstimate_wpiBlue(config.name);
+ if (est == null) {
+ return new RawFiducial[0];
+ }
+ return est.rawFiducials;
}
/**
@@ -284,7 +416,11 @@ public double getMegaTag1PoseTimestamp() {
if (!isAttached()) {
return 0;
}
- return LimelightHelpers.getBotPoseEstimate_wpiBlue(config.getName()).timestampSeconds;
+ PoseEstimate est = LimelightHelpers.getBotPoseEstimate_wpiBlue(config.getName());
+ if (est == null) {
+ return 0;
+ }
+ return est.timestampSeconds;
}
/**
@@ -296,8 +432,11 @@ public double getMegaTag2PoseTimestamp() {
if (!isAttached()) {
return 0;
}
- return LimelightHelpers.getBotPoseEstimate_wpiBlue_MegaTag2(config.getName())
- .timestampSeconds;
+ PoseEstimate est = LimelightHelpers.getBotPoseEstimate_wpiBlue_MegaTag2(config.getName());
+ if (est == null) {
+ return 0;
+ }
+ return est.timestampSeconds;
}
/**
@@ -329,15 +468,25 @@ public double getDistanceToTarget(double targetHeight) {
return 0;
}
return (targetHeight - config.up)
- / Math.tan(Units.degreesToRadians(config.roll + getVerticalOffset()));
+ / Math.tan(Units.degreesToRadians(config.pitch + getVerticalOffset()));
}
+ /**
+ * Marks this camera as actively integrating and updates the log status message.
+ *
+ * @param message a human-readable description of why integration is valid
+ */
public void sendValidStatus(String message) {
config.isIntegrating = true;
this.isIntegrating = config.isIntegrating;
logStatus = message;
}
+ /**
+ * Marks this camera as not integrating and updates the log status message.
+ *
+ * @param message a human-readable description of why integration is invalid
+ */
public void sendInvalidStatus(String message) {
config.isIntegrating = false;
this.isIntegrating = config.isIntegrating;
@@ -376,6 +525,12 @@ public void setRobotOrientation(double degrees) {
LimelightHelpers.SetRobotOrientation(config.name, degrees, 0, 0, 0, 0, 0);
}
+ /**
+ * Sets the robot orientation and yaw rate for the Limelight's internal IMU fusion (MegaTag2).
+ *
+ * @param degrees robot heading in degrees (positive counter-clockwise)
+ * @param angularRate current yaw rate in degrees per second
+ */
public void setRobotOrientation(double degrees, double angularRate) {
if (!isAttached()) {
return;
@@ -383,6 +538,11 @@ public void setRobotOrientation(double degrees, double angularRate) {
LimelightHelpers.SetRobotOrientation(config.name, degrees, angularRate, 0, 0, 0, 0);
}
+ /**
+ * Sets the IMU mode on the Limelight.
+ *
+ * @param mode the IMU mode index (refer to the LL documentation for valid values)
+ */
public void setIMUmode(int mode) {
if (!isAttached()) {
return;
@@ -390,6 +550,13 @@ public void setIMUmode(int mode) {
LimelightHelpers.SetIMUMode(config.name, mode);
}
+ /**
+ * Returns the X offset of the primary target in robot space (meters along the robot's
+ * left/right axis).
+ *
+ * @return target X translation in meters, or {@code -99999} if not attached or no target in
+ * view
+ */
public double getTagTx() {
if (!isAttached()) {
return -99999;
@@ -404,6 +571,11 @@ public double getTagTx() {
return tx;
}
+ /**
+ * Returns the area of the primary target as a percentage of the camera image.
+ *
+ * @return target area (0–100 %), or {@code -99999} if not attached or no target in view
+ */
public double getTagTA() {
if (!isAttached()) {
return -99999;
@@ -417,6 +589,13 @@ public double getTagTA() {
return ta;
}
+ /**
+ * Returns the Z-axis rotation of the primary target in robot space (yaw in radians, converted
+ * to degrees).
+ *
+ * @return target yaw rotation in degrees, or {@code -99999} if not attached or no target in
+ * view
+ */
public double getTagRotationDegrees() {
if (!isAttached()) {
return -99999;
@@ -425,10 +604,10 @@ public double getTagRotationDegrees() {
return -99999;
}
- double rotation =
+ double rotationRadians =
LimelightHelpers.getTargetPose3d_RobotSpace(cameraName).getRotation().getZ();
- return rotation;
+ return Math.toDegrees(rotationRadians);
}
/**
diff --git a/src/main/java/frc/spectrumLib/vision/VisionLogger.java b/src/main/java/frc/spectrumLib/vision/VisionLogger.java
index 6357b9da..e8007451 100644
--- a/src/main/java/frc/spectrumLib/vision/VisionLogger.java
+++ b/src/main/java/frc/spectrumLib/vision/VisionLogger.java
@@ -1,67 +1,120 @@
package frc.spectrumLib.vision;
import edu.wpi.first.math.geometry.Pose2d;
-import frc.spectrumLib.Telemetry;
+import frc.spectrumLib.telemetry.Telemetry;
import lombok.Getter;
+/**
+ * Logs telemetry data from a {@link Limelight} camera to the robot's data-logging system (DogLog)
+ * under the {@code Vision/ Each method reads the corresponding value from the camera, forwards it to {@link
+ * frc.spectrumLib.telemetry.Telemetry#log}, and also returns the value so callers can use it
+ * directly without a second camera query.
+ */
public class VisionLogger {
- /** Tracks position with Limelight using current logger (DogLog) to record data */
+ /** The Limelight camera whose data is being logged. */
private final Limelight limelight;
+ /** Namespace prefix used in all telemetry keys ({@code Vision/
+ *
+ */
+public class Vision implements Subsystem {
+
+ // =========================================================================
+ // Configuration
+ // =========================================================================
+
+ /**
+ * Static configuration for the Vision subsystem, including Limelight identifiers,
+ * camera-to-robot transforms, pipeline indices, and pose estimation covariance parameters.
+ */
+ public static class VisionConfig {
+
+ @Getter final String name = "Vision";
+
+ // -- Back Limelight ---------------------------------------------------
+
+ /** NetworkTables hostname for the rear-facing Limelight. */
+ @Getter final String backLL = "limelight-back";
+
+ /**
+ * Robot-relative pose of the rear Limelight. Translation in metres (x, y, z); rotation in
+ * degrees (roll, pitch, yaw).
+ */
+ @Getter
+ final LimelightConfig backConfig =
+ new LimelightConfig(backLL)
+ .withTranslation(-0.3084987734, 0.2134100126, 0.6502249886)
+ .withRotation(0, 0, 180);
+
+ // -- Left Limelight ---------------------------------------------------
+
+ /** NetworkTables hostname for the left-facing Limelight. */
+ @Getter final String leftLL = "limelight-left";
+
+ /**
+ * Robot-relative pose of the left Limelight. Translation in metres (x, y, z); rotation in
+ * degrees (roll, pitch, yaw).
+ */
+ @Getter
+ final LimelightConfig leftConfig =
+ new LimelightConfig(leftLL).withTranslation(0, 0.215, 0.188).withRotation(0, 0, 90);
+
+ // -- Right Limelight --------------------------------------------------
+
+ /** NetworkTables hostname for the right-facing Limelight. */
+ @Getter final String rightLL = "limelight-right";
+
+ /**
+ * Robot-relative pose of the right Limelight. Translation in metres (x, y, z); rotation in
+ * degrees (roll, pitch, yaw).
+ */
+ @Getter
+ final LimelightConfig rightConfig =
+ new LimelightConfig(rightLL)
+ .withTranslation(-0.04445, 0.3027487722, 0.7137249886)
+ .withRotation(0, 0, -90);
+
+ // -- Turret geometry --------------------------------------------------
+
+ /** Robot-centre to turret pivot offset (metres). */
+ @Getter
+ final Translation2d robotToTurretCenter =
+ new Translation2d(Units.inchesToMeters(-5.5), Units.inchesToMeters(4.7));
+
+ /** Turret pivot to camera offset (metres). */
+ @Getter
+ final Translation2d turretCenterToCamera =
+ new Translation2d(Units.inchesToMeters(-5.641455), 0);
+
+ // -- Pipeline indices -------------------------------------------------
+
+ @Getter final int backTagPipeline = 0;
+ @Getter final int leftTagPipeline = 0;
+ @Getter final int rightTagPipeline = 0;
+
+ // -- Pose estimation covariance ---------------------------------------
+
+ /**
+ * Default translational standard deviation for vision pose measurements (metres). Lower
+ * values trust vision more; higher values trust odometry more.
+ */
+ @Getter double visionStdDevX = 0.5;
+
+ /**
+ * @see #visionStdDevX
+ */
+ @Getter double visionStdDevY = 0.5;
+
+ /**
+ * Default rotational standard deviation for vision pose measurements (radians). Only used
+ * for MT1; MT2 heading is always discarded ({@link #kLargeVariance}).
+ */
+ @Getter double visionStdDevTheta = 0.2;
+
+ /**
+ * Variance used to effectively ignore a measurement dimension (e.g., heading from MT2 or
+ * single-tag MT1).
+ */
+ @Getter final double kLargeVariance = 999999.0;
+
+ /** Measurements older than this many seconds are not fused. */
+ @Getter final double kMaxTimeDeltaSeconds = 0.1;
+
+ /** Pre-built std-dev matrix using the default X/Y/theta values. */
+ @Getter
+ final Matrix
+ *
+ */
+ @Override
+ public void periodic() {
+ setLimeLightOrientation();
+ disabledLimelightUpdates();
+ enabledLimelightUpdates();
+ logTelemetry();
+ }
+
+ /**
+ * Logs connection status, integration status, tag status, MT1 poses, tag count, and target size
+ * for each Limelight via their {@link VisionLogger}. MT2 poses are only logged while disabled
+ * (they are unreliable when moving). Also updates the {@code Field2d} widget with the MT1 pose
+ * from each camera.
+ */
+ public void logTelemetry() {
+ for (VisionLogger logger : allLoggers) {
+ logger.getCameraConnection();
+ logger.getIntegratingStatus();
+ logger.getLogStatus();
+ logger.getTagStatus();
+ logger.getPose();
+ logger.getTagCount();
+ logger.getTargetSize();
+ }
+
+ // MT2 poses are only reliable when the robot is stationary
+ if (Util.disabled.getAsBoolean()) {
+ backLogger.getMegaPose();
+ leftLogger.getMegaPose();
+ rightLogger.getMegaPose();
+ }
+
+ // Update Field2d visualization (null-safe; returns Pose2d.kZero when no data)
+ Robot.getField2d().getObject(backLL.getCameraName()).setPose(getBackMegaTag1Pose());
+ Robot.getField2d().getObject(leftLL.getCameraName()).setPose(getLeftMegaTag1Pose());
+ Robot.getField2d().getObject(rightLL.getCameraName()).setPose(getRightMegaTag1Pose());
+ }
+
+ // =========================================================================
+ // Pose Estimation — Private Pipeline
+ // =========================================================================
+
+ /**
+ * Pushes the robot's current heading (from swerve odometry) to all Limelights each loop so
+ * MegaTag2 IMU fusion uses an up-to-date yaw.
+ */
+ private void setLimeLightOrientation() {
+ double yaw = Robot.getSwerve().getRobotPose().getRotation().getDegrees();
+ for (Limelight limelight : allLimelights) {
+ limelight.setRobotOrientation(yaw);
+ }
+ }
+
+ /**
+ * While the robot is disabled, integrates both MegaTag1 and MegaTag2 estimates from the best
+ * Limelight to pre-seed the pose estimator before enable.
+ */
+ private void disabledLimelightUpdates() {
+ if (Util.disabled.getAsBoolean()) {
+ Limelight bestLimelight = getBestLimelight();
+ integrateSingleEstimate(getMT1VisionEstimate(bestLimelight, true));
+ integrateSingleEstimate(getMT2VisionEstimate(bestLimelight));
+ }
+ }
+
+ /**
+ * While the robot is enabled (teleop, auto-update state, or auto-launching), integrates the
+ * MegaTag1 estimate from the best Limelight.
+ */
+ private void enabledLimelightUpdates() {
+ if (Util.teleop.getAsBoolean()
+ || Auton.autonPoseUpdate.getAsBoolean()
+ || Auton.autonLaunching.getAsBoolean()) {
+ Limelight bestLimelight = getBestLimelight();
+ integrateSingleEstimate(getMT1VisionEstimate(bestLimelight, false));
+ }
+ }
+
+ /**
+ * Builds a MegaTag1 (multi-tag, heading-fused) pose estimate for the given Limelight and
+ * decides whether it is trustworthy enough to add to the pose estimator.
+ *
+ *
+ *
+ *
+ *
+ *
+ *
+ * @param ll the Limelight to query
+ * @return a {@link VisionFieldPoseEstimate} ready to pass to the pose estimator, or {@code
+ * null} if rejected
+ */
+ private VisionFieldPoseEstimate getMT2VisionEstimate(Limelight ll) {
+ if (!ll.targetInView()) {
+ ll.setTagStatus("No Targets in View");
+ ll.sendInvalidStatus("No Targets in View Rejection");
+ return null;
+ }
+
+ boolean multiTags = ll.multipleTagsInView();
+ double targetSize = ll.getTargetSize();
+ Pose2d megaTag2Pose2d = ll.getMegaTag2_Pose2d();
+ ChassisSpeeds robotSpeed = Robot.getSwerve().getCurrentRobotChassisSpeeds();
+ double robotLinearSpeed =
+ Math.hypot(robotSpeed.vxMetersPerSecond, robotSpeed.vyMetersPerSecond);
+
+ double mt2PoseDifference =
+ Robot.getSwerve()
+ .getRobotPose()
+ .getTranslation()
+ .getDistance(megaTag2Pose2d.getTranslation());
+
+ if (rejectionCheck(ll, megaTag2Pose2d, targetSize)) {
+ return null;
+ }
+
+ // Select translational std-devs (heading is always discarded for MT2)
+ double xyStds;
+
+ if (robotLinearSpeed <= 0.2 && targetSize > 4) {
+ ll.sendValidStatus("Stationary close integration");
+ xyStds = 0.1;
+ } else if (multiTags && targetSize > 2) {
+ ll.sendValidStatus("Strong Multi integration");
+ xyStds = 0.1;
+ } else if (multiTags && targetSize > 0.2) {
+ ll.sendValidStatus("Multi integration");
+ xyStds = 0.25;
+ } else if (targetSize > 2 && (mt2PoseDifference < 0.5 || DriverStation.isDisabled())) {
+ ll.sendValidStatus("Close integration");
+ xyStds = 0.5;
+ } else if (targetSize > 1 && (mt2PoseDifference < 0.25 || DriverStation.isDisabled())) {
+ ll.sendValidStatus("Proximity integration");
+ xyStds = 1.0;
+ } else if (targetSize >= 0.03) {
+ ll.sendValidStatus("Stable integration");
+ xyStds = 1.5;
+ } else {
+ ll.sendInvalidStatus("Integration Criteria not Met");
+ return null;
+ }
+
+ double degStds = config.getKLargeVariance();
+
+ Pose2d integratedPose =
+ new Pose2d(megaTag2Pose2d.getTranslation(), megaTag2Pose2d.getRotation());
+
+ return new VisionFieldPoseEstimate(
+ integratedPose,
+ Utils.fpgaToCurrentTime(ll.getMegaTag2PoseTimestamp()),
+ VecBuilder.fill(xyStds, xyStds, Units.degreesToRadians(degStds)),
+ (int) ll.getTagCountInView());
+ }
+
+ /**
+ * Adds a vision measurement to the swerve pose estimator if the estimate is non-null.
+ *
+ * @param estimate the estimate to integrate, or {@code null} to skip
+ */
+ private void integrateSingleEstimate(VisionFieldPoseEstimate estimate) {
+ if (estimate != null) {
+ Robot.getSwerve()
+ .addVisionMeasurement(
+ estimate.getVisionRobotPoseMeters(),
+ estimate.getTimestampSeconds(),
+ estimate.getVisionMeasurementStdDevs());
+ }
+ }
+
+ /**
+ * Common rejection gate shared by both MT1 and MT2 pipelines.
+ *
+ *
+ *
+ *
+ * @param ll the Limelight (used for status reporting)
+ * @param pose the candidate pose to validate
+ * @param targetSize the Limelight target-size percentage
+ * @return {@code true} if the measurement should be rejected
+ */
+ private boolean rejectionCheck(Limelight ll, Pose2d pose, double targetSize) {
+ if (FieldHelpers.poseOutOfField(pose)) {
+ ll.sendInvalidStatus("Out of Field Rejection");
+ return true;
+ }
+
+ if (Math.abs(Robot.getSwerve().getCurrentRobotChassisSpeeds().omegaRadiansPerSecond)
+ >= 1.6) {
+ ll.sendInvalidStatus("Rotation Speed Rejection");
+ return true;
+ }
+
+ if (targetSize <= 0.025) {
+ ll.sendInvalidStatus("Target Size Rejection");
+ return true;
+ }
+
+ return false;
+ }
+
+ /**
+ * Updates the IMU mode on a Limelight only when the desired mode differs from the last written
+ * value, avoiding redundant NetworkTables writes.
+ *
+ * @param limelight the Limelight to configure
+ * @param desiredMode the IMU mode to apply (0 = external, 1 = internal, etc.)
+ */
+ private void setImuModeIfChanged(Limelight limelight, int desiredMode) {
+ Integer lastMode = lastImuModeByLL.get(limelight);
+ if (lastMode == null || lastMode.intValue() != desiredMode) {
+ limelight.setIMUmode(desiredMode);
+ lastImuModeByLL.put(limelight, desiredMode);
+ }
+ }
+
+ // =========================================================================
+ // Pose Access & Queries
+ // =========================================================================
+
+ /**
+ * Returns the MegaTag1 (MT1) pose from the back Limelight, or {@link Pose2d#kZero} if no
+ * estimate is available.
+ */
+ public Pose2d getBackMegaTag1Pose() {
+ Pose2d pose = backLL.getMegaTag1_Pose3d().toPose2d();
+ return pose != null ? pose : Pose2d.kZero;
+ }
+
+ /**
+ * Returns the MegaTag1 (MT1) pose from the left Limelight, or {@link Pose2d#kZero} if no
+ * estimate is available.
+ */
+ public Pose2d getLeftMegaTag1Pose() {
+ Pose2d pose = leftLL.getMegaTag1_Pose3d().toPose2d();
+ return pose != null ? pose : Pose2d.kZero;
+ }
+
+ /**
+ * Returns the MegaTag1 (MT1) pose from the right Limelight, or {@link Pose2d#kZero} if no
+ * estimate is available.
+ */
+ public Pose2d getRightMegaTag1Pose() {
+ Pose2d pose = rightLL.getMegaTag1_Pose3d().toPose2d();
+ return pose != null ? pose : Pose2d.kZero;
+ }
+
+ /**
+ * Returns the MegaTag2 (MT2) pose from the back Limelight, or {@link Pose2d#kZero} if no
+ * estimate is available.
+ */
+ public Pose2d getBackMegaTag2Pose() {
+ Pose2d pose = backLL.getMegaTag2_Pose2d();
+ return pose != null ? pose : Pose2d.kZero;
+ }
+
+ /**
+ * Returns the MegaTag2 (MT2) pose from the left Limelight, or {@link Pose2d#kZero} if no
+ * estimate is available.
+ */
+ public Pose2d getLeftMegaTag2Pose() {
+ Pose2d pose = leftLL.getMegaTag2_Pose2d();
+ return pose != null ? pose : Pose2d.kZero;
+ }
+
+ /**
+ * Returns the MegaTag2 (MT2) pose from the right Limelight, or {@link Pose2d#kZero} if no
+ * estimate is available.
+ */
+ public Pose2d getRightMegaTag2Pose() {
+ Pose2d pose = rightLL.getMegaTag2_Pose2d();
+ return pose != null ? pose : Pose2d.kZero;
+ }
+
+ /**
+ * Selects and returns the Limelight with the highest combined score of visible tag count and
+ * target size. Ties fall back to {@link #backLL}.
+ *
+ * @return the Limelight currently offering the best view of AprilTags
+ */
+ public Limelight getBestLimelight() {
+ Limelight bestLimelight = backLL;
+ double bestScore = 0;
+ for (Limelight limelight : allLimelights) {
+ double score = limelight.getTagCountInView() + limelight.getTargetSize();
+ if (score > bestScore) {
+ bestScore = score;
+ bestLimelight = limelight;
+ }
+ }
+ return bestLimelight;
+ }
+
+ /**
+ * Returns {@code true} if at least one Limelight reports an accurate pose (via {@link
+ * Limelight#hasAccuratePose()}).
+ */
+ public boolean hasAccuratePose() {
+ for (Limelight limelight : allLimelights) {
+ if (limelight.hasAccuratePose()) return true;
+ }
+ return false;
+ }
+
+ /**
+ * Returns {@code true} if any Limelight currently sees an AprilTag belonging to the current
+ * alliance's set of scoring targets.
+ *
+ *
+ *
+ *
+ * @param targetInView whether the Limelight has a target
+ * @param botpose3D the MT1 Pose3d from the Limelight
+ * @param megaPose the MT2 Pose2d (provides translation for the reset)
+ * @param poseTimestamp the FPGA timestamp of the pose estimate
+ * @return {@code true} if the pose was accepted and the reset was applied
+ */
+ public boolean resetPoseToVision(
+ boolean targetInView, Pose3d botpose3D, Pose2d megaPose, double poseTimestamp) {
+
+ if (!targetInView) return false;
+
+ Pose2d botpose = botpose3D.toPose2d();
+
+ if (FieldHelpers.poseOutOfField(botpose3D)) {
+ Telemetry.log("Vision/PoseReset/Rejection", "Out of field");
+ return false;
+ }
+ if (Math.abs(botpose3D.getZ()) > 0.25) {
+ Telemetry.log("Vision/PoseReset/Rejection", "Pose in air");
+ return false;
+ }
+ if (Math.abs(botpose3D.getRotation().getX()) > Math.toRadians(5)
+ || Math.abs(botpose3D.getRotation().getY()) > Math.toRadians(5)) {
+ Telemetry.log("Vision/PoseReset/Rejection", "Pose tilted");
+ return false;
+ }
+
+ double[] before = {botpose.getX(), botpose.getY(), botpose.getRotation().getDegrees()};
+ Telemetry.log("Vision/PoseReset/Before", before);
+
+ // Use MT2 translation + MT1 heading for best combined accuracy
+ Pose2d integratedPose = new Pose2d(megaPose.getTranslation(), botpose.getRotation());
+ Robot.getSwerve()
+ .addVisionMeasurement(
+ integratedPose, poseTimestamp, VecBuilder.fill(0.00001, 0.00001, 0.00001));
+
+ Pose2d updated = Robot.getSwerve().getRobotPose();
+ double[] after = {updated.getX(), updated.getY(), updated.getRotation().getDegrees()};
+ Telemetry.log("Vision/PoseReset/After", after);
+
+ return true;
+ }
+
+ // =========================================================================
+ // Camera Control
+ // =========================================================================
+
+ /**
+ * Sets all Limelights to the given pipeline index.
+ *
+ * @param pipeline the zero-indexed pipeline number to activate
+ */
+ public void setLimelightPipelines(int pipeline) {
+ for (Limelight limelight : allLimelights) {
+ limelight.setLimelightPipeline(pipeline);
+ }
+ }
+
+ // =========================================================================
+ // Commands
+ // =========================================================================
+
+ /**
+ * Returns a command that blinks all Limelight LEDs while active and turns them off when the
+ * command ends.
+ *
+ * @return the blink command
+ */
+ public Command blinkLimelights() {
+ Telemetry.print("Vision.blinkLimelights", PrintPriority.HIGH);
+ return startEnd(
+ () -> {
+ for (Limelight limelight : allLimelights) {
+ limelight.blinkLEDs();
+ }
+ },
+ () -> {
+ for (Limelight limelight : allLimelights) {
+ limelight.setLEDMode(false);
+ }
+ })
+ .withName("Vision.blinkLimelights");
+ }
+
+ /**
+ * Returns a command that holds all Limelight LEDs solid-on while active and turns them off when
+ * the command ends.
+ *
+ * @return the solid-LED command
+ */
+ public Command solidLimelight() {
+ return startEnd(
+ () -> {
+ for (Limelight limelight : allLimelights) {
+ limelight.setLEDMode(true);
+ }
+ },
+ () -> {
+ for (Limelight limelight : allLimelights) {
+ limelight.setLEDMode(false);
+ }
+ })
+ .withName("Vision.solidLimelight");
+ }
+
+ // =========================================================================
+ // Inner Classes
+ // =========================================================================
+
+ /**
+ * Immutable data class that bundles a vision-derived field pose with its FPGA timestamp and
+ * covariance matrix, ready for use with {@code
+ * SwerveDrivePoseEstimator.addVisionMeasurement()}.
+ */
+ @Getter
+ public class VisionFieldPoseEstimate {
+
+ /** The estimated field-relative robot pose (metres, radians). */
+ private final Pose2d visionRobotPoseMeters;
+
+ /** The FPGA-converted timestamp of this measurement (seconds). */
+ private final double timestampSeconds;
+
+ /**
+ * The 3×1 standard-deviation vector {@code [x, y, theta]} passed to the pose estimator.
+ * Larger values indicate less trust in that dimension.
+ */
+ private final Matrix
+ *
+ *
+ *
+ *
+ *
+ *
+ * 140–130 purple
+ * 130–105 startingColor
+ * 105–80 opponent color
+ * 80–55 startingColor
+ * 55–30 opponent color
+ * 30–0 purple
+ *
+ *
+ * Represents the simulation of a single {@link SwerveModule}.
*/
protected static class SimSwerveModule {
+ /** Constants (gear ratios, friction voltages, wheel radius, etc.) for this module. */
public final SwerveModuleConstants<
TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration>
moduleConstant;
+
+ /** Maple-Sim physics simulation instance for this module. */
public final SwerveModuleSimulation moduleSimulation;
+ /**
+ * Constructs a simulated swerve module by wiring the Maple-Sim physics model to the CTRE
+ * motor controllers.
+ *
+ * @param moduleConstant constants for this swerve module
+ * @param moduleSimulation Maple-Sim simulation instance for this module
+ * @param module the real CTRE {@link SwerveModule} whose sim states will be driven
+ */
public SimSwerveModule(
SwerveModuleConstants<
TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration>
@@ -170,16 +186,40 @@ public SimSwerveModule(
}
// Static utils classes
+
+ /**
+ * Adapts a {@link TalonFX} motor controller for use as a {@link SimulatedMotorController} in
+ * Maple-Sim by forwarding encoder state from the simulation into the CTRE sim state.
+ */
public static class TalonFXMotorControllerSim implements SimulatedMotorController {
+ /** CAN device ID of the underlying TalonFX. */
public final int id;
+ /** CTRE simulation state object used to inject position, velocity, and voltage. */
private final TalonFXSimState talonFXSimState;
+ /**
+ * Constructs the adapter for the given TalonFX.
+ *
+ * @param talonFX the TalonFX motor controller to wrap
+ */
public TalonFXMotorControllerSim(TalonFX talonFX) {
this.id = talonFX.getDeviceID();
this.talonFXSimState = talonFX.getSimState();
}
+ /**
+ * Injects simulated rotor position, velocity, and supply voltage into the TalonFX sim
+ * state, then returns the motor output voltage requested by the controller's closed-loop
+ * algorithm.
+ *
+ * @param mechanismAngle current mechanism-side angle from the physics model
+ * @param mechanismVelocity current mechanism-side angular velocity from the physics model
+ * @param encoderAngle current encoder angle (rotor-side) from the physics model
+ * @param encoderVelocity current encoder angular velocity (rotor-side) from the physics
+ * model
+ * @return the voltage the controller is requesting from the simulated battery
+ */
@Override
public Voltage updateControlSignal(
Angle mechanismAngle,
@@ -194,12 +234,25 @@ public Voltage updateControlSignal(
}
}
+ /**
+ * Extends {@link TalonFXMotorControllerSim} to also drive a remote CANcoder simulation state.
+ * Used for steer motors whose feedback device is a remote CANcoder.
+ */
@SuppressWarnings("all")
public static class TalonFXMotorControllerWithRemoteCanCoderSim
extends TalonFXMotorControllerSim {
+ /** CAN device ID of the remote CANcoder. */
private final int encoderId;
+
+ /** CTRE simulation state of the remote CANcoder. */
private final CANcoderSimState remoteCancoderSimState;
+ /**
+ * Constructs the adapter for a steer motor paired with a remote CANcoder.
+ *
+ * @param talonFX the steer TalonFX motor controller
+ * @param cancoder the remote CANcoder used as the steer feedback sensor
+ */
public TalonFXMotorControllerWithRemoteCanCoderSim(TalonFX talonFX, CANcoder cancoder) {
super(talonFX);
this.remoteCancoderSimState = cancoder.getSimState();
@@ -207,6 +260,18 @@ public TalonFXMotorControllerWithRemoteCanCoderSim(TalonFX talonFX, CANcoder can
this.encoderId = cancoder.getDeviceID();
}
+ /**
+ * Injects simulated supply voltage, position, and velocity into the remote CANcoder sim
+ * state, then delegates to the parent implementation to update the TalonFX and return the
+ * requested motor voltage.
+ *
+ * @param mechanismAngle current mechanism-side angle (written to the CANcoder)
+ * @param mechanismVelocity current mechanism-side angular velocity (written to the
+ * CANcoder)
+ * @param encoderAngle current encoder (rotor-side) angle (forwarded to TalonFX)
+ * @param encoderVelocity current encoder angular velocity (forwarded to TalonFX)
+ * @return the voltage the controller is requesting from the simulated battery
+ */
@Override
public Voltage updateControlSignal(
Angle mechanismAngle,
diff --git a/src/main/java/frc/spectrumLib/swerve/SysID.java b/src/main/java/frc/spectrumLib/swerve/SysID.java
index 0deeed69..e45396d4 100644
--- a/src/main/java/frc/spectrumLib/swerve/SysID.java
+++ b/src/main/java/frc/spectrumLib/swerve/SysID.java
@@ -6,23 +6,51 @@
import com.ctre.phoenix6.swerve.SwerveRequest;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine;
-import frc.robot.swerve.Swerve;
+import frc.robot.subsystems.swerve.Swerve;
import lombok.Getter;
+/**
+ * Encapsulates the three SysId characterization routines for a CTRE swerve drivetrain: translation,
+ * rotation, and steer-gains.
+ *
+ *
+ *
*/
public enum PrintPriority {
NORMAL,
HIGH
}
+ /** Minimum priority level a message must have to be written to the console. */
private static PrintPriority priority = PrintPriority.HIGH;
+ /**
+ * Creates a Telemetry instance and registers it as a WPILib subsystem so its {@link
+ * #periodic()} method is called every loop cycle.
+ */
public Telemetry() {
super();
register();
}
+ /** Called every robot loop cycle. Logs any newly active alerts from NetworkTables. */
@Override
public void periodic() {
logAlerts();
@@ -87,6 +102,12 @@ private static void setPriority(PrintPriority priority) {
Telemetry.priority = priority;
}
+ /**
+ * Wraps a command so that its initialization and end are logged to the "Commands" key.
+ *
+ * @param cmd The command to wrap
+ * @return a decorated command that logs lifecycle events and preserves the original name
+ */
public static Command log(Command cmd) {
return cmd.deadlineFor(
Commands.startEnd(
@@ -105,11 +126,20 @@ public static void print(String output, PrintPriority priority) {
log("Prints", out);
}
+ /**
+ * Prints a message at {@link PrintPriority#NORMAL} priority. The message is always written to
+ * the DogLog "Prints" key but only echoed to stdout when the global priority allows it.
+ *
+ * @param output The string to print
+ */
public static void print(String output) {
print(output, PrintPriority.NORMAL);
}
- // New method to log alerts from NetworkTables
+ /**
+ * Reads all active alerts from the SmartDashboard NetworkTable and logs any that are new since
+ * the last call under the "Alerts" DogLog key.
+ */
public static void logAlerts() {
NetworkTableInstance ntInstance = NetworkTableInstance.getDefault();
logAlertType(ntInstance, "errors", "ERROR");
diff --git a/src/main/java/frc/spectrumLib/telemetry/TuneValue.java b/src/main/java/frc/spectrumLib/telemetry/TuneValue.java
new file mode 100644
index 00000000..ef45183e
--- /dev/null
+++ b/src/main/java/frc/spectrumLib/telemetry/TuneValue.java
@@ -0,0 +1,50 @@
+package frc.spectrumLib.telemetry;
+
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import java.util.function.DoubleSupplier;
+import lombok.Getter;
+
+/**
+ * A numeric value that is published to SmartDashboard and can be modified at runtime without
+ * redeploying code. Useful for tuning PID gains, speeds, and other constants during development.
+ * Place a call to {@link #update()} or pass {@link #getSupplier()} inside a command to read the
+ * latest value each loop.
+ */
+public class TuneValue {
+ /** Current value, refreshed on each call to {@link #update()}. */
+ @Getter private double value;
+ /** SmartDashboard key under which this value is published and read. */
+ @Getter private String name;
+
+ /**
+ * Creates a TuneValue, publishing {@code defaultValue} to SmartDashboard under {@code name}.
+ *
+ * @param name SmartDashboard key
+ * @param defaultValue Initial value written to SmartDashboard
+ */
+ public TuneValue(String name, double defaultValue) {
+ SmartDashboard.putNumber(name, defaultValue);
+ value = defaultValue;
+ this.name = name;
+ }
+
+ /**
+ * Reads the current value from SmartDashboard and caches it locally.
+ *
+ * @return the latest value from SmartDashboard
+ */
+ public Double update() {
+ value = SmartDashboard.getNumber(name, value);
+ return value;
+ }
+
+ /**
+ * Returns a {@link DoubleSupplier} that calls {@link #update()} each time it is queried,
+ * suitable for passing to command factories that accept live-updating suppliers.
+ *
+ * @return a supplier backed by this TuneValue
+ */
+ public DoubleSupplier getSupplier() {
+ return this::update;
+ }
+}
diff --git a/src/main/java/frc/spectrumLib/CachedDouble.java b/src/main/java/frc/spectrumLib/util/CachedDouble.java
similarity index 58%
rename from src/main/java/frc/spectrumLib/CachedDouble.java
rename to src/main/java/frc/spectrumLib/util/CachedDouble.java
index 1fcf6246..02d1bebb 100644
--- a/src/main/java/frc/spectrumLib/CachedDouble.java
+++ b/src/main/java/frc/spectrumLib/util/CachedDouble.java
@@ -1,4 +1,4 @@
-package frc.spectrumLib;
+package frc.spectrumLib.util;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import java.util.function.DoubleSupplier;
@@ -12,15 +12,30 @@ public class CachedDouble extends SubsystemBase implements DoubleSupplier {
private double value;
private final DoubleSupplier source;
+ /**
+ * Creates a CachedDouble wrapping the given supplier.
+ *
+ * @param source the underlying supplier whose value is cached each scheduler iteration
+ */
public CachedDouble(DoubleSupplier source) {
this.source = source;
}
+ /**
+ * Called by the scheduler each iteration to invalidate the cached value so the next {@link
+ * #getAsDouble()} call re-queries the source.
+ */
@Override
public void periodic() {
cached = false;
}
+ /**
+ * Returns the cached value of the source supplier, querying the supplier at most once per
+ * scheduler iteration.
+ *
+ * @return the supplier's value for the current iteration
+ */
@Override
public double getAsDouble() {
if (!cached) {
diff --git a/src/main/java/frc/spectrumLib/util/CanDeviceId.java b/src/main/java/frc/spectrumLib/util/CanDeviceId.java
index eeb855bd..bd334d29 100644
--- a/src/main/java/frc/spectrumLib/util/CanDeviceId.java
+++ b/src/main/java/frc/spectrumLib/util/CanDeviceId.java
@@ -2,28 +2,60 @@
// Based on 254-2023 Class
// https://github.com/Team254/FRC-2023-Public/blob/main/src/main/java/com/team254/lib/drivers/CanDeviceId.java
+/**
+ * Identifies a CAN device by its numeric device ID and the CAN bus name it lives on. Equality and
+ * hashing consider both fields, so two instances with the same device number on different buses are
+ * treated as distinct.
+ */
public class CanDeviceId {
private final int mDeviceNumber;
private final String mBus;
+ /**
+ * Creates a CAN device identifier with an explicit bus name.
+ *
+ * @param deviceNumber the numeric CAN ID assigned to the device
+ * @param bus the name of the CAN bus (e.g. {@code "rio"} or {@code "canivore"})
+ */
public CanDeviceId(int deviceNumber, String bus) {
mDeviceNumber = deviceNumber;
mBus = bus;
}
// Use the default bus name (empty string).
+ /**
+ * Creates a CAN device identifier on the default CAN bus (empty string).
+ *
+ * @param deviceNumber the numeric CAN ID assigned to the device
+ */
public CanDeviceId(int deviceNumber) {
this(deviceNumber, "");
}
+ /**
+ * Returns the numeric CAN device ID.
+ *
+ * @return the device number
+ */
public int getDeviceNumber() {
return mDeviceNumber;
}
+ /**
+ * Returns the CAN bus name this device is on.
+ *
+ * @return the bus name, or an empty string for the default bus
+ */
public String getBus() {
return mBus;
}
+ /**
+ * Type-safe equality check against another {@link CanDeviceId}.
+ *
+ * @param other the other instance to compare
+ * @return {@code true} if both the device number and bus name match
+ */
public boolean equals(CanDeviceId other) {
return equals((Object) other);
}
diff --git a/src/main/java/frc/spectrumLib/util/CrashTracker.java b/src/main/java/frc/spectrumLib/util/CrashTracker.java
index 904311b9..b9b1999d 100644
--- a/src/main/java/frc/spectrumLib/util/CrashTracker.java
+++ b/src/main/java/frc/spectrumLib/util/CrashTracker.java
@@ -1,7 +1,7 @@
package frc.spectrumLib.util;
import edu.wpi.first.wpilibj.RobotBase;
-import frc.spectrumLib.Telemetry;
+import frc.spectrumLib.telemetry.Telemetry;
import java.io.FileNotFoundException;
import java.io.FileWriter;
import java.io.IOException;
diff --git a/src/main/java/frc/spectrumLib/util/ExpCurve.java b/src/main/java/frc/spectrumLib/util/ExpCurve.java
index a77dcaac..cc5313d2 100644
--- a/src/main/java/frc/spectrumLib/util/ExpCurve.java
+++ b/src/main/java/frc/spectrumLib/util/ExpCurve.java
@@ -44,7 +44,10 @@ public ExpCurve(double expVal, double offset, double scalar, double deadzone) {
}
/**
- * @param input value to be mapped
+ * Applies the full exponential curve pipeline: deadzone, exponent, scalar, then offset.
+ *
+ * @param input the raw input value to be mapped (typically in [-1, 1])
+ * @return the mapped output value
*/
@Override
public double calculate(double input) {
diff --git a/src/main/java/frc/spectrumLib/util/Network.java b/src/main/java/frc/spectrumLib/util/Network.java
index 11e228b7..57462fa9 100644
--- a/src/main/java/frc/spectrumLib/util/Network.java
+++ b/src/main/java/frc/spectrumLib/util/Network.java
@@ -63,9 +63,11 @@ public static String getIPaddress() {
}
/**
- * Gets the IP Address of the device at the address such as "limelight.local"
+ * Resolves and returns the IP address of a device identified by its mDNS or hostname address
+ * (e.g. {@code "limelight.local"}).
*
- * @return the IP Address of the device
+ * @param deviceNameAddress the hostname or mDNS name to resolve
+ * @return the resolved IP address string, or {@code "UNKNOWN"} if resolution fails
*/
public static String getIPaddress(String deviceNameAddress) {
InetAddress localHost;
diff --git a/src/main/java/frc/spectrumLib/util/Trio.java b/src/main/java/frc/spectrumLib/util/Trio.java
index 5cf77472..df6ff51f 100644
--- a/src/main/java/frc/spectrumLib/util/Trio.java
+++ b/src/main/java/frc/spectrumLib/util/Trio.java
@@ -47,6 +47,11 @@ public B getSecond() {
return m_second;
}
+ /**
+ * Returns the third object.
+ *
+ * @return The third object.
+ */
public C getThird() {
return m_third;
}
diff --git a/src/main/java/frc/spectrumLib/util/Util.java b/src/main/java/frc/spectrumLib/util/Util.java
index f62570ec..809ac07f 100644
--- a/src/main/java/frc/spectrumLib/util/Util.java
+++ b/src/main/java/frc/spectrumLib/util/Util.java
@@ -9,20 +9,42 @@
/** From 254 lib imported from 1678-2024 Contains basic functions that are used often. */
public class Util {
+ /** Small value used for floating-point equality comparisons. */
public static final double EPSILON = 1e-12;
/** Prevent this class from being instantiated. */
private Util() {}
- /** Limits the given input to the given magnitude. */
+ /**
+ * Clamps {@code v} to the range [{@code -maxMagnitude}, {@code maxMagnitude}].
+ *
+ * @param v the value to clamp
+ * @param maxMagnitude the maximum absolute value allowed
+ * @return the clamped value
+ */
public static double limit(double v, double maxMagnitude) {
return limit(v, -maxMagnitude, maxMagnitude);
}
+ /**
+ * Clamps {@code v} to the range [{@code min}, {@code max}].
+ *
+ * @param v the value to clamp
+ * @param min the lower bound (inclusive)
+ * @param max the upper bound (inclusive)
+ * @return the clamped value
+ */
public static double limit(double v, double min, double max) {
return Math.min(max, Math.max(min, v));
}
+ /**
+ * Checks whether {@code v} is strictly within [{@code -maxMagnitude}, {@code maxMagnitude}].
+ *
+ * @param v the value to test
+ * @param maxMagnitude the maximum absolute value (exclusive bound)
+ * @return {@code true} if {@code |v| < maxMagnitude}
+ */
public static boolean inRange(double v, double maxMagnitude) {
return inRange(v, -maxMagnitude, maxMagnitude);
}
@@ -32,15 +54,40 @@ public static boolean inRange(double v, double min, double max) {
return v > min && v < max;
}
+ /**
+ * Checks whether the value supplied by {@code v} is strictly between the values supplied by
+ * {@code min} and {@code max}.
+ *
+ * @param v supplier of the value to test
+ * @param min supplier of the lower bound (exclusive)
+ * @param max supplier of the upper bound (exclusive)
+ * @return {@code true} if {@code min.get() < v.get() < max.get()}
+ */
public static boolean inRange(DoubleSupplier v, DoubleSupplier min, DoubleSupplier max) {
return v.getAsDouble() > min.getAsDouble() && v.getAsDouble() < max.getAsDouble();
}
+ /**
+ * Linearly interpolates between {@code a} and {@code b} by a factor {@code x}, clamped to [0,
+ * 1].
+ *
+ * @param a the start value ({@code x = 0})
+ * @param b the end value ({@code x = 1})
+ * @param x the interpolation factor, clamped to [0, 1]
+ * @return the interpolated value
+ */
public static double interpolate(double a, double b, double x) {
x = limit(x, 0.0, 1.0);
return a + (b - a) * x;
}
+ /**
+ * Joins a list of objects into a single string with the given delimiter.
+ *
+ * @param delim the delimiter placed between consecutive elements
+ * @param strings the list of objects whose {@code toString()} values are joined
+ * @return the joined string
+ */
public static String joinStrings(final String delim, final List strings) {
StringBuilder sb = new StringBuilder();
for (int i = 0; i < strings.size(); ++i) {
@@ -52,18 +99,49 @@ public static String joinStrings(final String delim, final List strings) {
return sb.toString();
}
+ /**
+ * Checks whether {@code a} and {@code b} are within {@code epsilon} of each other.
+ *
+ * @param a first value
+ * @param b second value
+ * @param epsilon the allowed absolute difference
+ * @return {@code true} if {@code |a - b| <= epsilon}
+ */
public static boolean epsilonEquals(double a, double b, double epsilon) {
return (a - epsilon <= b) && (a + epsilon >= b);
}
+ /**
+ * Checks whether {@code a} and {@code b} are within {@link #EPSILON} of each other.
+ *
+ * @param a first value
+ * @param b second value
+ * @return {@code true} if {@code |a - b| <= EPSILON}
+ */
public static boolean epsilonEquals(double a, double b) {
return epsilonEquals(a, b, EPSILON);
}
+ /**
+ * Checks whether integer {@code a} and {@code b} are within {@code epsilon} of each other.
+ *
+ * @param a first integer value
+ * @param b second integer value
+ * @param epsilon the allowed absolute difference
+ * @return {@code true} if {@code |a - b| <= epsilon}
+ */
public static boolean epsilonEquals(int a, int b, int epsilon) {
return (a - epsilon <= b) && (a + epsilon >= b);
}
+ /**
+ * Checks whether every element in {@code list} is within {@code epsilon} of {@code value}.
+ *
+ * @param list the list of doubles to check
+ * @param value the target value each element is compared against
+ * @param epsilon the allowed absolute difference for each comparison
+ * @return {@code true} if all elements are within {@code epsilon} of {@code value}
+ */
public static boolean allCloseTo(final List
+ *
+ */
public class Limelight {
/* Limelight Configuration */
+ /**
+ * Configuration for a single Limelight camera, including its network-table name and physical
+ * mounting position on the robot.
+ *
+ *