From fb754bc68ff72b1edaa96bd5d67d3faa2f7da9ba Mon Sep 17 00:00:00 2001 From: Nilesh Agarwalla Date: Wed, 19 Mar 2025 19:42:07 -0400 Subject: [PATCH] Added arm lower limit switch support --- .../config/game/reefscape2025/RobotConfigComp.java | 12 ++++++++++++ 1 file changed, 12 insertions(+) diff --git a/src/main/java/frc/robot/config/game/reefscape2025/RobotConfigComp.java b/src/main/java/frc/robot/config/game/reefscape2025/RobotConfigComp.java index c30bcaf..31aea57 100644 --- a/src/main/java/frc/robot/config/game/reefscape2025/RobotConfigComp.java +++ b/src/main/java/frc/robot/config/game/reefscape2025/RobotConfigComp.java @@ -14,9 +14,12 @@ import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.AddressableLED; import edu.wpi.first.wpilibj.AddressableLEDBuffer; +import edu.wpi.first.wpilibj.DigitalInput; import edu.wpi.first.wpilibj.LEDPattern; import edu.wpi.first.wpilibj.util.Color; import edu.wpi.first.wpilibj.util.Color8Bit; +import edu.wpi.first.wpilibj2.command.InstantCommand; +import edu.wpi.first.wpilibj2.command.button.Trigger; import frc.robot.commands.common.arm.ArmToPosition; import frc.robot.commands.common.elevator.ElevatorToPosition; import frc.robot.io.implementations.motor.MotorIOBase.MotorIOBaseSettings; @@ -39,6 +42,7 @@ public class RobotConfigComp extends RobotConfig { private final AddressableLED led; private final AddressableLEDBuffer ledBuffer; + private final DigitalInput armLowerLimit = new DigitalInput(0); // Normally Open (False) public RobotConfigComp() { super(false, false, false, false, false, true, false); @@ -237,6 +241,14 @@ public RobotConfigComp() { led.start(); } + // Auto Zero Arm Using Limit Switch + Trigger armLowerLimitTrigger = new Trigger(() -> armLowerLimit.get()); + armLowerLimitTrigger.onTrue( + new InstantCommand( + () -> + coralArm.resetEncoder( + Units.degreesToRadians(coralArm.getSettings().minAngleInDegrees)))); + // Auto(s) NamedCommands.registerCommand( "Move Elevator to 0.5 meter", new ElevatorToPosition(elevator, () -> 0.5));