diff --git a/src/main/deploy/apriltag/practice_map.json b/src/main/deploy/apriltag/practice_map.json new file mode 100644 index 0000000..d986f62 --- /dev/null +++ b/src/main/deploy/apriltag/practice_map.json @@ -0,0 +1,261 @@ +{ + "tags": [ + { + "ID": 2, + "pose": { + "translation": { + "x": 2.91849684715271, + "y": -4.538444519042969, + "z": 0.5754019021987915 + }, + "rotation": { + "quaternion": { + "W": -0.0015464945463463664, + "X": -0.002658718964084983, + "Y": -0.0027845054864883423, + "Z": 0.9999912977218628 + } + } + } + }, + { + "ID": 3, + "pose": { + "translation": { + "x": 3.127329111099243, + "y": -5.131237030029297, + "z": 0.5727609395980835 + }, + "rotation": { + "quaternion": { + "W": 0.7085180282592773, + "X": 0.006599326618015766, + "Y": 0.011610775254666805, + "Z": -0.7055661678314209 + } + } + } + }, + { + "ID": 4, + "pose": { + "translation": { + "x": 3.4936864376068115, + "y": -5.135638236999512, + "z": 0.5759871602058411 + }, + "rotation": { + "quaternion": { + "W": 0.718272864818573, + "X": 0.014575175009667873, + "Y": 0.012147395871579647, + "Z": -0.6955026984214783 + } + } + } + }, + { + "ID": 5, + "pose": { + "translation": { + "x": 4.056725025177002, + "y": -4.525740146636963, + "z": 0.5659568905830383 + }, + "rotation": { + "quaternion": { + "W": 0.9966632127761841, + "X": 0.0158087145537138, + "Y": 0.05140738934278488, + "Z": -0.061398304998874664 + } + } + } + }, + { + "ID": 6, + "pose": { + "translation": { + "x": 6.9187750816345215, + "y": -4.504202842712402, + "z": 0.33130407333374023 + }, + "rotation": { + "quaternion": { + "W": 0.7302694320678711, + "X": -0.015144702978432178, + "Y": -0.021585887297987938, + "Z": -0.6826500296592712 + } + } + } + }, + { + "ID": 7, + "pose": { + "translation": { + "x": 6.847953796386719, + "y": -4.535053253173828, + "z": 0.3529137372970581 + }, + "rotation": { + "quaternion": { + "W": 0.6875620484352112, + "X": -0.07726679742336273, + "Y": 0.07092981785535812, + "Z": 0.7185103893280029 + } + } + } + }, + { + "ID": 8, + "pose": { + "translation": { + "x": 4.087681293487549, + "y": -4.171364784240723, + "z": 0.5756258964538574 + }, + "rotation": { + "quaternion": { + "W": 0.9976387619972229, + "X": -0.0018898008856922388, + "Y": -0.06773331016302109, + "Z": 0.011198120191693306 + } + } + } + }, + { + "ID": 9, + "pose": { + "translation": { + "x": 3.9083917140960693, + "y": -3.990419626235962, + "z": 0.5789980888366699 + }, + "rotation": { + "quaternion": { + "W": 0.6651341319084167, + "X": -0.028980275616049767, + "Y": 0.010910031385719776, + "Z": 0.746081531047821 + } + } + } + }, + { + "ID": 10, + "pose": { + "translation": { + "x": 3.5427753925323486, + "y": -3.962294578552246, + "z": 0.5722489356994629 + }, + "rotation": { + "quaternion": { + "W": 0.7077287435531616, + "X": 0.007673805113881826, + "Y": -0.006959905382245779, + "Z": 0.7064082622528076 + } + } + } + }, + { + "ID": 11, + "pose": { + "translation": { + "x": 2.918107748031616, + "y": -4.178390026092529, + "z": 0.5687437057495117 + }, + "rotation": { + "quaternion": { + "W": 0.011189894750714302, + "X": 0.047166742384433746, + "Y": 0.007510459516197443, + "Z": 0.9987960457801819 + } + } + } + }, + { + "ID": 13, + "pose": { + "translation": { + "x": 0.0, + "y": 0.0, + "z": 0.0 + }, + "rotation": { + "quaternion": { + "W": 0.7069476246833801, + "X": 0.009696598164737225, + "Y": 0.015003656037151814, + "Z": -0.7070401310920715 + } + } + } + }, + { + "ID": 14, + "pose": { + "translation": { + "x": 0.442140132188797, + "y": 0.02667858824133873, + "z": -0.00486380560323596 + }, + "rotation": { + "quaternion": { + "W": 0.7105119824409485, + "X": -0.07282736897468567, + "Y": -0.0551576092839241, + "Z": -0.6977295279502869 + } + } + } + }, + { + "ID": 15, + "pose": { + "translation": { + "x": 3.3096587657928467, + "y": 0.017690449953079224, + "z": 0.0063992817886173725 + }, + "rotation": { + "quaternion": { + "W": 0.702315092086792, + "X": 0.007732210215181112, + "Y": -0.003434067592024803, + "Z": -0.7118158936500549 + } + } + } + }, + { + "ID": 16, + "pose": { + "translation": { + "x": 3.851165771484375, + "y": 0.019229983910918236, + "z": 0.014089105650782585 + }, + "rotation": { + "quaternion": { + "W": 0.7048658728599548, + "X": 0.0179976224899292, + "Y": -0.003626205725595355, + "Z": -0.709102988243103 + } + } + } + } + ], + "field": { + "length": 6.2402091979980465, + "width": 9.050808906555176 + }, + "_comment": "generated by PractiCal 1.0.4 (7) on 1/29/2026" +} diff --git a/src/main/java/org/team2342/frc/Robot.java b/src/main/java/org/team2342/frc/Robot.java index 56fc0c6..f211f70 100644 --- a/src/main/java/org/team2342/frc/Robot.java +++ b/src/main/java/org/team2342/frc/Robot.java @@ -29,12 +29,14 @@ import org.littletonrobotics.junction.networktables.NT4Publisher; import org.littletonrobotics.junction.wpilog.WPILOGReader; import org.littletonrobotics.junction.wpilog.WPILOGWriter; +// import org.team2342.frc.subsystems.CANdleSystem.CANdleSystem; import org.team2342.frc.util.PhoenixUtils; import org.team2342.lib.logging.ExecutionLogger; public class Robot extends LoggedRobot { private Command autonomousCommand; private static final double loopOverrunWarningTimeout = 0.2; + // private final CANdleSystem candle = new CANdleSystem(null); private final RobotContainer robotContainer; @@ -160,6 +162,7 @@ public void robotPeriodic() { robotContainer.updateAlerts(); ExecutionLogger.log("RobotPeriodic"); + // candle.periodic(); } @Override diff --git a/src/main/java/org/team2342/frc/util/PhoenixUtils.java b/src/main/java/org/team2342/frc/util/PhoenixUtils.java index 13be60b..d49b330 100644 --- a/src/main/java/org/team2342/frc/util/PhoenixUtils.java +++ b/src/main/java/org/team2342/frc/util/PhoenixUtils.java @@ -8,6 +8,8 @@ import com.ctre.phoenix6.BaseStatusSignal; import com.ctre.phoenix6.StatusCode; +import com.ctre.phoenix6.signals.RGBWColor; +import edu.wpi.first.wpilibj.util.Color; import java.util.function.Supplier; public class PhoenixUtils { @@ -47,4 +49,17 @@ public static void refreshSignals() { // Check to make sure there are signals to refresh if (registeredSignals.length > 0) BaseStatusSignal.refreshAll(registeredSignals); } + + /** + * Used to convert {@link edu.wpi.first.wpilibj.util.Color} to {@link + * com.ctre.phoenix6.signals.RGBWColor} + * + * @param c WPILib Color to be converted + */ + public static RGBWColor toCTREColor(Color c) { + if (c == null) { + return new RGBWColor(0, 0, 0, 0); + } + return new RGBWColor((int) (c.red * 255), (int) (c.green * 255), (int) (c.blue * 255), 0); + } } diff --git a/src/main/java/org/team2342/lib/leds/LedIO.java b/src/main/java/org/team2342/lib/leds/LedIO.java index a8f2921..e34ebff 100644 --- a/src/main/java/org/team2342/lib/leds/LedIO.java +++ b/src/main/java/org/team2342/lib/leds/LedIO.java @@ -6,4 +6,37 @@ package org.team2342.lib.leds; -public interface LedIO {} +import edu.wpi.first.wpilibj.util.Color; +import org.littletonrobotics.junction.AutoLog; + +public interface LedIO { + @AutoLog + public static class LedIOInputs { + public Color firstHalfColor = Color.kBlack; + public Color secondHalfColor = Color.kBlack; + public LedEffect firstHalfEffect = LedEffect.OFF; + public LedEffect secondHalfEffect = LedEffect.OFF; + } + + public default void updateInputs(LedIOInputs inputs) {} + + public default void setEffect(Half half, LedEffect effect, Color color) {} + + public enum Half { + FIRST, + SECOND, + // ALL + } + + public enum LedEffect { + SOLID, + FLASHING, + FIRE, + TWINKLE, + COLOR_FLOW, + LARSON, + RGB_FADE, + RAINBOW, + OFF + } +} diff --git a/src/main/java/org/team2342/lib/leds/LedIOCANdle.java b/src/main/java/org/team2342/lib/leds/LedIOCANdle.java index 9375e50..1add339 100644 --- a/src/main/java/org/team2342/lib/leds/LedIOCANdle.java +++ b/src/main/java/org/team2342/lib/leds/LedIOCANdle.java @@ -6,4 +6,179 @@ package org.team2342.lib.leds; -public class LedIOCANdle implements LedIO {} +import com.ctre.phoenix6.CANBus; +import com.ctre.phoenix6.configs.CANdleConfiguration; +import com.ctre.phoenix6.controls.ColorFlowAnimation; +import com.ctre.phoenix6.controls.EmptyAnimation; +import com.ctre.phoenix6.controls.FireAnimation; +import com.ctre.phoenix6.controls.LarsonAnimation; +import com.ctre.phoenix6.controls.RainbowAnimation; +import com.ctre.phoenix6.controls.RgbFadeAnimation; +import com.ctre.phoenix6.controls.SolidColor; +import com.ctre.phoenix6.controls.StrobeAnimation; +import com.ctre.phoenix6.controls.TwinkleAnimation; +import com.ctre.phoenix6.hardware.CANdle; +import com.ctre.phoenix6.signals.Enable5VRailValue; +import com.ctre.phoenix6.signals.StripTypeValue; +import edu.wpi.first.wpilibj.util.Color; +import org.team2342.frc.util.PhoenixUtils; + +public class LedIOCANdle implements LedIO { + private final CANdle candle; + private final int halfLength; + + private final int slot0StartIdx; + private final int slot0EndIdx; + private final int slot1StartIdx; + private final int slot1EndIdx; + private final int candleFirstStart = 0; + private final int canddleFirstEnd = 3; + private final int candleSecondStart = 4; + private final int candleSecondEnd = 7; + private Color firstColor = Color.kBlack; + private Color secondColor = Color.kBlack; + private LedEffect firstEffect = LedEffect.OFF; + private LedEffect secondEffect = LedEffect.OFF; + + public LedIOCANdle(int canId, int ledCount) { + this.candle = new CANdle(canId, new CANBus("rio")); + this.halfLength = ledCount / 2; + slot0StartIdx = 8; + slot0EndIdx = this.halfLength; + slot1StartIdx = slot0EndIdx + 1; + slot1EndIdx = ledCount; + + CANdleConfiguration config = new CANdleConfiguration(); + config.LED.StripType = StripTypeValue.GRB; + config.LED.BrightnessScalar = 0.7; + config.CANdleFeatures.Enable5VRail = Enable5VRailValue.Enabled; + candle.getConfigurator().apply(config); + } + + @Override + public void updateInputs(LedIOInputs inputs) { + inputs.firstHalfColor = firstColor; + inputs.secondHalfColor = secondColor; + inputs.firstHalfEffect = firstEffect; + inputs.secondHalfEffect = secondEffect; + } + + @Override + public void setEffect(Half half, LedEffect effect, Color color) { + if (color == null) { + color = Color.kBlack; + } + switch (half) { + case FIRST -> { + firstColor = color; + firstEffect = effect; + candle.setControl( + new SolidColor(candleFirstStart, canddleFirstEnd) + .withColor(PhoenixUtils.toCTREColor(color))); + switch (effect) { + case SOLID -> { + candle.setControl( + new SolidColor(slot0StartIdx, slot0EndIdx) + .withColor(PhoenixUtils.toCTREColor(color))); + } + case FLASHING -> { + candle.setControl( + new StrobeAnimation(slot0StartIdx, slot0EndIdx) + .withSlot(0) + .withColor(PhoenixUtils.toCTREColor(color))); + } + case FIRE -> { + candle.setControl(new FireAnimation(slot0StartIdx, slot0EndIdx).withSlot(0)); + } + case TWINKLE -> { + candle.setControl( + new TwinkleAnimation(slot0StartIdx, slot0EndIdx) + .withSlot(0) + .withColor(PhoenixUtils.toCTREColor(color))); + } + case COLOR_FLOW -> { + candle.setControl( + new ColorFlowAnimation(slot0StartIdx, slot0EndIdx) + .withSlot(0) + .withColor(PhoenixUtils.toCTREColor(color))); + } + case LARSON -> { + candle.setControl( + new LarsonAnimation(slot0StartIdx, slot0EndIdx) + .withSlot(0) + .withColor(PhoenixUtils.toCTREColor(color))); + } + case RGB_FADE -> { + candle.setControl(new RgbFadeAnimation(slot0StartIdx, slot0EndIdx).withSlot(0)); + } + case RAINBOW -> { + candle.setControl(new RainbowAnimation(slot0StartIdx, slot0EndIdx).withSlot(0)); + } + case OFF -> { + candle.setControl( + new SolidColor(slot0StartIdx, slot0EndIdx) + .withColor(PhoenixUtils.toCTREColor(Color.kBlack))); + } + } + } + case SECOND -> { + secondColor = color; + secondEffect = effect; + candle.setControl( + new SolidColor(candleSecondStart, candleSecondEnd) + .withColor(PhoenixUtils.toCTREColor(color))); + switch (effect) { + case SOLID -> { + candle.setControl( + new SolidColor(slot1StartIdx, slot1EndIdx) + .withColor(PhoenixUtils.toCTREColor(color))); + } + case FLASHING -> { + candle.setControl( + new StrobeAnimation(slot1StartIdx, slot1EndIdx) + .withSlot(1) + .withColor(PhoenixUtils.toCTREColor(color))); + } + case FIRE -> { + candle.setControl(new FireAnimation(slot1StartIdx, slot1EndIdx).withSlot(1)); + } + case TWINKLE -> { + candle.setControl( + new TwinkleAnimation(slot1StartIdx, slot1EndIdx) + .withSlot(1) + .withColor(PhoenixUtils.toCTREColor(color))); + } + case COLOR_FLOW -> { + candle.setControl( + new ColorFlowAnimation(slot1StartIdx, slot1EndIdx) + .withSlot(1) + .withColor(PhoenixUtils.toCTREColor(color))); + } + case LARSON -> { + candle.setControl( + new LarsonAnimation(slot1StartIdx, slot1EndIdx) + .withSlot(1) + .withColor(PhoenixUtils.toCTREColor(color))); + } + case RGB_FADE -> { + candle.setControl(new RgbFadeAnimation(slot1StartIdx, slot1EndIdx).withSlot(1)); + } + case RAINBOW -> { + candle.setControl(new RainbowAnimation(slot1StartIdx, slot1EndIdx).withSlot(1)); + } + case OFF -> { + candle.setControl( + new SolidColor(slot1StartIdx, slot1EndIdx) + .withColor(PhoenixUtils.toCTREColor(Color.kBlack))); + } + } + } + } + } + + public void clearAll() { + for (int i = 0; i < 8; ++i) { + candle.setControl(new EmptyAnimation(i)); + } + } +} diff --git a/src/main/java/org/team2342/lib/leds/LedIOSim.java b/src/main/java/org/team2342/lib/leds/LedIOSim.java new file mode 100644 index 0000000..c78cee9 --- /dev/null +++ b/src/main/java/org/team2342/lib/leds/LedIOSim.java @@ -0,0 +1,48 @@ +// Copyright (c) 2026 Team 2342 +// https://github.com/FRCTeamPhoenix +// +// This source code is licensed under the MIT License. +// See the LICENSE file in the root directory of this project. + +package org.team2342.lib.leds; + +import edu.wpi.first.wpilibj.util.Color; +import org.littletonrobotics.junction.Logger; + +public class LedIOSim implements LedIO { + private Color firstColor = Color.kBlack; + private Color secondColor = Color.kBlack; + private LedEffect firstEffect = LedEffect.OFF; + private LedEffect secondEffect = LedEffect.OFF; + + @Override + public void setEffect(Half half, LedEffect effect, Color color) { + // setColor(half, color); + + switch (half) { + case FIRST: + firstEffect = effect; + break; + case SECOND: + secondEffect = effect; + break; + } + } + + @Override + public void updateInputs(LedIOInputs inputs) { + inputs.firstHalfColor = firstColor; + inputs.secondHalfColor = secondColor; + inputs.firstHalfEffect = firstEffect; + inputs.secondHalfEffect = secondEffect; + + Logger.recordOutput( + "LED/FirstHalf/Color", new double[] {firstColor.red, firstColor.green, firstColor.blue}); + Logger.recordOutput("LED/FirstHalf/Effect", firstEffect.name()); + + Logger.recordOutput( + "LED/SecondHalf/Color", + new double[] {secondColor.red, secondColor.green, secondColor.blue}); + Logger.recordOutput("LED/SecondHalf/Effect", secondEffect.name()); + } +} diff --git a/src/main/java/org/team2342/lib/leds/LedStrip.java b/src/main/java/org/team2342/lib/leds/LedStrip.java new file mode 100644 index 0000000..bdacaaf --- /dev/null +++ b/src/main/java/org/team2342/lib/leds/LedStrip.java @@ -0,0 +1,32 @@ +// Copyright (c) 2026 Team 2342 +// https://github.com/FRCTeamPhoenix +// +// This source code is licensed under the MIT License. +// See the LICENSE file in the root directory of this project. + +package org.team2342.lib.leds; + +import edu.wpi.first.wpilibj.util.Color; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import org.team2342.lib.leds.LedIO.LedEffect; + +public class LedStrip extends SubsystemBase { + private final LedIOCANdle io; + + public LedStrip(LedIOCANdle io, String name) { + this.io = io; + } + + public void setFirst(LedEffect effect, Color color) { + io.setEffect(LedIO.Half.FIRST, effect, color); + } + + public void setSecond(LedEffect effect, Color color) { + io.setEffect(LedIO.Half.SECOND, effect, color); + } + + public void setAll(LedEffect effect, Color color) { + setFirst(effect, color); + setSecond(effect, color); + } +} diff --git a/src/main/java/org/team2342/lib/motors/dumb/DumbMotorIO.java b/src/main/java/org/team2342/lib/motors/dumb/DumbMotorIO.java index 2769f97..0f6679d 100644 --- a/src/main/java/org/team2342/lib/motors/dumb/DumbMotorIO.java +++ b/src/main/java/org/team2342/lib/motors/dumb/DumbMotorIO.java @@ -8,7 +8,16 @@ import org.littletonrobotics.junction.AutoLog; +/* + * Interface for dumb motor input/output + * Lets simulation, real hardware, and different motor types to use same structure + */ + public interface DumbMotorIO { + /** + * Container class for motor inputs - used for logging and data updates. The @AutoLog annotation + * automatically generates code for logging with the AdvantageKit framework. + */ @AutoLog public static class DumbMotorIOInputs { public boolean connected = false; @@ -16,7 +25,18 @@ public static class DumbMotorIOInputs { public double currentAmps = 0.0; } + /** + * Called periodically to update the motor input data. + * + * @param inputs The object that stores motor readings to be logged or used somewhere else. + */ public default void updateInputs(DumbMotorIOInputs inputs) {} + /** + * Runs the motor at a specified voltage. This method is intended to be overridden by + * implementations to control the motor. + * + * @param voltage The voltage to apply to the motor. + */ public default void runVoltage(double voltage) {} } diff --git a/src/main/java/org/team2342/lib/motors/dumb/DumbMotorIOSim.java b/src/main/java/org/team2342/lib/motors/dumb/DumbMotorIOSim.java index 3fc85d5..494911c 100644 --- a/src/main/java/org/team2342/lib/motors/dumb/DumbMotorIOSim.java +++ b/src/main/java/org/team2342/lib/motors/dumb/DumbMotorIOSim.java @@ -11,15 +11,27 @@ import edu.wpi.first.math.system.plant.DCMotor; import edu.wpi.first.wpilibj.simulation.LinearSystemSim; +/** Simulation implementation of DumbMotorIO Uses a LinearSystemSim to simulate */ public class DumbMotorIOSim implements DumbMotorIO { private final LinearSystemSim sim; private final DCMotor motor; + /** + * Constructs a new DumbMotorIOSim instance. + * + * @param motor The DC motor model to simulate + * @param sim The linear system simulation representing the motor's behavior + */ public DumbMotorIOSim(DCMotor motor, LinearSystemSim sim) { this.motor = motor; this.sim = sim; } + /** + * Updates the inputs for the motor controller + * + * @param inputs The inputs object to update with current values + */ @Override public void updateInputs(DumbMotorIOInputs inputs) { sim.update(0.02); @@ -31,6 +43,11 @@ public void updateInputs(DumbMotorIOInputs inputs) { inputs.currentAmps = current; } + /** + * Sets the motor to run at the specified voltage + * + * @param voltage The desired voltage to apply to the motor + */ @Override public void runVoltage(double voltage) { sim.setInput(voltage); diff --git a/src/main/java/org/team2342/lib/motors/dumb/DumbMotorIOTalonFX.java b/src/main/java/org/team2342/lib/motors/dumb/DumbMotorIOTalonFX.java index 8c19f26..a430d84 100644 --- a/src/main/java/org/team2342/lib/motors/dumb/DumbMotorIOTalonFX.java +++ b/src/main/java/org/team2342/lib/motors/dumb/DumbMotorIOTalonFX.java @@ -20,6 +20,10 @@ import org.team2342.frc.util.PhoenixUtils; import org.team2342.lib.motors.MotorConfig; +/** + * Implementation of DumbMotorIO for a TalonFX motor controller Handles configuration, input + * updates, and voltage control for the motor + */ public class DumbMotorIOTalonFX implements DumbMotorIO { private final TalonFX talon; @@ -29,6 +33,12 @@ public class DumbMotorIOTalonFX implements DumbMotorIO { private final VoltageOut voltageRequest = new VoltageOut(0); private final Debouncer connectedDebouncer = new Debouncer(0.5); + /** + * Constructor to configure the TalonFX motor controller + * + * @param canID The CAN ID of the TalonFX motor controller + * @param config The configuration settings for the motor + */ public DumbMotorIOTalonFX(int canID, MotorConfig config) { talon = new TalonFX(canID); @@ -58,6 +68,11 @@ public DumbMotorIOTalonFX(int canID, MotorConfig config) { PhoenixUtils.tryUntilOk(5, () -> ParentDevice.optimizeBusUtilizationForAll(talon)); } + /** + * Updates the inputs for the motor controller + * + * @param inputs The inputs object to update with current values + */ @Override public void updateInputs(DumbMotorIOInputs inputs) { inputs.connected = @@ -66,6 +81,11 @@ public void updateInputs(DumbMotorIOInputs inputs) { inputs.currentAmps = current.getValueAsDouble(); } + /** + * Sets the motor to run at the specified voltage + * + * @param voltage The desired voltage to apply to the motor + */ @Override public void runVoltage(double voltage) { talon.setControl(voltageRequest.withOutput(voltage)); diff --git a/src/main/java/org/team2342/lib/util/AllianceUtils.java b/src/main/java/org/team2342/lib/util/AllianceUtils.java index 18bb4db..9b51186 100644 --- a/src/main/java/org/team2342/lib/util/AllianceUtils.java +++ b/src/main/java/org/team2342/lib/util/AllianceUtils.java @@ -27,6 +27,14 @@ public class AllianceUtils { private static AprilTagFieldLayout fieldLayout = AprilTagFieldLayout.loadField(AprilTagFields.k2026RebuiltAndymark); + public static void loadFieldLayout(String resourcePath) { + try { + fieldLayout = AprilTagFieldLayout.loadFromResource(resourcePath); + } catch (Exception e) { + DriverStation.reportError("Failed to load AprilTagFieldLayout from resource", false); + } + } + public static boolean isRedAlliance() { var alliance = DriverStation.getAlliance();