From bf8842dffa245977876abf014e3090d77dc43309 Mon Sep 17 00:00:00 2001 From: Strflightmight09 <148347057+Strflightmight09@users.noreply.github.com> Date: Thu, 25 Sep 2025 23:11:08 +0300 Subject: [PATCH 1/4] Climb done except for score negate trigger --- .../java/frc/trigon/robot/RobotContainer.java | 7 +++---- .../commandfactories/ClimbCommands.java | 20 ++++++++++++++++--- .../robot/constants/OperatorConstants.java | 3 ++- .../robot/subsystems/arm/ArmConstants.java | 2 +- .../elevator/ElevatorConstants.java | 9 +++++---- .../subsystems/intake/IntakeConstants.java | 2 +- 6 files changed, 29 insertions(+), 14 deletions(-) diff --git a/src/main/java/frc/trigon/robot/RobotContainer.java b/src/main/java/frc/trigon/robot/RobotContainer.java index 2265ac21..0b4a48bc 100644 --- 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; @@ -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 index 947caac6..af7991be 100644 --- a/src/main/java/frc/trigon/robot/commands/commandfactories/ClimbCommands.java +++ b/src/main/java/frc/trigon/robot/commands/commandfactories/ClimbCommands.java @@ -7,13 +7,19 @@ import frc.trigon.robot.RobotContainer; import frc.trigon.robot.constants.LEDConstants; import frc.trigon.robot.constants.OperatorConstants; +import frc.trigon.robot.subsystems.arm.ArmCommands; +import frc.trigon.robot.subsystems.arm.ArmConstants; import frc.trigon.robot.subsystems.climber.ClimberCommands; import frc.trigon.robot.subsystems.climber.ClimberConstants; +import frc.trigon.robot.subsystems.elevator.ElevatorCommands; +import frc.trigon.robot.subsystems.elevator.ElevatorConstants; +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; + public static boolean IS_CLIMBING = false;//TODO: Make score triggers not work while climbing public static Command getClimbCommand() {//TODO: Set other component positions return new SequentialCommandGroup( @@ -23,11 +29,11 @@ public static Command getClimbCommand() {//TODO: Set other component positions ClimberCommands.getSetTargetStateCommand(ClimberConstants.ClimberState.CLIMB) .until(RobotContainer.CLIMBER::atTargetState), getAdjustClimbManuallyCommand() - ).alongWith(getClimbLEDCommand()).finallyDo(() -> IS_CLIMBING = false); + ).alongWith(getSetSubsystemsToRestForClimbCommand(), getClimbLEDCommand()).finallyDo(() -> IS_CLIMBING = false); } private static Command getClimbLEDCommand() { - return LEDCommands.getAnimateCommand(LEDConstants.CLIMB_ANIMATION_SETTINGS);//TODO: Add LEDStrip + return LEDCommands.getAnimateCommand(LEDConstants.CLIMB_ANIMATION_SETTINGS); } private static Command getAdjustClimbManuallyCommand() { @@ -40,4 +46,12 @@ private static Command getAdjustClimbManuallyCommand() { ) ); } + + private static Command getSetSubsystemsToRestForClimbCommand() { + return new ParallelCommandGroup( + ArmCommands.getSetTargetStateCommand(ArmConstants.ArmState.REST_FOR_CLIMB), + ElevatorCommands.getSetTargetStateCommand(ElevatorConstants.ElevatorState.REST_FOR_CLIMB), + IntakeCommands.getSetTargetStateCommand(IntakeConstants.IntakeState.REST_FOR_CLIMB) + ); + } } diff --git a/src/main/java/frc/trigon/robot/constants/OperatorConstants.java b/src/main/java/frc/trigon/robot/constants/OperatorConstants.java index 147f64e6..d1303510 100644 --- 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/subsystems/arm/ArmConstants.java b/src/main/java/frc/trigon/robot/subsystems/arm/ArmConstants.java index 53396fd4..e2e7938e 100644 --- a/src/main/java/frc/trigon/robot/subsystems/arm/ArmConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/arm/ArmConstants.java @@ -237,7 +237,7 @@ private static void configureDistanceSensor() { public enum ArmState { REST(Rotation2d.fromDegrees(0), Rotation2d.fromDegrees(0), 0), REST_WITH_CORAL(Rotation2d.fromDegrees(180), Rotation2d.fromDegrees(180), 0), - REST_FOR_CLIMB(Rotation2d.fromDegrees(0), Rotation2d.fromDegrees(0), 0), + REST_FOR_CLIMB(Rotation2d.fromDegrees(120), Rotation2d.fromDegrees(120), 0), LOAD_CORAL(Rotation2d.fromDegrees(0), Rotation2d.fromDegrees(0), -4), HOLD_ALGAE(Rotation2d.fromDegrees(90), Rotation2d.fromDegrees(90), -4), EJECT(Rotation2d.fromDegrees(60), Rotation2d.fromDegrees(60), 4), diff --git a/src/main/java/frc/trigon/robot/subsystems/elevator/ElevatorConstants.java b/src/main/java/frc/trigon/robot/subsystems/elevator/ElevatorConstants.java index afff2a4f..7a094a34 100644 --- a/src/main/java/frc/trigon/robot/subsystems/elevator/ElevatorConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/elevator/ElevatorConstants.java @@ -174,15 +174,16 @@ private static void configureReverseLimitSensor() { public enum ElevatorState { REST(0.603, 0.603, 0.7), + REST_WITH_ALGAE(0.603, 0.603, 0.3), + REST_FOR_CLIMB(0, 0, 0.5), LOAD_CORAL(0.5519, 0.5519, 0.7), SCORE_L1(0.1, 0.1, 1), SCORE_L2(0.3, 0.4, 1), SCORE_L3(0.7, 0.8, 1), SCORE_L4(1.2, 1.3, 1), - COLLECT_ALGAE_FROM_L2(0.603, 0.603, 1), - COLLECT_ALGAE_FROM_L3(0.953, 0.953, 1), - COLLECT_ALGAE_FROM_GROUND(0, 0, 0.7), - REST_WITH_ALGAE(0.603, 0.603, 0.3), + COLLECT_ALGAE_FROM_L2(0.603, 0.603, 0.7), + COLLECT_ALGAE_FROM_L3(0.953, 0.953, 0.7), + COLLECT_ALGAE_FROM_GROUND(0, 0, 1), SCORE_NET(1.382, 1.382, 0.3); public final double targetPositionMeters; diff --git a/src/main/java/frc/trigon/robot/subsystems/intake/IntakeConstants.java b/src/main/java/frc/trigon/robot/subsystems/intake/IntakeConstants.java index 9113d59d..b7d1209d 100644 --- 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); From efc58d4655686a7e81ea677fee182e0576234f9b Mon Sep 17 00:00:00 2001 From: Strflightmight09 <148347057+Strflightmight09@users.noreply.github.com> Date: Sun, 28 Sep 2025 01:37:41 +0300 Subject: [PATCH 2/4] hehehe --- .../commands/commandfactories/ClimbCommands.java | 14 +++++++------- .../commandfactories/CoralCollectionCommands.java | 7 +------ .../frc/trigon/robot/constants/LEDConstants.java | 9 --------- 3 files changed, 8 insertions(+), 22 deletions(-) diff --git a/src/main/java/frc/trigon/robot/commands/commandfactories/ClimbCommands.java b/src/main/java/frc/trigon/robot/commands/commandfactories/ClimbCommands.java index af7991be..48be229a 100644 --- a/src/main/java/frc/trigon/robot/commands/commandfactories/ClimbCommands.java +++ b/src/main/java/frc/trigon/robot/commands/commandfactories/ClimbCommands.java @@ -5,7 +5,6 @@ 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.ArmCommands; import frc.trigon.robot.subsystems.arm.ArmConstants; @@ -16,12 +15,11 @@ 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;//TODO: Make score triggers not work while climbing + 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) @@ -29,11 +27,13 @@ public static Command getClimbCommand() {//TODO: Set other component positions ClimberCommands.getSetTargetStateCommand(ClimberConstants.ClimberState.CLIMB) .until(RobotContainer.CLIMBER::atTargetState), getAdjustClimbManuallyCommand() - ).alongWith(getSetSubsystemsToRestForClimbCommand(), getClimbLEDCommand()).finallyDo(() -> IS_CLIMBING = false); + ) + .alongWith(getSetSubsystemsToRestForClimbCommand()) + .finallyDo(() -> IS_CLIMBING = false); } - private static Command getClimbLEDCommand() { - return LEDCommands.getAnimateCommand(LEDConstants.CLIMB_ANIMATION_SETTINGS); + public static boolean isClimbing() { + return IS_CLIMBING; } private static Command getAdjustClimbManuallyCommand() { diff --git a/src/main/java/frc/trigon/robot/commands/commandfactories/CoralCollectionCommands.java b/src/main/java/frc/trigon/robot/commands/commandfactories/CoralCollectionCommands.java index f4d70ecb..8e56d873 100644 --- a/src/main/java/frc/trigon/robot/commands/commandfactories/CoralCollectionCommands.java +++ b/src/main/java/frc/trigon/robot/commands/commandfactories/CoralCollectionCommands.java @@ -5,7 +5,6 @@ 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.ArmCommands; import frc.trigon.robot.subsystems.arm.ArmConstants; @@ -15,7 +14,6 @@ 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)); } } \ No newline at end of file diff --git a/src/main/java/frc/trigon/robot/constants/LEDConstants.java b/src/main/java/frc/trigon/robot/constants/LEDConstants.java index 3d137879..3fea33ed 100644 --- 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. From 079adea66e420d29a80160a7b4d50803860664c4 Mon Sep 17 00:00:00 2001 From: Strflightmight09 <148347057+Strflightmight09@users.noreply.github.com> Date: Sun, 28 Sep 2025 12:13:13 +0300 Subject: [PATCH 3/4] Rename dir --- src/main/java/frc/trigon/robot/RobotContainer.java | 4 ++-- .../robot/commands/commandfactories/ClimbCommands.java | 9 +++------ .../commandfactories/CoralCollectionCommands.java | 4 ++-- .../commands/commandfactories/CoralEjectionCommands.java | 4 ++-- .../commands/commandfactories/CoralPlacingCommands.java | 4 ++-- src/main/java/frc/trigon/robot/misc/ReefChooser.java | 3 +-- .../misc/simulatedfield/SimulationFieldHandler.java | 2 +- .../trigon/robot/subsystems/armelevator/ArmElevator.java | 2 +- .../subsystems/armelevator/ArmElevatorCommands.java | 2 +- .../subsystems/armelevator/ArmElevatorConstants.java | 4 ++-- 10 files changed, 17 insertions(+), 21 deletions(-) diff --git a/src/main/java/frc/trigon/robot/RobotContainer.java b/src/main/java/frc/trigon/robot/RobotContainer.java index feba67f7..2f7bb6fc 100644 --- a/src/main/java/frc/trigon/robot/RobotContainer.java +++ b/src/main/java/frc/trigon/robot/RobotContainer.java @@ -22,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; diff --git a/src/main/java/frc/trigon/robot/commands/commandfactories/ClimbCommands.java b/src/main/java/frc/trigon/robot/commands/commandfactories/ClimbCommands.java index 48be229a..ff9cd032 100644 --- a/src/main/java/frc/trigon/robot/commands/commandfactories/ClimbCommands.java +++ b/src/main/java/frc/trigon/robot/commands/commandfactories/ClimbCommands.java @@ -6,12 +6,10 @@ import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import frc.trigon.robot.RobotContainer; import frc.trigon.robot.constants.OperatorConstants; -import frc.trigon.robot.subsystems.arm.ArmCommands; -import frc.trigon.robot.subsystems.arm.ArmConstants; +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.elevator.ElevatorCommands; -import frc.trigon.robot.subsystems.elevator.ElevatorConstants; import frc.trigon.robot.subsystems.intake.IntakeCommands; import frc.trigon.robot.subsystems.intake.IntakeConstants; import frc.trigon.robot.subsystems.swerve.SwerveCommands; @@ -49,8 +47,7 @@ private static Command getAdjustClimbManuallyCommand() { private static Command getSetSubsystemsToRestForClimbCommand() { return new ParallelCommandGroup( - ArmCommands.getSetTargetStateCommand(ArmConstants.ArmState.REST_FOR_CLIMB), - ElevatorCommands.getSetTargetStateCommand(ElevatorConstants.ElevatorState.REST_FOR_CLIMB), + 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 index 8f7861eb..d8a2f92a 100644 --- a/src/main/java/frc/trigon/robot/commands/commandfactories/CoralCollectionCommands.java +++ b/src/main/java/frc/trigon/robot/commands/commandfactories/CoralCollectionCommands.java @@ -6,8 +6,8 @@ import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import frc.trigon.robot.RobotContainer; 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; diff --git a/src/main/java/frc/trigon/robot/commands/commandfactories/CoralEjectionCommands.java b/src/main/java/frc/trigon/robot/commands/commandfactories/CoralEjectionCommands.java index b0a9a4ed..2287965d 100644 --- 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 index 29ff5e76..583c52e5 100644 --- 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/misc/ReefChooser.java b/src/main/java/frc/trigon/robot/misc/ReefChooser.java index 3d45a603..aebe0f54 100644 --- 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 index 4a4972f6..a07b61b6 100644 --- 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 index 73b5fd78..567e26e9 100644 --- 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 index 87a0655d..1363f515 100644 --- 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 index ee976ede..8a9efe3d 100644 --- 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), From 48e94d97492bb74cb818a59a250771c2532ab2c5 Mon Sep 17 00:00:00 2001 From: Strflightmight09 <148347057+Strflightmight09@users.noreply.github.com> Date: Sun, 28 Sep 2025 14:04:47 +0300 Subject: [PATCH 4/4] Fix servos --- .../robot/subsystems/climber/Climber.java | 4 +-- .../subsystems/climber/ClimberConstants.java | 27 ++++++++++++++++++- 2 files changed, 28 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/trigon/robot/subsystems/climber/Climber.java b/src/main/java/frc/trigon/robot/subsystems/climber/Climber.java index c4b7129a..c112dbd0 100644 --- 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 index d1d9c6e4..9d2e6996 100644 --- 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));