From 3ab35d6b3b7f9fcb726eb0d0f102795ffba30e30 Mon Sep 17 00:00:00 2001 From: ShmayaR Date: Sun, 18 Jan 2026 21:21:28 +0200 Subject: [PATCH 01/12] initial commit --- .../java/frc/trigon/robot/RobotContainer.java | 2 + .../robot/subsystems/climber/Climber.java | 134 +++++++++++++++ .../subsystems/climber/ClimberCommands.java | 43 +++++ .../subsystems/climber/ClimberConstants.java | 162 ++++++++++++++++++ 4 files changed, 341 insertions(+) create mode 100644 src/main/java/frc/trigon/robot/subsystems/climber/Climber.java create mode 100644 src/main/java/frc/trigon/robot/subsystems/climber/ClimberCommands.java create mode 100644 src/main/java/frc/trigon/robot/subsystems/climber/ClimberConstants.java diff --git a/src/main/java/frc/trigon/robot/RobotContainer.java b/src/main/java/frc/trigon/robot/RobotContainer.java index 80cf8d04..6b57011c 100644 --- a/src/main/java/frc/trigon/robot/RobotContainer.java +++ b/src/main/java/frc/trigon/robot/RobotContainer.java @@ -20,6 +20,7 @@ import frc.trigon.robot.misc.simulatedfield.SimulatedGamePieceConstants; import frc.trigon.robot.poseestimation.robotposeestimator.RobotPoseEstimator; import frc.trigon.robot.subsystems.MotorSubsystem; +import frc.trigon.robot.subsystems.climber.Climber; import frc.trigon.robot.subsystems.shooter.Shooter; import frc.trigon.robot.subsystems.shooter.ShooterCommands; import frc.trigon.robot.subsystems.swerve.Swerve; @@ -33,6 +34,7 @@ public class RobotContainer { CameraConstants.OBJECT_DETECTION_CAMERA ); public static final Swerve SWERVE = new Swerve(); + public static final Climber CLIMBER = new Climber(); public static final Shooter SHOOTER = new Shooter(); private LoggedDashboardChooser autoChooser; diff --git a/src/main/java/frc/trigon/robot/subsystems/climber/Climber.java b/src/main/java/frc/trigon/robot/subsystems/climber/Climber.java new file mode 100644 index 00000000..955b9dbf --- /dev/null +++ b/src/main/java/frc/trigon/robot/subsystems/climber/Climber.java @@ -0,0 +1,134 @@ +package frc.trigon.robot.subsystems.climber; + +import com.ctre.phoenix6.controls.DynamicMotionMagicVoltage; +import com.ctre.phoenix6.controls.VoltageOut; +import edu.wpi.first.math.geometry.Pose3d; +import edu.wpi.first.math.geometry.Rotation3d; +import edu.wpi.first.math.geometry.Transform3d; +import edu.wpi.first.math.geometry.Translation3d; +import edu.wpi.first.units.Units; +import edu.wpi.first.wpilibj.sysid.SysIdRoutineLog; +import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; +import frc.trigon.lib.hardware.phoenix6.talonfx.TalonFXMotor; +import frc.trigon.lib.hardware.phoenix6.talonfx.TalonFXSignal; +import frc.trigon.lib.utilities.Conversions; +import frc.trigon.robot.subsystems.MotorSubsystem; +import org.littletonrobotics.junction.AutoLogOutput; +import org.littletonrobotics.junction.Logger; + +public class Climber extends MotorSubsystem { + private final TalonFXMotor + masterMotor = ClimberConstants.MASTER_MOTOR, + followerMotor = ClimberConstants.FOLLOWER_MOTOR; + private final VoltageOut voltageRequest = new VoltageOut(0).withEnableFOC(ClimberConstants.FOC_ENABLED); + private final DynamicMotionMagicVoltage positionRequest = new DynamicMotionMagicVoltage(0, ClimberConstants.DEFAULT_MAXIMUM_VELOCITY, ClimberConstants.DEFAULT_MAXIMUM_ACCELERATION).withEnableFOC(ClimberConstants.FOC_ENABLED); + private ClimberConstants.ClimberState targetState = ClimberConstants.ClimberState.REST; + + public Climber() { + setName("Climber"); + } + + @Override + public SysIdRoutine.Config getSysIDConfig() { + return ClimberConstants.SYSID_CONFIG; + } + + @Override + public void setBrake(boolean brake) { + masterMotor.setBrake(brake); + followerMotor.setBrake(brake); + } + + @Override + public void stop() { + masterMotor.stopMotor(); + } + + @Override + public void updateLog(SysIdRoutineLog log) { + log.motor("Climber") + .linearPosition(Units.Meters.of(getPositionRotations())) + .linearVelocity(Units.MetersPerSecond.of(masterMotor.getSignal(TalonFXSignal.VELOCITY))) + .voltage(Units.Volts.of(masterMotor.getSignal(TalonFXSignal.MOTOR_VOLTAGE))); + } + + @Override + public void updateMechanism() { + Logger.recordOutput("Poses/Components/ClimberPose", calculateComponentPose()); + + ClimberConstants.MECHANISM.update( + getPositionMeters(), + rotationsToMeters(masterMotor.getSignal(TalonFXSignal.CLOSED_LOOP_REFERENCE)) + ); + } + + @Override + public void updatePeriodically() { + masterMotor.update(); + Logger.recordOutput("Elevator/CurrentPositionMeters", getPositionMeters()); + } + + @Override + public void sysIDDrive(double targetVoltage) { + masterMotor.setControl(voltageRequest.withOutput(targetVoltage)); + } + + public boolean atExtendedState(ClimberConstants.ClimberState targetState) { + return targetState == this.targetState && atTargetExtendedState(); + } + + @AutoLogOutput(key = "Elevator/AtTargetState") + public boolean atTargetExtendedState() { + return calculateTargetExtendedStateDistance() < ClimberConstants.POSITION_TOLERANCE_METERS; + } + + private double calculateTargetExtendedStateDistance() { + return Math.abs(targetState.targetExtendedPositionMeters - getPositionMeters()); + } + + public double metersToRotations(double positionMeters) { + return Conversions.distanceToRotations(positionMeters, ClimberConstants.DRUM_DIAMETER_METERS); + } + + void setTargetExtendedState(ClimberConstants.ClimberState targetState) { + this.targetState = targetState; + scalePositionRequestSpeed(targetState.speedScalar); + setTargetPositionRotations(metersToRotations(targetState.targetExtendedPositionMeters)); + } + + void setTargetRetractedState(ClimberConstants.ClimberState targetState) { + if (atExtendedState(targetState)) + setTargetPositionRotations(metersToRotations(targetState.targetRetractedPositionMeters)); + } + + private void scalePositionRequestSpeed(double speedScalar) { + positionRequest.Velocity = ClimberConstants.DEFAULT_MAXIMUM_VELOCITY * speedScalar; + positionRequest.Acceleration = ClimberConstants.DEFAULT_MAXIMUM_ACCELERATION * speedScalar; + positionRequest.Jerk = positionRequest.Acceleration * 10; + } + + void setTargetPositionRotations(double targetPositionRotations) { + masterMotor.setControl(positionRequest.withPosition(targetPositionRotations)); + } + + private Pose3d calculateComponentPose() { + return ClimberConstants.FIRST_STAGE_VISUALIZATION_ORIGIN_POINT.transformBy( + new Transform3d( + new Translation3d(0, 0, getPositionMeters()), + new Rotation3d() + ) + ); + } + + private double getPositionRotations() { + return masterMotor.getSignal(TalonFXSignal.POSITION); + } + + private double getPositionMeters() { + return rotationsToMeters(getPositionRotations()); + } + + private double rotationsToMeters(double positionRotations) { + return Conversions.rotationsToDistance(positionRotations, ClimberConstants.DRUM_DIAMETER_METERS); + } +} \ No newline at end of file diff --git a/src/main/java/frc/trigon/robot/subsystems/climber/ClimberCommands.java b/src/main/java/frc/trigon/robot/subsystems/climber/ClimberCommands.java new file mode 100644 index 00000000..65822e15 --- /dev/null +++ b/src/main/java/frc/trigon/robot/subsystems/climber/ClimberCommands.java @@ -0,0 +1,43 @@ +package frc.trigon.robot.subsystems.climber; + +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.StartEndCommand; +import frc.trigon.lib.commands.NetworkTablesCommand; +import frc.trigon.robot.RobotContainer; + +import java.util.Set; + +public class ClimberCommands { + public static Command getDebuggingCommand() { + return new NetworkTablesCommand( + RobotContainer.CLIMBER::setTargetPositionRotations, + false, + Set.of(RobotContainer.CLIMBER), + "Debugging/ElevatorTargetPositionRotations" + ); + } + + public static Command getSetTargetVoltageCommand(double targetVoltage) { + return new StartEndCommand( + () -> RobotContainer.CLIMBER.sysIDDrive(targetVoltage), + RobotContainer.CLIMBER::stop, + RobotContainer.CLIMBER + ); + } + + public static Command getSetTargetExtendedStateCommand(ClimberConstants.ClimberState targetState) { + return new StartEndCommand( + () -> RobotContainer.CLIMBER.setTargetExtendedState(targetState), + RobotContainer.CLIMBER::stop, + RobotContainer.CLIMBER + ); + } + + public static Command getSetTargetRetractedStateCommand(ClimberConstants.ClimberState targetState) { + return new StartEndCommand( + () -> RobotContainer.CLIMBER.setTargetRetractedState(targetState), + RobotContainer.CLIMBER::stop, + RobotContainer.CLIMBER + ); + } +} \ No newline at end of file diff --git a/src/main/java/frc/trigon/robot/subsystems/climber/ClimberConstants.java b/src/main/java/frc/trigon/robot/subsystems/climber/ClimberConstants.java new file mode 100644 index 00000000..61c10ec0 --- /dev/null +++ b/src/main/java/frc/trigon/robot/subsystems/climber/ClimberConstants.java @@ -0,0 +1,162 @@ +package frc.trigon.robot.subsystems.climber; + +import com.ctre.phoenix6.configs.TalonFXConfiguration; +import com.ctre.phoenix6.controls.Follower; +import com.ctre.phoenix6.signals.*; +import edu.wpi.first.math.geometry.Pose3d; +import edu.wpi.first.math.geometry.Rotation3d; +import edu.wpi.first.math.geometry.Translation3d; +import edu.wpi.first.math.system.plant.DCMotor; +import edu.wpi.first.units.Units; +import edu.wpi.first.wpilibj.util.Color; +import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; +import frc.trigon.lib.hardware.RobotHardwareStats; +import frc.trigon.lib.hardware.phoenix6.talonfx.TalonFXMotor; +import frc.trigon.lib.hardware.phoenix6.talonfx.TalonFXSignal; +import frc.trigon.lib.hardware.simulation.ElevatorSimulation; +import frc.trigon.lib.utilities.mechanisms.ElevatorMechanism2d; + +public class ClimberConstants { + private static final int + MASTER_MOTOR_ID = 19, + FOLLOWER_MOTOR_ID = 20; + private static final String + MASTER_MOTOR_NAME = "ClimberMasterMotor", + FOLLOWER_MOTOR_NAME = "ClimberFollowerMotor"; + static final TalonFXMotor + MASTER_MOTOR = new TalonFXMotor(MASTER_MOTOR_ID, MASTER_MOTOR_NAME), + FOLLOWER_MOTOR = new TalonFXMotor(FOLLOWER_MOTOR_ID, FOLLOWER_MOTOR_NAME); + + private static final double GEAR_RATIO = 5; + private static final MotorAlignmentValue SHOULD_FOLLOWER_OPPOSE_MASTER = MotorAlignmentValue.Aligned; + static final double + DEFAULT_MAXIMUM_VELOCITY = RobotHardwareStats.isSimulation() ? 80 : 20, + DEFAULT_MAXIMUM_ACCELERATION = RobotHardwareStats.isSimulation() ? 80 : 50; + static final boolean FOC_ENABLED = true; + + private static final int MOTOR_AMOUNT = 2; + private static final DCMotor GEARBOX = DCMotor.getKrakenX60(MOTOR_AMOUNT); + private static final double + CLIMBER_MASS_KILOGRAMS = 8, + DRUM_RADIUS_METERS = 0.025, + MINIMUM_CLIMBER_HEIGHT_METERS = 0.73, + MAXIMUM_CLIMBER_HEIGHT_METERS = 1.8; + private static final boolean SHOULD_SIMULATE_GRAVITY = true; + private static final ElevatorSimulation SIMULATION = new ElevatorSimulation( + GEARBOX, + GEAR_RATIO, + CLIMBER_MASS_KILOGRAMS, + DRUM_RADIUS_METERS, + MINIMUM_CLIMBER_HEIGHT_METERS, + MAXIMUM_CLIMBER_HEIGHT_METERS, + SHOULD_SIMULATE_GRAVITY + ); + + static final SysIdRoutine.Config SYSID_CONFIG = new SysIdRoutine.Config( + Units.Volts.of(1.25).per(Units.Seconds), + Units.Volts.of(3), + Units.Second.of(1000) + ); + + public static final Pose3d FIRST_STAGE_VISUALIZATION_ORIGIN_POINT = new Pose3d( + new Translation3d(0, 0, 0), + new Rotation3d(0, 0, 0) + ); + + static final ElevatorMechanism2d MECHANISM = new ElevatorMechanism2d( + "ClimberMechanism", + MAXIMUM_CLIMBER_HEIGHT_METERS, + MINIMUM_CLIMBER_HEIGHT_METERS, + Color.kYellow + ); + + static final double DRUM_DIAMETER_METERS = DRUM_RADIUS_METERS * 2; + static final double POSITION_TOLERANCE_METERS = 0.07; + + static { + configureMasterMotor(); + configureFollowerMotor(); + } + + private static void configureMasterMotor() { + final TalonFXConfiguration config = new TalonFXConfiguration(); + + config.Audio.BeepOnBoot = false; + config.Audio.BeepOnConfig = false; + + config.MotorOutput.NeutralMode = NeutralModeValue.Brake; + config.MotorOutput.Inverted = InvertedValue.Clockwise_Positive; + config.Feedback.SensorToMechanismRatio = GEAR_RATIO; + + config.Slot0.kP = RobotHardwareStats.isSimulation() ? 40 : 6.5905; + config.Slot0.kI = RobotHardwareStats.isSimulation() ? 0 : 0; + config.Slot0.kD = RobotHardwareStats.isSimulation() ? 0.22774 : 0.2; + config.Slot0.kS = RobotHardwareStats.isSimulation() ? 0.066659 : 0.25513; + config.Slot0.kV = RobotHardwareStats.isSimulation() ? 0.74502 : 0.52756; + config.Slot0.kA = RobotHardwareStats.isSimulation() ? 0 : 0; + config.Slot0.kG = RobotHardwareStats.isSimulation() ? 0.30539 : 0.4; + + config.Slot0.GravityType = GravityTypeValue.Elevator_Static; + config.Slot0.StaticFeedforwardSign = StaticFeedforwardSignValue.UseVelocitySign; + + config.HardwareLimitSwitch.ReverseLimitEnable = true; + config.HardwareLimitSwitch.ReverseLimitType = ReverseLimitTypeValue.NormallyOpen; + config.HardwareLimitSwitch.ReverseLimitSource = ReverseLimitSourceValue.LimitSwitchPin; + config.HardwareLimitSwitch.ReverseLimitAutosetPositionEnable = true; + config.HardwareLimitSwitch.ReverseLimitAutosetPositionValue = 0; + + config.HardwareLimitSwitch.ForwardLimitEnable = false; + + config.SoftwareLimitSwitch.ReverseSoftLimitEnable = true; + config.SoftwareLimitSwitch.ReverseSoftLimitThreshold = 0; + + config.SoftwareLimitSwitch.ForwardSoftLimitEnable = true; + config.SoftwareLimitSwitch.ForwardSoftLimitThreshold = 6.8; + + config.MotionMagic.MotionMagicCruiseVelocity = DEFAULT_MAXIMUM_VELOCITY; + config.MotionMagic.MotionMagicAcceleration = DEFAULT_MAXIMUM_ACCELERATION; + config.MotionMagic.MotionMagicJerk = config.MotionMagic.MotionMagicAcceleration * 10; + + MASTER_MOTOR.applyConfiguration(config); + MASTER_MOTOR.setPhysicsSimulation(SIMULATION); + + MASTER_MOTOR.registerSignal(TalonFXSignal.POSITION, 100); + MASTER_MOTOR.registerSignal(TalonFXSignal.VELOCITY, 100); + MASTER_MOTOR.registerSignal(TalonFXSignal.MOTOR_VOLTAGE, 100); + MASTER_MOTOR.registerSignal(TalonFXSignal.CLOSED_LOOP_REFERENCE, 100); + MASTER_MOTOR.registerSignal(TalonFXSignal.STATOR_CURRENT, 100); + } + + private static void configureFollowerMotor() { + final TalonFXConfiguration config = new TalonFXConfiguration(); + + config.Audio.BeepOnBoot = false; + config.Audio.BeepOnConfig = false; + + config.MotorOutput.NeutralMode = NeutralModeValue.Brake; + config.MotorOutput.Inverted = InvertedValue.CounterClockwise_Positive; + + FOLLOWER_MOTOR.applyConfiguration(config); + + final Follower followerRequest = new Follower(MASTER_MOTOR.getID(), SHOULD_FOLLOWER_OPPOSE_MASTER); + FOLLOWER_MOTOR.setControl(followerRequest); + } + + public enum ClimberState { + REST(0, 0, 0.7), + CLIMB_L1(0.5, 0, 1), + CLIMB_L2(0.5, 0, 1), + CLIMB_L3(0.5, 0, 1), + SCORE_L4(0.5, 0, 1); + + public final double targetExtendedPositionMeters; + public final double targetRetractedPositionMeters; + final double speedScalar; + + ClimberState(double targetExtendedPositionMeters, double targetRetractedPositionMeters, double speedScalar) { + this.targetExtendedPositionMeters = targetExtendedPositionMeters; + this.targetRetractedPositionMeters = targetRetractedPositionMeters; + this.speedScalar = speedScalar; + } + } +} \ No newline at end of file From 0a0096ad703566ac33c9d64222c2fe2a730b36d8 Mon Sep 17 00:00:00 2001 From: ShmayaR Date: Sun, 18 Jan 2026 22:47:58 +0200 Subject: [PATCH 02/12] fixed ligic issue in logging --- .../robot/subsystems/climber/Climber.java | 32 ++++++++----------- .../subsystems/climber/ClimberCommands.java | 2 +- 2 files changed, 14 insertions(+), 20 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 955b9dbf..fe55cf14 100644 --- a/src/main/java/frc/trigon/robot/subsystems/climber/Climber.java +++ b/src/main/java/frc/trigon/robot/subsystems/climber/Climber.java @@ -13,7 +13,6 @@ import frc.trigon.lib.hardware.phoenix6.talonfx.TalonFXSignal; import frc.trigon.lib.utilities.Conversions; import frc.trigon.robot.subsystems.MotorSubsystem; -import org.littletonrobotics.junction.AutoLogOutput; import org.littletonrobotics.junction.Logger; public class Climber extends MotorSubsystem { @@ -47,7 +46,7 @@ public void stop() { @Override public void updateLog(SysIdRoutineLog log) { log.motor("Climber") - .linearPosition(Units.Meters.of(getPositionRotations())) + .linearPosition(Units.Meters.of(getPositionMeters())) .linearVelocity(Units.MetersPerSecond.of(masterMotor.getSignal(TalonFXSignal.VELOCITY))) .voltage(Units.Volts.of(masterMotor.getSignal(TalonFXSignal.MOTOR_VOLTAGE))); } @@ -65,7 +64,7 @@ public void updateMechanism() { @Override public void updatePeriodically() { masterMotor.update(); - Logger.recordOutput("Elevator/CurrentPositionMeters", getPositionMeters()); + Logger.recordOutput("Climber/CurrentPositionMeters", getPositionMeters()); } @Override @@ -73,19 +72,6 @@ public void sysIDDrive(double targetVoltage) { masterMotor.setControl(voltageRequest.withOutput(targetVoltage)); } - public boolean atExtendedState(ClimberConstants.ClimberState targetState) { - return targetState == this.targetState && atTargetExtendedState(); - } - - @AutoLogOutput(key = "Elevator/AtTargetState") - public boolean atTargetExtendedState() { - return calculateTargetExtendedStateDistance() < ClimberConstants.POSITION_TOLERANCE_METERS; - } - - private double calculateTargetExtendedStateDistance() { - return Math.abs(targetState.targetExtendedPositionMeters - getPositionMeters()); - } - public double metersToRotations(double positionMeters) { return Conversions.distanceToRotations(positionMeters, ClimberConstants.DRUM_DIAMETER_METERS); } @@ -97,18 +83,26 @@ void setTargetExtendedState(ClimberConstants.ClimberState targetState) { } void setTargetRetractedState(ClimberConstants.ClimberState targetState) { - if (atExtendedState(targetState)) + if (atTargetExtendedState()) setTargetPositionRotations(metersToRotations(targetState.targetRetractedPositionMeters)); } + void setTargetPositionRotations(double targetPositionRotations) { + masterMotor.setControl(positionRequest.withPosition(targetPositionRotations)); + } + private void scalePositionRequestSpeed(double speedScalar) { positionRequest.Velocity = ClimberConstants.DEFAULT_MAXIMUM_VELOCITY * speedScalar; positionRequest.Acceleration = ClimberConstants.DEFAULT_MAXIMUM_ACCELERATION * speedScalar; positionRequest.Jerk = positionRequest.Acceleration * 10; } - void setTargetPositionRotations(double targetPositionRotations) { - masterMotor.setControl(positionRequest.withPosition(targetPositionRotations)); + private boolean atTargetExtendedState() { + return calculateTargetExtendedStateDistance() < ClimberConstants.POSITION_TOLERANCE_METERS; + } + + private double calculateTargetExtendedStateDistance() { + return Math.abs(targetState.targetExtendedPositionMeters - getPositionMeters()); } private Pose3d calculateComponentPose() { diff --git a/src/main/java/frc/trigon/robot/subsystems/climber/ClimberCommands.java b/src/main/java/frc/trigon/robot/subsystems/climber/ClimberCommands.java index 65822e15..c393e770 100644 --- a/src/main/java/frc/trigon/robot/subsystems/climber/ClimberCommands.java +++ b/src/main/java/frc/trigon/robot/subsystems/climber/ClimberCommands.java @@ -13,7 +13,7 @@ public static Command getDebuggingCommand() { RobotContainer.CLIMBER::setTargetPositionRotations, false, Set.of(RobotContainer.CLIMBER), - "Debugging/ElevatorTargetPositionRotations" + "Debugging/ClimberTargetPositionRotations" ); } From fda7d41eab571624cbb652f6f9c412b9882871c7 Mon Sep 17 00:00:00 2001 From: ShmayaR Date: Mon, 19 Jan 2026 11:13:17 +0200 Subject: [PATCH 03/12] fixed sysid bug --- .../java/frc/trigon/robot/subsystems/climber/Climber.java | 7 +++---- .../trigon/robot/subsystems/climber/ClimberCommands.java | 2 +- 2 files changed, 4 insertions(+), 5 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 fe55cf14..78f89d12 100644 --- a/src/main/java/frc/trigon/robot/subsystems/climber/Climber.java +++ b/src/main/java/frc/trigon/robot/subsystems/climber/Climber.java @@ -47,7 +47,7 @@ public void stop() { public void updateLog(SysIdRoutineLog log) { log.motor("Climber") .linearPosition(Units.Meters.of(getPositionMeters())) - .linearVelocity(Units.MetersPerSecond.of(masterMotor.getSignal(TalonFXSignal.VELOCITY))) + .linearVelocity(Units.MetersPerSecond.of(rotationsToMeters(masterMotor.getSignal(TalonFXSignal.VELOCITY)))) .voltage(Units.Volts.of(masterMotor.getSignal(TalonFXSignal.MOTOR_VOLTAGE))); } @@ -82,9 +82,8 @@ void setTargetExtendedState(ClimberConstants.ClimberState targetState) { setTargetPositionRotations(metersToRotations(targetState.targetExtendedPositionMeters)); } - void setTargetRetractedState(ClimberConstants.ClimberState targetState) { - if (atTargetExtendedState()) - setTargetPositionRotations(metersToRotations(targetState.targetRetractedPositionMeters)); + void setTargetRetractedState() { + setTargetPositionRotations(metersToRotations(targetState.targetRetractedPositionMeters)); } void setTargetPositionRotations(double targetPositionRotations) { diff --git a/src/main/java/frc/trigon/robot/subsystems/climber/ClimberCommands.java b/src/main/java/frc/trigon/robot/subsystems/climber/ClimberCommands.java index c393e770..f40bdac4 100644 --- a/src/main/java/frc/trigon/robot/subsystems/climber/ClimberCommands.java +++ b/src/main/java/frc/trigon/robot/subsystems/climber/ClimberCommands.java @@ -35,7 +35,7 @@ public static Command getSetTargetExtendedStateCommand(ClimberConstants.ClimberS public static Command getSetTargetRetractedStateCommand(ClimberConstants.ClimberState targetState) { return new StartEndCommand( - () -> RobotContainer.CLIMBER.setTargetRetractedState(targetState), + RobotContainer.CLIMBER::setTargetRetractedState, RobotContainer.CLIMBER::stop, RobotContainer.CLIMBER ); From 66d12e7f0ed10a7790d72fb597339fe50da317ba Mon Sep 17 00:00:00 2001 From: ShmayaR Date: Mon, 19 Jan 2026 14:39:40 +0200 Subject: [PATCH 04/12] Update ClimberCommands.java --- .../frc/trigon/robot/subsystems/climber/ClimberCommands.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/trigon/robot/subsystems/climber/ClimberCommands.java b/src/main/java/frc/trigon/robot/subsystems/climber/ClimberCommands.java index f40bdac4..fc271b3a 100644 --- a/src/main/java/frc/trigon/robot/subsystems/climber/ClimberCommands.java +++ b/src/main/java/frc/trigon/robot/subsystems/climber/ClimberCommands.java @@ -33,7 +33,7 @@ public static Command getSetTargetExtendedStateCommand(ClimberConstants.ClimberS ); } - public static Command getSetTargetRetractedStateCommand(ClimberConstants.ClimberState targetState) { + public static Command getSetTargetRetractedStateCommand() { return new StartEndCommand( RobotContainer.CLIMBER::setTargetRetractedState, RobotContainer.CLIMBER::stop, From 1310b1ca4addb5aea9ca2b6bdf8711b361688b8c Mon Sep 17 00:00:00 2001 From: ShmayaR Date: Thu, 29 Jan 2026 15:48:21 +0200 Subject: [PATCH 05/12] Update lib --- src/main/java/frc/trigon/lib | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/trigon/lib b/src/main/java/frc/trigon/lib index 6b6c7adb..9403200d 160000 --- a/src/main/java/frc/trigon/lib +++ b/src/main/java/frc/trigon/lib @@ -1 +1 @@ -Subproject commit 6b6c7adbf9d1111fa67fdc0b801e0520044bc7f8 +Subproject commit 9403200d5d1161a2c75be560251618af01230b5a From 3cd3856cd7e122a6f129725a698b99e446835879 Mon Sep 17 00:00:00 2001 From: ShmayaR Date: Thu, 29 Jan 2026 16:10:30 +0200 Subject: [PATCH 06/12] updated constants to fit pdr --- .../robot/subsystems/climber/Climber.java | 29 ++++---- .../subsystems/climber/ClimberConstants.java | 73 +++++++------------ 2 files changed, 42 insertions(+), 60 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 78f89d12..3634850a 100644 --- a/src/main/java/frc/trigon/robot/subsystems/climber/Climber.java +++ b/src/main/java/frc/trigon/robot/subsystems/climber/Climber.java @@ -20,7 +20,11 @@ public class Climber extends MotorSubsystem { masterMotor = ClimberConstants.MASTER_MOTOR, followerMotor = ClimberConstants.FOLLOWER_MOTOR; private final VoltageOut voltageRequest = new VoltageOut(0).withEnableFOC(ClimberConstants.FOC_ENABLED); - private final DynamicMotionMagicVoltage positionRequest = new DynamicMotionMagicVoltage(0, ClimberConstants.DEFAULT_MAXIMUM_VELOCITY, ClimberConstants.DEFAULT_MAXIMUM_ACCELERATION).withEnableFOC(ClimberConstants.FOC_ENABLED); + private final DynamicMotionMagicVoltage positionRequest = new DynamicMotionMagicVoltage( + 0, + ClimberConstants.DEFAULT_MAXIMUM_VELOCITY, + ClimberConstants.DEFAULT_MAXIMUM_ACCELERATION + ).withEnableFOC(ClimberConstants.FOC_ENABLED); private ClimberConstants.ClimberState targetState = ClimberConstants.ClimberState.REST; public Climber() { @@ -46,25 +50,24 @@ public void stop() { @Override public void updateLog(SysIdRoutineLog log) { log.motor("Climber") - .linearPosition(Units.Meters.of(getPositionMeters())) - .linearVelocity(Units.MetersPerSecond.of(rotationsToMeters(masterMotor.getSignal(TalonFXSignal.VELOCITY)))) + .angularPosition(Units.Rotations.of(getPositionRotations())) + .angularVelocity(Units.RotationsPerSecond.of(masterMotor.getSignal(TalonFXSignal.VELOCITY))) .voltage(Units.Volts.of(masterMotor.getSignal(TalonFXSignal.MOTOR_VOLTAGE))); } @Override public void updateMechanism() { - Logger.recordOutput("Poses/Components/ClimberPose", calculateComponentPose()); - ClimberConstants.MECHANISM.update( - getPositionMeters(), - rotationsToMeters(masterMotor.getSignal(TalonFXSignal.CLOSED_LOOP_REFERENCE)) + getPositionRotations(), + (masterMotor.getSignal(TalonFXSignal.CLOSED_LOOP_REFERENCE)) ); + + Logger.recordOutput("Poses/Components/ClimberPose", calculateComponentPose()); } @Override public void updatePeriodically() { masterMotor.update(); - Logger.recordOutput("Climber/CurrentPositionMeters", getPositionMeters()); } @Override @@ -72,10 +75,6 @@ public void sysIDDrive(double targetVoltage) { masterMotor.setControl(voltageRequest.withOutput(targetVoltage)); } - public double metersToRotations(double positionMeters) { - return Conversions.distanceToRotations(positionMeters, ClimberConstants.DRUM_DIAMETER_METERS); - } - void setTargetExtendedState(ClimberConstants.ClimberState targetState) { this.targetState = targetState; scalePositionRequestSpeed(targetState.speedScalar); @@ -105,7 +104,7 @@ private double calculateTargetExtendedStateDistance() { } private Pose3d calculateComponentPose() { - return ClimberConstants.FIRST_STAGE_VISUALIZATION_ORIGIN_POINT.transformBy( + return ClimberConstants.CLIMBER_VISUALIZATION_ORIGIN_POINT.transformBy( new Transform3d( new Translation3d(0, 0, getPositionMeters()), new Rotation3d() @@ -113,6 +112,10 @@ private Pose3d calculateComponentPose() { ); } + private double metersToRotations(double positionMeters) { + return Conversions.distanceToRotations(positionMeters, ClimberConstants.DRUM_DIAMETER_METERS); + } + private double getPositionRotations() { return masterMotor.getSignal(TalonFXSignal.POSITION); } 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 61c10ec0..7b937ac2 100644 --- a/src/main/java/frc/trigon/robot/subsystems/climber/ClimberConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/climber/ClimberConstants.java @@ -1,7 +1,6 @@ package frc.trigon.robot.subsystems.climber; import com.ctre.phoenix6.configs.TalonFXConfiguration; -import com.ctre.phoenix6.controls.Follower; import com.ctre.phoenix6.signals.*; import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.geometry.Rotation3d; @@ -17,28 +16,21 @@ import frc.trigon.lib.utilities.mechanisms.ElevatorMechanism2d; public class ClimberConstants { - private static final int - MASTER_MOTOR_ID = 19, - FOLLOWER_MOTOR_ID = 20; - private static final String - MASTER_MOTOR_NAME = "ClimberMasterMotor", - FOLLOWER_MOTOR_NAME = "ClimberFollowerMotor"; - static final TalonFXMotor - MASTER_MOTOR = new TalonFXMotor(MASTER_MOTOR_ID, MASTER_MOTOR_NAME), - FOLLOWER_MOTOR = new TalonFXMotor(FOLLOWER_MOTOR_ID, FOLLOWER_MOTOR_NAME); - - private static final double GEAR_RATIO = 5; - private static final MotorAlignmentValue SHOULD_FOLLOWER_OPPOSE_MASTER = MotorAlignmentValue.Aligned; + private static final int MOTOR_ID = 18; + private static final String MASTER_MOTOR_NAME = "ClimberMasterMotor"; + static final TalonFXMotor MASTER_MOTOR = new TalonFXMotor(MOTOR_ID, MASTER_MOTOR_NAME); + + private static final double GEAR_RATIO = 28; static final double DEFAULT_MAXIMUM_VELOCITY = RobotHardwareStats.isSimulation() ? 80 : 20, DEFAULT_MAXIMUM_ACCELERATION = RobotHardwareStats.isSimulation() ? 80 : 50; static final boolean FOC_ENABLED = true; - private static final int MOTOR_AMOUNT = 2; - private static final DCMotor GEARBOX = DCMotor.getKrakenX60(MOTOR_AMOUNT); + private static final int MOTOR_AMOUNT = 1; + private static final DCMotor GEARBOX = DCMotor.getKrakenX44Foc(MOTOR_AMOUNT); private static final double - CLIMBER_MASS_KILOGRAMS = 8, - DRUM_RADIUS_METERS = 0.025, + CLIMBER_MASS_KILOGRAMS = 60, + DRUM_RADIUS_METERS = 0.04, MINIMUM_CLIMBER_HEIGHT_METERS = 0.73, MAXIMUM_CLIMBER_HEIGHT_METERS = 1.8; private static final boolean SHOULD_SIMULATE_GRAVITY = true; @@ -58,27 +50,28 @@ public class ClimberConstants { Units.Second.of(1000) ); - public static final Pose3d FIRST_STAGE_VISUALIZATION_ORIGIN_POINT = new Pose3d( + public static final Pose3d CLIMBER_VISUALIZATION_ORIGIN_POINT = new Pose3d( new Translation3d(0, 0, 0), new Rotation3d(0, 0, 0) ); + private static final String MECHANISM_NAME = "ClimberMechanism"; + private static final Color MECHANISM_COLOR = Color.kYellow; static final ElevatorMechanism2d MECHANISM = new ElevatorMechanism2d( - "ClimberMechanism", + MECHANISM_NAME, MAXIMUM_CLIMBER_HEIGHT_METERS, MINIMUM_CLIMBER_HEIGHT_METERS, - Color.kYellow + MECHANISM_COLOR ); static final double DRUM_DIAMETER_METERS = DRUM_RADIUS_METERS * 2; static final double POSITION_TOLERANCE_METERS = 0.07; static { - configureMasterMotor(); - configureFollowerMotor(); + configureMotor(); } - private static void configureMasterMotor() { + private static void configureMotor() { final TalonFXConfiguration config = new TalonFXConfiguration(); config.Audio.BeepOnBoot = false; @@ -86,15 +79,16 @@ private static void configureMasterMotor() { config.MotorOutput.NeutralMode = NeutralModeValue.Brake; config.MotorOutput.Inverted = InvertedValue.Clockwise_Positive; + config.Feedback.SensorToMechanismRatio = GEAR_RATIO; - config.Slot0.kP = RobotHardwareStats.isSimulation() ? 40 : 6.5905; + config.Slot0.kP = RobotHardwareStats.isSimulation() ? 0 : 0; config.Slot0.kI = RobotHardwareStats.isSimulation() ? 0 : 0; - config.Slot0.kD = RobotHardwareStats.isSimulation() ? 0.22774 : 0.2; - config.Slot0.kS = RobotHardwareStats.isSimulation() ? 0.066659 : 0.25513; - config.Slot0.kV = RobotHardwareStats.isSimulation() ? 0.74502 : 0.52756; + config.Slot0.kD = RobotHardwareStats.isSimulation() ? 0 : 0; + config.Slot0.kS = RobotHardwareStats.isSimulation() ? 0 : 0; + config.Slot0.kV = RobotHardwareStats.isSimulation() ? 0 : 0; config.Slot0.kA = RobotHardwareStats.isSimulation() ? 0 : 0; - config.Slot0.kG = RobotHardwareStats.isSimulation() ? 0.30539 : 0.4; + config.Slot0.kG = RobotHardwareStats.isSimulation() ? 0 : 0; config.Slot0.GravityType = GravityTypeValue.Elevator_Static; config.Slot0.StaticFeedforwardSign = StaticFeedforwardSignValue.UseVelocitySign; @@ -117,6 +111,9 @@ private static void configureMasterMotor() { config.MotionMagic.MotionMagicAcceleration = DEFAULT_MAXIMUM_ACCELERATION; config.MotionMagic.MotionMagicJerk = config.MotionMagic.MotionMagicAcceleration * 10; + config.CurrentLimits.StatorCurrentLimitEnable = true; + config.CurrentLimits.StatorCurrentLimit = 80; + MASTER_MOTOR.applyConfiguration(config); MASTER_MOTOR.setPhysicsSimulation(SIMULATION); @@ -127,27 +124,9 @@ private static void configureMasterMotor() { MASTER_MOTOR.registerSignal(TalonFXSignal.STATOR_CURRENT, 100); } - private static void configureFollowerMotor() { - final TalonFXConfiguration config = new TalonFXConfiguration(); - - config.Audio.BeepOnBoot = false; - config.Audio.BeepOnConfig = false; - - config.MotorOutput.NeutralMode = NeutralModeValue.Brake; - config.MotorOutput.Inverted = InvertedValue.CounterClockwise_Positive; - - FOLLOWER_MOTOR.applyConfiguration(config); - - final Follower followerRequest = new Follower(MASTER_MOTOR.getID(), SHOULD_FOLLOWER_OPPOSE_MASTER); - FOLLOWER_MOTOR.setControl(followerRequest); - } - public enum ClimberState { REST(0, 0, 0.7), - CLIMB_L1(0.5, 0, 1), - CLIMB_L2(0.5, 0, 1), - CLIMB_L3(0.5, 0, 1), - SCORE_L4(0.5, 0, 1); + CLIMB_L1(0.5, 0, 1); public final double targetExtendedPositionMeters; public final double targetRetractedPositionMeters; From f91adf0a77ff46b23c35b651c9d270edd9605867 Mon Sep 17 00:00:00 2001 From: ShmayaR Date: Thu, 29 Jan 2026 16:13:24 +0200 Subject: [PATCH 07/12] update motor id's --- .../java/frc/trigon/robot/subsystems/hood/HoodConstants.java | 2 +- .../frc/trigon/robot/subsystems/shooter/ShooterConstants.java | 4 ++-- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/trigon/robot/subsystems/hood/HoodConstants.java b/src/main/java/frc/trigon/robot/subsystems/hood/HoodConstants.java index 31c43b7e..5409b9c7 100644 --- a/src/main/java/frc/trigon/robot/subsystems/hood/HoodConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/hood/HoodConstants.java @@ -19,7 +19,7 @@ public class HoodConstants { private static final int - MOTOR_ID = 18, + MOTOR_ID = 17, ENCODER_ID = 18; private static final String MOTOR_NAME = "HoodMotor", diff --git a/src/main/java/frc/trigon/robot/subsystems/shooter/ShooterConstants.java b/src/main/java/frc/trigon/robot/subsystems/shooter/ShooterConstants.java index 7e7c5b9c..ad41af9b 100644 --- a/src/main/java/frc/trigon/robot/subsystems/shooter/ShooterConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/shooter/ShooterConstants.java @@ -16,8 +16,8 @@ public class ShooterConstants { private static final int - MASTER_MOTOR_ID = 16, - FOLLOWER_MOTOR_ID = 17; + MASTER_MOTOR_ID = 15, + FOLLOWER_MOTOR_ID = 16; private static final String MASTER_MOTOR_NAME = "ShooterMasterMotor", FOLLOWER_MOTOR_NAME = "ShooterFollowerMotor"; From c5fea380e45d77f07e0ef7a529ee4f1dd9f50de5 Mon Sep 17 00:00:00 2001 From: ShmayaR Date: Thu, 29 Jan 2026 16:16:47 +0200 Subject: [PATCH 08/12] fully removed follower motor --- .../robot/subsystems/climber/Climber.java | 22 +++++++++---------- .../subsystems/climber/ClimberCommands.java | 2 +- .../subsystems/climber/ClimberConstants.java | 18 +++++++-------- 3 files changed, 20 insertions(+), 22 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 3634850a..9e759229 100644 --- a/src/main/java/frc/trigon/robot/subsystems/climber/Climber.java +++ b/src/main/java/frc/trigon/robot/subsystems/climber/Climber.java @@ -17,8 +17,7 @@ public class Climber extends MotorSubsystem { private final TalonFXMotor - masterMotor = ClimberConstants.MASTER_MOTOR, - followerMotor = ClimberConstants.FOLLOWER_MOTOR; + motor = ClimberConstants.MOTOR; private final VoltageOut voltageRequest = new VoltageOut(0).withEnableFOC(ClimberConstants.FOC_ENABLED); private final DynamicMotionMagicVoltage positionRequest = new DynamicMotionMagicVoltage( 0, @@ -38,28 +37,27 @@ public SysIdRoutine.Config getSysIDConfig() { @Override public void setBrake(boolean brake) { - masterMotor.setBrake(brake); - followerMotor.setBrake(brake); + motor.setBrake(brake); } @Override public void stop() { - masterMotor.stopMotor(); + motor.stopMotor(); } @Override public void updateLog(SysIdRoutineLog log) { log.motor("Climber") .angularPosition(Units.Rotations.of(getPositionRotations())) - .angularVelocity(Units.RotationsPerSecond.of(masterMotor.getSignal(TalonFXSignal.VELOCITY))) - .voltage(Units.Volts.of(masterMotor.getSignal(TalonFXSignal.MOTOR_VOLTAGE))); + .angularVelocity(Units.RotationsPerSecond.of(motor.getSignal(TalonFXSignal.VELOCITY))) + .voltage(Units.Volts.of(motor.getSignal(TalonFXSignal.MOTOR_VOLTAGE))); } @Override public void updateMechanism() { ClimberConstants.MECHANISM.update( getPositionRotations(), - (masterMotor.getSignal(TalonFXSignal.CLOSED_LOOP_REFERENCE)) + (motor.getSignal(TalonFXSignal.CLOSED_LOOP_REFERENCE)) ); Logger.recordOutput("Poses/Components/ClimberPose", calculateComponentPose()); @@ -67,12 +65,12 @@ public void updateMechanism() { @Override public void updatePeriodically() { - masterMotor.update(); + motor.update(); } @Override public void sysIDDrive(double targetVoltage) { - masterMotor.setControl(voltageRequest.withOutput(targetVoltage)); + motor.setControl(voltageRequest.withOutput(targetVoltage)); } void setTargetExtendedState(ClimberConstants.ClimberState targetState) { @@ -86,7 +84,7 @@ void setTargetRetractedState() { } void setTargetPositionRotations(double targetPositionRotations) { - masterMotor.setControl(positionRequest.withPosition(targetPositionRotations)); + motor.setControl(positionRequest.withPosition(targetPositionRotations)); } private void scalePositionRequestSpeed(double speedScalar) { @@ -117,7 +115,7 @@ private double metersToRotations(double positionMeters) { } private double getPositionRotations() { - return masterMotor.getSignal(TalonFXSignal.POSITION); + return motor.getSignal(TalonFXSignal.POSITION); } private double getPositionMeters() { diff --git a/src/main/java/frc/trigon/robot/subsystems/climber/ClimberCommands.java b/src/main/java/frc/trigon/robot/subsystems/climber/ClimberCommands.java index fc271b3a..ae617127 100644 --- a/src/main/java/frc/trigon/robot/subsystems/climber/ClimberCommands.java +++ b/src/main/java/frc/trigon/robot/subsystems/climber/ClimberCommands.java @@ -40,4 +40,4 @@ public static Command getSetTargetRetractedStateCommand() { RobotContainer.CLIMBER ); } -} \ No newline at end of file +} 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 7b937ac2..c9553f75 100644 --- a/src/main/java/frc/trigon/robot/subsystems/climber/ClimberConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/climber/ClimberConstants.java @@ -17,8 +17,8 @@ public class ClimberConstants { private static final int MOTOR_ID = 18; - private static final String MASTER_MOTOR_NAME = "ClimberMasterMotor"; - static final TalonFXMotor MASTER_MOTOR = new TalonFXMotor(MOTOR_ID, MASTER_MOTOR_NAME); + private static final String MOTOR_NAME = "ClimberMasterMotor"; + static final TalonFXMotor MOTOR = new TalonFXMotor(MOTOR_ID, MOTOR_NAME); private static final double GEAR_RATIO = 28; static final double @@ -114,14 +114,14 @@ private static void configureMotor() { config.CurrentLimits.StatorCurrentLimitEnable = true; config.CurrentLimits.StatorCurrentLimit = 80; - MASTER_MOTOR.applyConfiguration(config); - MASTER_MOTOR.setPhysicsSimulation(SIMULATION); + MOTOR.applyConfiguration(config); + MOTOR.setPhysicsSimulation(SIMULATION); - MASTER_MOTOR.registerSignal(TalonFXSignal.POSITION, 100); - MASTER_MOTOR.registerSignal(TalonFXSignal.VELOCITY, 100); - MASTER_MOTOR.registerSignal(TalonFXSignal.MOTOR_VOLTAGE, 100); - MASTER_MOTOR.registerSignal(TalonFXSignal.CLOSED_LOOP_REFERENCE, 100); - MASTER_MOTOR.registerSignal(TalonFXSignal.STATOR_CURRENT, 100); + MOTOR.registerSignal(TalonFXSignal.POSITION, 100); + MOTOR.registerSignal(TalonFXSignal.VELOCITY, 100); + MOTOR.registerSignal(TalonFXSignal.MOTOR_VOLTAGE, 100); + MOTOR.registerSignal(TalonFXSignal.CLOSED_LOOP_REFERENCE, 100); + MOTOR.registerSignal(TalonFXSignal.STATOR_CURRENT, 100); } public enum ClimberState { From 08cbd3aad772158f3cfa96faaf14f5ce91fe7640 Mon Sep 17 00:00:00 2001 From: ShmayaR Date: Thu, 29 Jan 2026 19:35:28 +0200 Subject: [PATCH 09/12] refromat --- .../robot/subsystems/climber/Climber.java | 20 ++++++++++--------- .../subsystems/climber/ClimberConstants.java | 8 -------- 2 files changed, 11 insertions(+), 17 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 9e759229..46c0b2f1 100644 --- a/src/main/java/frc/trigon/robot/subsystems/climber/Climber.java +++ b/src/main/java/frc/trigon/robot/subsystems/climber/Climber.java @@ -57,7 +57,7 @@ public void updateLog(SysIdRoutineLog log) { public void updateMechanism() { ClimberConstants.MECHANISM.update( getPositionRotations(), - (motor.getSignal(TalonFXSignal.CLOSED_LOOP_REFERENCE)) + motor.getSignal(TalonFXSignal.CLOSED_LOOP_REFERENCE) ); Logger.recordOutput("Poses/Components/ClimberPose", calculateComponentPose()); @@ -73,6 +73,11 @@ public void sysIDDrive(double targetVoltage) { motor.setControl(voltageRequest.withOutput(targetVoltage)); } + + public boolean atTargetExtendedState() { + return calculateTargetExtendedStateDistance() < ClimberConstants.POSITION_TOLERANCE_METERS; + } + void setTargetExtendedState(ClimberConstants.ClimberState targetState) { this.targetState = targetState; scalePositionRequestSpeed(targetState.speedScalar); @@ -93,10 +98,6 @@ private void scalePositionRequestSpeed(double speedScalar) { positionRequest.Jerk = positionRequest.Acceleration * 10; } - private boolean atTargetExtendedState() { - return calculateTargetExtendedStateDistance() < ClimberConstants.POSITION_TOLERANCE_METERS; - } - private double calculateTargetExtendedStateDistance() { return Math.abs(targetState.targetExtendedPositionMeters - getPositionMeters()); } @@ -114,14 +115,15 @@ private double metersToRotations(double positionMeters) { return Conversions.distanceToRotations(positionMeters, ClimberConstants.DRUM_DIAMETER_METERS); } - private double getPositionRotations() { - return motor.getSignal(TalonFXSignal.POSITION); - } - private double getPositionMeters() { return rotationsToMeters(getPositionRotations()); } + + private double getPositionRotations() { + return motor.getSignal(TalonFXSignal.POSITION); + } + private double rotationsToMeters(double positionRotations) { return Conversions.rotationsToDistance(positionRotations, ClimberConstants.DRUM_DIAMETER_METERS); } 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 c9553f75..a1cc71d5 100644 --- a/src/main/java/frc/trigon/robot/subsystems/climber/ClimberConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/climber/ClimberConstants.java @@ -99,14 +99,6 @@ private static void configureMotor() { config.HardwareLimitSwitch.ReverseLimitAutosetPositionEnable = true; config.HardwareLimitSwitch.ReverseLimitAutosetPositionValue = 0; - config.HardwareLimitSwitch.ForwardLimitEnable = false; - - config.SoftwareLimitSwitch.ReverseSoftLimitEnable = true; - config.SoftwareLimitSwitch.ReverseSoftLimitThreshold = 0; - - config.SoftwareLimitSwitch.ForwardSoftLimitEnable = true; - config.SoftwareLimitSwitch.ForwardSoftLimitThreshold = 6.8; - config.MotionMagic.MotionMagicCruiseVelocity = DEFAULT_MAXIMUM_VELOCITY; config.MotionMagic.MotionMagicAcceleration = DEFAULT_MAXIMUM_ACCELERATION; config.MotionMagic.MotionMagicJerk = config.MotionMagic.MotionMagicAcceleration * 10; From 315995dcb347bfbdd4ed4246e9e5376e84f82d96 Mon Sep 17 00:00:00 2001 From: ShmayaR Date: Fri, 30 Jan 2026 00:25:56 +0200 Subject: [PATCH 10/12] added command groups --- .../commandfactories/ClimbCommands.java | 20 +++++++++++++++++++ .../robot/subsystems/climber/Climber.java | 5 +++++ .../subsystems/climber/ClimberCommands.java | 10 ++++++++++ .../subsystems/climber/ClimberConstants.java | 4 +++- 4 files changed, 38 insertions(+), 1 deletion(-) create mode 100644 src/main/java/frc/trigon/robot/commands/commandfactories/ClimbCommands.java diff --git a/src/main/java/frc/trigon/robot/commands/commandfactories/ClimbCommands.java b/src/main/java/frc/trigon/robot/commands/commandfactories/ClimbCommands.java new file mode 100644 index 00000000..5a9eaa36 --- /dev/null +++ b/src/main/java/frc/trigon/robot/commands/commandfactories/ClimbCommands.java @@ -0,0 +1,20 @@ +package frc.trigon.robot.commands.commandfactories; + +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import edu.wpi.first.wpilibj2.command.button.Trigger; +import frc.trigon.robot.subsystems.climber.ClimberCommands; +import frc.trigon.robot.subsystems.climber.ClimberConstants; + +public class ClimbCommands { + public static Command getClimbToL1Command(Trigger continueTrigger) { + return new SequentialCommandGroup( + ClimberCommands.getSetTargetExtendedStateCommand(ClimberConstants.ClimberState.CLIMB_L1).until(continueTrigger), + ClimberCommands.getSetTargetRetractedStateCommand() + ); + } + + public static Command getClimbDownFromL1Command() { + return ClimberCommands.getSetTargetExtendedStateCommand(ClimberConstants.ClimberState.CLIMB_L1); + } +} 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 46c0b2f1..3a70e135 100644 --- a/src/main/java/frc/trigon/robot/subsystems/climber/Climber.java +++ b/src/main/java/frc/trigon/robot/subsystems/climber/Climber.java @@ -92,6 +92,11 @@ void setTargetPositionRotations(double targetPositionRotations) { motor.setControl(positionRequest.withPosition(targetPositionRotations)); } + void resetPosition() { + targetState = ClimberConstants.ClimberState.REST; + motor.setControl(voltageRequest.withOutput(ClimberConstants.CLIMBER_RESET_VOLTAGE)); + } + private void scalePositionRequestSpeed(double speedScalar) { positionRequest.Velocity = ClimberConstants.DEFAULT_MAXIMUM_VELOCITY * speedScalar; positionRequest.Acceleration = ClimberConstants.DEFAULT_MAXIMUM_ACCELERATION * speedScalar; diff --git a/src/main/java/frc/trigon/robot/subsystems/climber/ClimberCommands.java b/src/main/java/frc/trigon/robot/subsystems/climber/ClimberCommands.java index ae617127..869201e8 100644 --- a/src/main/java/frc/trigon/robot/subsystems/climber/ClimberCommands.java +++ b/src/main/java/frc/trigon/robot/subsystems/climber/ClimberCommands.java @@ -1,6 +1,7 @@ package frc.trigon.robot.subsystems.climber; import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.StartEndCommand; import frc.trigon.lib.commands.NetworkTablesCommand; import frc.trigon.robot.RobotContainer; @@ -17,6 +18,14 @@ public static Command getDebuggingCommand() { ); } + public static Command getResetPositionCommand() { + return new StartEndCommand( + RobotContainer.CLIMBER::resetPosition, + RobotContainer.CLIMBER::stop, + RobotContainer.CLIMBER + ); + } + public static Command getSetTargetVoltageCommand(double targetVoltage) { return new StartEndCommand( () -> RobotContainer.CLIMBER.sysIDDrive(targetVoltage), @@ -40,4 +49,5 @@ public static Command getSetTargetRetractedStateCommand() { RobotContainer.CLIMBER ); } + } 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 a1cc71d5..6ba71b15 100644 --- a/src/main/java/frc/trigon/robot/subsystems/climber/ClimberConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/climber/ClimberConstants.java @@ -26,12 +26,12 @@ public class ClimberConstants { DEFAULT_MAXIMUM_ACCELERATION = RobotHardwareStats.isSimulation() ? 80 : 50; static final boolean FOC_ENABLED = true; + static final double MINIMUM_CLIMBER_HEIGHT_METERS = 0.73; private static final int MOTOR_AMOUNT = 1; private static final DCMotor GEARBOX = DCMotor.getKrakenX44Foc(MOTOR_AMOUNT); private static final double CLIMBER_MASS_KILOGRAMS = 60, DRUM_RADIUS_METERS = 0.04, - MINIMUM_CLIMBER_HEIGHT_METERS = 0.73, MAXIMUM_CLIMBER_HEIGHT_METERS = 1.8; private static final boolean SHOULD_SIMULATE_GRAVITY = true; private static final ElevatorSimulation SIMULATION = new ElevatorSimulation( @@ -66,6 +66,7 @@ public class ClimberConstants { static final double DRUM_DIAMETER_METERS = DRUM_RADIUS_METERS * 2; static final double POSITION_TOLERANCE_METERS = 0.07; + static final double CLIMBER_RESET_VOLTAGE = -0.5; static { configureMotor(); @@ -114,6 +115,7 @@ private static void configureMotor() { MOTOR.registerSignal(TalonFXSignal.MOTOR_VOLTAGE, 100); MOTOR.registerSignal(TalonFXSignal.CLOSED_LOOP_REFERENCE, 100); MOTOR.registerSignal(TalonFXSignal.STATOR_CURRENT, 100); + MOTOR.registerSignal(TalonFXSignal.REVERSE_LIMIT, 100); } public enum ClimberState { From e2b4ab1567b0ff01340f87126197d5ed72196872 Mon Sep 17 00:00:00 2001 From: ShmayaR Date: Fri, 30 Jan 2026 00:47:14 +0200 Subject: [PATCH 11/12] added second slot for climbing and opening climb --- .../robot/subsystems/climber/Climber.java | 11 +++++----- .../subsystems/climber/ClimberCommands.java | 3 +-- .../subsystems/climber/ClimberConstants.java | 22 ++++++++++++++----- 3 files changed, 24 insertions(+), 12 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 3a70e135..94007bc5 100644 --- a/src/main/java/frc/trigon/robot/subsystems/climber/Climber.java +++ b/src/main/java/frc/trigon/robot/subsystems/climber/Climber.java @@ -80,16 +80,17 @@ public boolean atTargetExtendedState() { void setTargetExtendedState(ClimberConstants.ClimberState targetState) { this.targetState = targetState; - scalePositionRequestSpeed(targetState.speedScalar); - setTargetPositionRotations(metersToRotations(targetState.targetExtendedPositionMeters)); + scalePositionRequestSpeed(targetState.extendedSpeedScalar); + setTargetPositionRotations(metersToRotations(targetState.targetExtendedPositionMeters), ClimberConstants.CLIMBER_WEIGHT_SLOT); } void setTargetRetractedState() { - setTargetPositionRotations(metersToRotations(targetState.targetRetractedPositionMeters)); + scalePositionRequestSpeed(targetState.retractedSpeedScalar); + setTargetPositionRotations(metersToRotations(targetState.targetRetractedPositionMeters), ClimberConstants.FULL_ROBOT_WEIGHT_SLOT); } - void setTargetPositionRotations(double targetPositionRotations) { - motor.setControl(positionRequest.withPosition(targetPositionRotations)); + void setTargetPositionRotations(double targetPositionRotations, int slot) { + motor.setControl(positionRequest.withPosition(targetPositionRotations).withSlot(slot)); } void resetPosition() { diff --git a/src/main/java/frc/trigon/robot/subsystems/climber/ClimberCommands.java b/src/main/java/frc/trigon/robot/subsystems/climber/ClimberCommands.java index 869201e8..d0399277 100644 --- a/src/main/java/frc/trigon/robot/subsystems/climber/ClimberCommands.java +++ b/src/main/java/frc/trigon/robot/subsystems/climber/ClimberCommands.java @@ -1,7 +1,6 @@ package frc.trigon.robot.subsystems.climber; import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.StartEndCommand; import frc.trigon.lib.commands.NetworkTablesCommand; import frc.trigon.robot.RobotContainer; @@ -11,7 +10,7 @@ public class ClimberCommands { public static Command getDebuggingCommand() { return new NetworkTablesCommand( - RobotContainer.CLIMBER::setTargetPositionRotations, + (targetPosition) -> RobotContainer.CLIMBER.setTargetPositionRotations(targetPosition, ClimberConstants.CLIMBER_WEIGHT_SLOT), false, Set.of(RobotContainer.CLIMBER), "Debugging/ClimberTargetPositionRotations" 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 6ba71b15..d4df2af4 100644 --- a/src/main/java/frc/trigon/robot/subsystems/climber/ClimberConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/climber/ClimberConstants.java @@ -67,6 +67,8 @@ public class ClimberConstants { static final double DRUM_DIAMETER_METERS = DRUM_RADIUS_METERS * 2; static final double POSITION_TOLERANCE_METERS = 0.07; static final double CLIMBER_RESET_VOLTAGE = -0.5; + static final int CLIMBER_WEIGHT_SLOT = 0; + static final int FULL_ROBOT_WEIGHT_SLOT = 1; static { configureMotor(); @@ -94,6 +96,14 @@ private static void configureMotor() { config.Slot0.GravityType = GravityTypeValue.Elevator_Static; config.Slot0.StaticFeedforwardSign = StaticFeedforwardSignValue.UseVelocitySign; + config.Slot1.kP = RobotHardwareStats.isSimulation() ? 0 : 0; + config.Slot1.kI = RobotHardwareStats.isSimulation() ? 0 : 0; + config.Slot1.kD = RobotHardwareStats.isSimulation() ? 0 : 0; + config.Slot1.kS = RobotHardwareStats.isSimulation() ? 0 : 0; + config.Slot1.kV = RobotHardwareStats.isSimulation() ? 0 : 0; + config.Slot1.kA = RobotHardwareStats.isSimulation() ? 0 : 0; + config.Slot1.kG = RobotHardwareStats.isSimulation() ? 0 : 0; + config.HardwareLimitSwitch.ReverseLimitEnable = true; config.HardwareLimitSwitch.ReverseLimitType = ReverseLimitTypeValue.NormallyOpen; config.HardwareLimitSwitch.ReverseLimitSource = ReverseLimitSourceValue.LimitSwitchPin; @@ -119,17 +129,19 @@ private static void configureMotor() { } public enum ClimberState { - REST(0, 0, 0.7), - CLIMB_L1(0.5, 0, 1); + REST(0, 0, 0.7, 0.7), + CLIMB_L1(0.5, 0, 1, 0.5); public final double targetExtendedPositionMeters; public final double targetRetractedPositionMeters; - final double speedScalar; + final double extendedSpeedScalar; + final double retractedSpeedScalar; - ClimberState(double targetExtendedPositionMeters, double targetRetractedPositionMeters, double speedScalar) { + ClimberState(double targetExtendedPositionMeters, double targetRetractedPositionMeters, double extendedSpeedScalar, double retractedSpeedScalar) { this.targetExtendedPositionMeters = targetExtendedPositionMeters; this.targetRetractedPositionMeters = targetRetractedPositionMeters; - this.speedScalar = speedScalar; + this.extendedSpeedScalar = extendedSpeedScalar; + this.retractedSpeedScalar = retractedSpeedScalar; } } } \ No newline at end of file From a38b8ef7a9a764156825000524a74c1e49add49d Mon Sep 17 00:00:00 2001 From: ShmayaR Date: Fri, 30 Jan 2026 01:12:23 +0200 Subject: [PATCH 12/12] sysid --- .../frc/trigon/robot/subsystems/climber/Climber.java | 2 +- .../robot/subsystems/climber/ClimberConstants.java | 10 +++++----- 2 files changed, 6 insertions(+), 6 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 94007bc5..eae2fa4a 100644 --- a/src/main/java/frc/trigon/robot/subsystems/climber/Climber.java +++ b/src/main/java/frc/trigon/robot/subsystems/climber/Climber.java @@ -86,7 +86,7 @@ void setTargetExtendedState(ClimberConstants.ClimberState targetState) { void setTargetRetractedState() { scalePositionRequestSpeed(targetState.retractedSpeedScalar); - setTargetPositionRotations(metersToRotations(targetState.targetRetractedPositionMeters), ClimberConstants.FULL_ROBOT_WEIGHT_SLOT); + setTargetPositionRotations(metersToRotations(targetState.targetRetractedPositionMeters), ClimberConstants.ROBOT_WEIGHT_SLOT); } void setTargetPositionRotations(double targetPositionRotations, int slot) { 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 d4df2af4..81c8d9ff 100644 --- a/src/main/java/frc/trigon/robot/subsystems/climber/ClimberConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/climber/ClimberConstants.java @@ -20,7 +20,7 @@ public class ClimberConstants { private static final String MOTOR_NAME = "ClimberMasterMotor"; static final TalonFXMotor MOTOR = new TalonFXMotor(MOTOR_ID, MOTOR_NAME); - private static final double GEAR_RATIO = 28; + private static final double GEAR_RATIO = 280; static final double DEFAULT_MAXIMUM_VELOCITY = RobotHardwareStats.isSimulation() ? 80 : 20, DEFAULT_MAXIMUM_ACCELERATION = RobotHardwareStats.isSimulation() ? 80 : 50; @@ -68,7 +68,7 @@ public class ClimberConstants { static final double POSITION_TOLERANCE_METERS = 0.07; static final double CLIMBER_RESET_VOLTAGE = -0.5; static final int CLIMBER_WEIGHT_SLOT = 0; - static final int FULL_ROBOT_WEIGHT_SLOT = 1; + static final int ROBOT_WEIGHT_SLOT = 1; static { configureMotor(); @@ -88,10 +88,10 @@ private static void configureMotor() { config.Slot0.kP = RobotHardwareStats.isSimulation() ? 0 : 0; config.Slot0.kI = RobotHardwareStats.isSimulation() ? 0 : 0; config.Slot0.kD = RobotHardwareStats.isSimulation() ? 0 : 0; - config.Slot0.kS = RobotHardwareStats.isSimulation() ? 0 : 0; - config.Slot0.kV = RobotHardwareStats.isSimulation() ? 0 : 0; + config.Slot0.kS = RobotHardwareStats.isSimulation() ? 0.0087929 : 0; + config.Slot0.kV = RobotHardwareStats.isSimulation() ? 2.7126 : 0; config.Slot0.kA = RobotHardwareStats.isSimulation() ? 0 : 0; - config.Slot0.kG = RobotHardwareStats.isSimulation() ? 0 : 0; + config.Slot0.kG = RobotHardwareStats.isSimulation() ? 0.052127 : 0; config.Slot0.GravityType = GravityTypeValue.Elevator_Static; config.Slot0.StaticFeedforwardSign = StaticFeedforwardSignValue.UseVelocitySign;