Climb Commands (#20)

Strflightmight09 · web-flow · commit 65daf993bc77 · 2025-09-28T16:14:01.000+03:00
* Climb done except for score negate trigger

* hehehe

* Rename dir

* Fix servos
diff --git a/src/main/java/frc/trigon/robot/RobotContainer.java b/src/main/java/frc/trigon/robot/RobotContainer.java
@@ -12,10 +12,7 @@
 import edu.wpi.first.wpilibj2.command.InstantCommand;
 import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine;
 import frc.trigon.robot.commands.CommandConstants;
-import frc.trigon.robot.commands.commandfactories.CoralCollectionCommands;
-import frc.trigon.robot.commands.commandfactories.CoralEjectionCommands;
-import frc.trigon.robot.commands.commandfactories.CoralPlacingCommands;
-import frc.trigon.robot.commands.commandfactories.GeneralCommands;
+import frc.trigon.robot.commands.commandfactories.*;
 import frc.trigon.robot.constants.CameraConstants;
 import frc.trigon.robot.constants.LEDConstants;
 import frc.trigon.robot.constants.OperatorConstants;
@@ -25,8 +22,8 @@
 import frc.trigon.robot.misc.simulatedfield.SimulationFieldHandler;
 import frc.trigon.robot.poseestimation.poseestimator.PoseEstimator;
 import frc.trigon.robot.subsystems.MotorSubsystem;
-import frc.trigon.robot.subsystems.arm.ArmElevator;
-import frc.trigon.robot.subsystems.arm.ArmElevatorCommands;
+import frc.trigon.robot.subsystems.armelevator.ArmElevator;
+import frc.trigon.robot.subsystems.armelevator.ArmElevatorCommands;
 import frc.trigon.robot.subsystems.climber.Climber;
 import frc.trigon.robot.subsystems.climber.ClimberCommands;
 import frc.trigon.robot.subsystems.climber.ClimberConstants;
@@ -109,6 +106,8 @@ private void bindControllerCommands() {
         OperatorConstants.EJECT_CORAL_TRIGGER.whileTrue(CoralEjectionCommands.getCoralEjectionCommand());
         OperatorConstants.SCORE_CORAL_LEFT_TRIGGER.whileTrue(CoralPlacingCommands.getScoreInReefCommand(false));
         OperatorConstants.SCORE_CORAL_RIGHT_TRIGGER.whileTrue(CoralPlacingCommands.getScoreInReefCommand(true));
+
+        OperatorConstants.CLIMB_TRIGGER.toggleOnTrue(ClimbCommands.getClimbCommand());
     }
 
     private void configureSysIDBindings(MotorSubsystem subsystem) {
diff --git a/src/main/java/frc/trigon/robot/commands/commandfactories/ClimbCommands.java b/src/main/java/frc/trigon/robot/commands/commandfactories/ClimbCommands.java
@@ -5,29 +5,33 @@
 import edu.wpi.first.wpilibj2.command.ParallelCommandGroup;
 import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
 import frc.trigon.robot.RobotContainer;
-import frc.trigon.robot.constants.LEDConstants;
 import frc.trigon.robot.constants.OperatorConstants;
+import frc.trigon.robot.subsystems.armelevator.ArmElevatorCommands;
+import frc.trigon.robot.subsystems.armelevator.ArmElevatorConstants;
 import frc.trigon.robot.subsystems.climber.ClimberCommands;
 import frc.trigon.robot.subsystems.climber.ClimberConstants;
+import frc.trigon.robot.subsystems.intake.IntakeCommands;
+import frc.trigon.robot.subsystems.intake.IntakeConstants;
 import frc.trigon.robot.subsystems.swerve.SwerveCommands;
-import lib.hardware.misc.leds.LEDCommands;
 
 public class ClimbCommands {
-    public static boolean IS_CLIMBING = false;
+    private static boolean IS_CLIMBING = false;//TODO: Make score triggers not work while climbing
 
-    public static Command getClimbCommand() {//TODO: Set other component positions
+    public static Command getClimbCommand() {
         return new SequentialCommandGroup(
                 new InstantCommand(() -> IS_CLIMBING = true),
                 ClimberCommands.getSetTargetStateCommand(ClimberConstants.ClimberState.PREPARE_FOR_CLIMB)
                         .until(() -> RobotContainer.CLIMBER.hasCage() || OperatorConstants.CONTINUE_TRIGGER.getAsBoolean()),
                 ClimberCommands.getSetTargetStateCommand(ClimberConstants.ClimberState.CLIMB)
                         .until(RobotContainer.CLIMBER::atTargetState),
                 getAdjustClimbManuallyCommand()
-        ).alongWith(getClimbLEDCommand()).finallyDo(() -> IS_CLIMBING = false);
+        )
+                .alongWith(getSetSubsystemsToRestForClimbCommand())
+                .finallyDo(() -> IS_CLIMBING = false);
     }
 
-    private static Command getClimbLEDCommand() {
-        return LEDCommands.getAnimateCommand(LEDConstants.CLIMB_ANIMATION_SETTINGS);//TODO: Add LEDStrip
+    public static boolean isClimbing() {
+        return IS_CLIMBING;
     }
 
     private static Command getAdjustClimbManuallyCommand() {
@@ -40,4 +44,11 @@ private static Command getAdjustClimbManuallyCommand() {
                 )
         );
     }
+
+    private static Command getSetSubsystemsToRestForClimbCommand() {
+        return new ParallelCommandGroup(
+                ArmElevatorCommands.getSetTargetStateCommand(ArmElevatorConstants.ArmElevatorState.REST_FOR_CLIMB),
+                IntakeCommands.getSetTargetStateCommand(IntakeConstants.IntakeState.REST_FOR_CLIMB)
+        );
+    }
 }
diff --git a/src/main/java/frc/trigon/robot/commands/commandfactories/CoralCollectionCommands.java b/src/main/java/frc/trigon/robot/commands/commandfactories/CoralCollectionCommands.java
@@ -5,17 +5,15 @@
 import edu.wpi.first.wpilibj2.command.ParallelCommandGroup;
 import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
 import frc.trigon.robot.RobotContainer;
-import frc.trigon.robot.constants.LEDConstants;
 import frc.trigon.robot.constants.OperatorConstants;
-import frc.trigon.robot.subsystems.arm.ArmElevatorCommands;
-import frc.trigon.robot.subsystems.arm.ArmElevatorConstants;
+import frc.trigon.robot.subsystems.armelevator.ArmElevatorCommands;
+import frc.trigon.robot.subsystems.armelevator.ArmElevatorConstants;
 import frc.trigon.robot.subsystems.endeffector.EndEffectorCommands;
 import frc.trigon.robot.subsystems.endeffector.EndEffectorConstants;
 import frc.trigon.robot.subsystems.intake.IntakeCommands;
 import frc.trigon.robot.subsystems.intake.IntakeConstants;
 import frc.trigon.robot.subsystems.transporter.TransporterCommands;
 import frc.trigon.robot.subsystems.transporter.TransporterConstants;
-import lib.hardware.misc.leds.LEDCommands;
 
 public class CoralCollectionCommands {
 
@@ -58,9 +56,6 @@ private static Command getAlignCoralCommand() {
     }
 
     private static Command getCollectionConfirmationCommand() {
-        return new ParallelCommandGroup(
-                new InstantCommand(() -> OperatorConstants.DRIVER_CONTROLLER.rumble(OperatorConstants.RUMBLE_DURATION_SECONDS, OperatorConstants.RUMBLE_POWER)),
-                LEDCommands.getAnimateCommand(LEDConstants.COLLECTION_CONFIRMATION_ANIMATION_SETTINGS) //TODO: add LEDs
-        );
+        return new InstantCommand(() -> OperatorConstants.DRIVER_CONTROLLER.rumble(OperatorConstants.RUMBLE_DURATION_SECONDS, OperatorConstants.RUMBLE_POWER));
     }
 }
diff --git a/src/main/java/frc/trigon/robot/commands/commandfactories/CoralEjectionCommands.java b/src/main/java/frc/trigon/robot/commands/commandfactories/CoralEjectionCommands.java
@@ -5,8 +5,8 @@
 import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
 import frc.trigon.robot.RobotContainer;
 import frc.trigon.robot.misc.simulatedfield.SimulationFieldHandler;
-import frc.trigon.robot.subsystems.arm.ArmElevatorCommands;
-import frc.trigon.robot.subsystems.arm.ArmElevatorConstants;
+import frc.trigon.robot.subsystems.armelevator.ArmElevatorCommands;
+import frc.trigon.robot.subsystems.armelevator.ArmElevatorConstants;
 import frc.trigon.robot.subsystems.endeffector.EndEffectorCommands;
 import frc.trigon.robot.subsystems.endeffector.EndEffectorConstants;
 import frc.trigon.robot.subsystems.intake.IntakeCommands;
diff --git a/src/main/java/frc/trigon/robot/commands/commandfactories/CoralPlacingCommands.java b/src/main/java/frc/trigon/robot/commands/commandfactories/CoralPlacingCommands.java
@@ -13,8 +13,8 @@
 import frc.trigon.robot.constants.FieldConstants;
 import frc.trigon.robot.constants.OperatorConstants;
 import frc.trigon.robot.misc.ReefChooser;
-import frc.trigon.robot.subsystems.arm.ArmElevatorCommands;
-import frc.trigon.robot.subsystems.arm.ArmElevatorConstants;
+import frc.trigon.robot.subsystems.armelevator.ArmElevatorCommands;
+import frc.trigon.robot.subsystems.armelevator.ArmElevatorConstants;
 import frc.trigon.robot.subsystems.endeffector.EndEffectorCommands;
 import frc.trigon.robot.subsystems.endeffector.EndEffectorConstants;
 import frc.trigon.robot.subsystems.swerve.SwerveCommands;
diff --git a/src/main/java/frc/trigon/robot/constants/LEDConstants.java b/src/main/java/frc/trigon/robot/constants/LEDConstants.java
@@ -1,16 +1,7 @@
 package frc.trigon.robot.constants;
 
-import edu.wpi.first.wpilibj.util.Color;
-import lib.hardware.misc.leds.LEDStripAnimationSettings;
-
 public class LEDConstants {
     //TODO: Implement LEDConstants
-    public static LEDStripAnimationSettings.ColorFlowSettings COLLECTION_CONFIRMATION_ANIMATION_SETTINGS = new LEDStripAnimationSettings.ColorFlowSettings(
-            Color.kGreen,
-            0.5,
-            false
-    );
-    public static final LEDStripAnimationSettings.BlinkSettings CLIMB_ANIMATION_SETTINGS = new LEDStripAnimationSettings.BlinkSettings(Color.kYellow, 0.1);//TODO: Add fade animation
 
     /**
      * Initializes LEDConstants. Needed to be called for the LED strips to be initialized before being used.
diff --git a/src/main/java/frc/trigon/robot/constants/OperatorConstants.java b/src/main/java/frc/trigon/robot/constants/OperatorConstants.java
@@ -48,7 +48,8 @@ public class OperatorConstants {
             SPAWN_CORAL_TRIGGER = OPERATOR_CONTROLLER.equals(),
             SCORE_CORAL_LEFT_TRIGGER = DRIVER_CONTROLLER.leftBumper(),
             SCORE_CORAL_RIGHT_TRIGGER = DRIVER_CONTROLLER.rightBumper(),
-            EJECT_CORAL_TRIGGER = DRIVER_CONTROLLER.back().or(OPERATOR_CONTROLLER.e());
+            EJECT_CORAL_TRIGGER = DRIVER_CONTROLLER.back().or(OPERATOR_CONTROLLER.e()),
+            CLIMB_TRIGGER = DRIVER_CONTROLLER.back().or(OPERATOR_CONTROLLER.c());
 
     public static final Trigger
             SET_TARGET_SCORING_REEF_LEVEL_L1_TRIGGER = OPERATOR_CONTROLLER.numpad0().or(DRIVER_CONTROLLER.a()),
diff --git a/src/main/java/frc/trigon/robot/misc/ReefChooser.java b/src/main/java/frc/trigon/robot/misc/ReefChooser.java
@@ -3,11 +3,10 @@
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.InstantCommand;
 import edu.wpi.first.wpilibj2.command.WaitCommand;
-import edu.wpi.first.wpilibj2.command.button.CommandGenericHID;
 import frc.trigon.robot.commands.commandfactories.CoralPlacingCommands;
 import frc.trigon.robot.constants.FieldConstants;
 import frc.trigon.robot.constants.OperatorConstants;
-import frc.trigon.robot.subsystems.arm.ArmElevatorConstants;
+import frc.trigon.robot.subsystems.armelevator.ArmElevatorConstants;
 
 import java.util.function.Supplier;
 
diff --git a/src/main/java/frc/trigon/robot/misc/simulatedfield/SimulationFieldHandler.java b/src/main/java/frc/trigon/robot/misc/simulatedfield/SimulationFieldHandler.java
@@ -3,7 +3,7 @@
 import edu.wpi.first.math.geometry.Pose3d;
 import edu.wpi.first.math.geometry.Transform3d;
 import frc.trigon.robot.RobotContainer;
-import frc.trigon.robot.subsystems.arm.ArmElevatorConstants;
+import frc.trigon.robot.subsystems.armelevator.ArmElevatorConstants;
 import frc.trigon.robot.subsystems.intake.IntakeConstants;
 import frc.trigon.robot.subsystems.transporter.TransporterConstants;
 import lib.utilities.flippable.FlippablePose3d;
diff --git a/src/main/java/frc/trigon/robot/subsystems/armelevator/ArmElevator.java b/src/main/java/frc/trigon/robot/subsystems/armelevator/ArmElevator.java
@@ -1,4 +1,4 @@
-package frc.trigon.robot.subsystems.arm;
+package frc.trigon.robot.subsystems.armelevator;
 
 import com.ctre.phoenix6.controls.DynamicMotionMagicVoltage;
 import com.ctre.phoenix6.controls.VoltageOut;
diff --git a/src/main/java/frc/trigon/robot/subsystems/armelevator/ArmElevatorCommands.java b/src/main/java/frc/trigon/robot/subsystems/armelevator/ArmElevatorCommands.java
@@ -1,4 +1,4 @@
-package frc.trigon.robot.subsystems.arm;
+package frc.trigon.robot.subsystems.armelevator;
 
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.wpilibj2.command.Command;
diff --git a/src/main/java/frc/trigon/robot/subsystems/armelevator/ArmElevatorConstants.java b/src/main/java/frc/trigon/robot/subsystems/armelevator/ArmElevatorConstants.java
@@ -1,4 +1,4 @@
-package frc.trigon.robot.subsystems.arm;
+package frc.trigon.robot.subsystems.armelevator;
 
 import com.ctre.phoenix6.configs.CANcoderConfiguration;
 import com.ctre.phoenix6.configs.TalonFXConfiguration;
@@ -334,7 +334,7 @@ public enum ArmElevatorState {
         REST(Rotation2d.fromDegrees(0), 0.603, null, false, 0.7),
         REST_WITH_CORAL(Rotation2d.fromDegrees(180), 0.603, null, false, 0.7),
         REST_WITH_ALGAE(Rotation2d.fromDegrees(90), 0.603, null, false, 0.7),
-        REST_FOR_CLIMB(Rotation2d.fromDegrees(0), 0.603, null, false, 0.7),
+        REST_FOR_CLIMB(Rotation2d.fromDegrees(180), 0.603, null, false, 0.7),
         LOAD_CORAL(Rotation2d.fromDegrees(0), 0.5519, REST, true, 0.7),
         EJECT(Rotation2d.fromDegrees(60), 0.603, null, false, 0.7),
         SCORE_L1(Rotation2d.fromDegrees(70), 0.4, null, false, 1),
diff --git a/src/main/java/frc/trigon/robot/subsystems/climber/Climber.java b/src/main/java/frc/trigon/robot/subsystems/climber/Climber.java
@@ -101,8 +101,8 @@ void setTargetVoltage(double targetVoltage) {
     }
 
     private void setServoPowers(double power) {
-        rightServo.set(power);
-        leftServo.set(-power);
+        rightServo.setTargetSpeed(power);
+        leftServo.setTargetSpeed(-power);
     }
 
     private Pose3d calculateVisualizationPose() {
diff --git a/src/main/java/frc/trigon/robot/subsystems/climber/ClimberConstants.java b/src/main/java/frc/trigon/robot/subsystems/climber/ClimberConstants.java
@@ -50,11 +50,18 @@ public class ClimberConstants {
     private static final double GEAR_RATIO = 37.5;
     private static final double REVERSE_LIMIT_RESET_POSITION_ROTATIONS = 0;
     private static final double FORWARD_SOFT_LIMIT_POSITION_ROTATIONS = 3;
+    private static final int
+            SERVO_PULSE_WIDTH_MICROSECONDS = 20000,
+            SERVO_MAXIMUM_DEADBAND_RANGE_MICROSECONDS = 0,
+            SERVO_CENTER_PULSE_WIDTH_MICROSECONDS = 1500,
+            SERVO_MINIMUM_DEADBAND_RANGE_MICROSECONDS = 1000,
+            SERVO_MAXIMUM_PULSE_WIDTH_MICROSECONDS = 2000;
+
     static final boolean FOC_ENABLED = true;
 
     private static final int MOTOR_AMOUNT = 1;
     private static final DCMotor
-            GEARBOX = DCMotor.getFalcon500Foc(MOTOR_AMOUNT);//TODO: KrakenX44
+            GEARBOX = DCMotor.getFalcon500Foc(MOTOR_AMOUNT);
     private static final double MOMENT_OF_INERTIA = 0.003;
     private static final SimpleMotorSimulation MOTOR_SIMULATION = new SimpleMotorSimulation(
             GEARBOX,
@@ -96,6 +103,7 @@ public class ClimberConstants {
 
     static {
         configureMotor();
+        configureServos();
         configureReverseLimitSensor();
     }
 
@@ -145,6 +153,23 @@ private static void configureMotor() {
         MOTOR.registerSignal(TalonFXSignal.FORWARD_LIMIT, 100);
     }
 
+    private static void configureServos() {
+        RIGHT_SERVO.setPWMBoundaries(
+                SERVO_PULSE_WIDTH_MICROSECONDS,
+                SERVO_MAXIMUM_DEADBAND_RANGE_MICROSECONDS,
+                SERVO_CENTER_PULSE_WIDTH_MICROSECONDS,
+                SERVO_MINIMUM_DEADBAND_RANGE_MICROSECONDS,
+                SERVO_MAXIMUM_PULSE_WIDTH_MICROSECONDS
+        );
+        LEFT_SERVO.setPWMBoundaries(
+                SERVO_PULSE_WIDTH_MICROSECONDS,
+                SERVO_MAXIMUM_DEADBAND_RANGE_MICROSECONDS,
+                SERVO_CENTER_PULSE_WIDTH_MICROSECONDS,
+                SERVO_MINIMUM_DEADBAND_RANGE_MICROSECONDS,
+                SERVO_MAXIMUM_PULSE_WIDTH_MICROSECONDS
+        );
+    }
+
     private static void configureReverseLimitSensor() {
         REVERSE_LIMIT_SENSOR.setSimulationSupplier(REVERSE_LIMIT_SENSORS_SIMULATION_SUPPLIER);
         REVERSE_LIMIT_SENSOR_BOOLEAN_EVENT.ifHigh(() -> MOTOR.setPosition(REVERSE_LIMIT_RESET_POSITION_ROTATIONS));
diff --git a/src/main/java/frc/trigon/robot/subsystems/intake/IntakeConstants.java b/src/main/java/frc/trigon/robot/subsystems/intake/IntakeConstants.java
@@ -22,7 +22,6 @@
 import lib.hardware.phoenix6.talonfx.TalonFXSignal;
 import lib.hardware.simulation.SimpleMotorSimulation;
 import lib.hardware.simulation.SingleJointedArmSimulation;
-import lib.utilities.Conversions;
 import lib.utilities.mechanisms.SingleJointedArmMechanism2d;
 import lib.utilities.mechanisms.SpeedMechanism2d;
 
@@ -219,6 +218,7 @@ private static void configureDistanceSensor() {
 
     public enum IntakeState {
         REST(0, MINIMUM_ANGLE),
+        REST_FOR_CLIMB(0, MAXIMUM_ANGLE),
         COLLECT(5, MAXIMUM_ANGLE),
         EJECT(-5, MAXIMUM_ANGLE);
 

Original file line number	Diff line number	Diff line change
`@@ -1,4 +1,4 @@`
`1`		`-package frc.trigon.robot.subsystems.arm;`
	`1`	`+package frc.trigon.robot.subsystems.armelevator;`
`2`	`2`
`3`	`3`	`import com.ctre.phoenix6.controls.DynamicMotionMagicVoltage;`
`4`	`4`	`import com.ctre.phoenix6.controls.VoltageOut;`