Climber

src/main/java/frc/trigon/robot/RobotContainer.java

@@ -22,19 +22,20 @@
 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;
 import frc.trigon.robot.subsystems.hood.Hood;
 import frc.trigon.robot.subsystems.hood.HoodCommands;
 import frc.trigon.robot.subsystems.intake.Intake;
 import frc.trigon.robot.subsystems.intake.IntakeCommands;
 import frc.trigon.robot.subsystems.loader.Loader;
 import frc.trigon.robot.subsystems.loader.LoaderCommands;
 import frc.trigon.robot.subsystems.loader.LoaderConstants;
-import frc.trigon.robot.subsystems.shooter.Shooter;
-import frc.trigon.robot.subsystems.shooter.ShooterCommands;
 import frc.trigon.robot.subsystems.spindexer.Spindexer;
 import frc.trigon.robot.subsystems.spindexer.SpindexerCommands;
 import frc.trigon.robot.subsystems.spindexer.SpindexerConstants;
-import frc.trigon.robot.subsystems.swerve.Swerve;
 import frc.trigon.robot.subsystems.turret.Turret;
 import frc.trigon.robot.subsystems.turret.TurretCommands;
 import org.littletonrobotics.junction.networktables.LoggedDashboardChooser;
@@ -47,6 +48,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 Hood HOOD = new Hood();
     public static final Intake INTAKE = new Intake();
     public static final Loader LOADER = new Loader();

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);
+    }
+}

src/main/java/frc/trigon/robot/subsystems/climber/Climber.java

@@ -0,0 +1,136 @@
+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.Logger;
+
+public class Climber extends MotorSubsystem {
+    private final TalonFXMotor
+            motor = ClimberConstants.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) {
+        motor.setBrake(brake);
+    }
+
+    @Override
+    public void stop() {
+        motor.stopMotor();
+    }
+
+    @Override
+    public void updateLog(SysIdRoutineLog log) {
+        log.motor("Climber")
+                .angularPosition(Units.Rotations.of(getPositionRotations()))
+                .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(),
+                motor.getSignal(TalonFXSignal.CLOSED_LOOP_REFERENCE)
+        );
+
+        Logger.recordOutput("Poses/Components/ClimberPose", calculateComponentPose());
+    }
+
+    @Override
+    public void updatePeriodically() {
+        motor.update();
+    }
+
+    @Override
+    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.extendedSpeedScalar);
+        setTargetPositionRotations(metersToRotations(targetState.targetExtendedPositionMeters), ClimberConstants.CLIMBER_WEIGHT_SLOT);
+    }
+
+    void setTargetRetractedState() {
+        scalePositionRequestSpeed(targetState.retractedSpeedScalar);
+        setTargetPositionRotations(metersToRotations(targetState.targetRetractedPositionMeters), ClimberConstants.ROBOT_WEIGHT_SLOT);
+    }
+
+    void setTargetPositionRotations(double targetPositionRotations, int slot) {
+        motor.setControl(positionRequest.withPosition(targetPositionRotations).withSlot(slot));
+    }
+
+    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;
+        positionRequest.Jerk = positionRequest.Acceleration * 10;
+    }
+
+    private double calculateTargetExtendedStateDistance() {
+        return Math.abs(targetState.targetExtendedPositionMeters - getPositionMeters());
+    }
+
+    private Pose3d calculateComponentPose() {
+        return ClimberConstants.CLIMBER_VISUALIZATION_ORIGIN_POINT.transformBy(
+                new Transform3d(
+                        new Translation3d(0, 0, getPositionMeters()),
+                        new Rotation3d()
+                )
+        );
+    }
+
+    private double metersToRotations(double positionMeters) {
+        return Conversions.distanceToRotations(positionMeters, ClimberConstants.DRUM_DIAMETER_METERS);
+    }
+
+    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);
+    }
+}

src/main/java/frc/trigon/robot/subsystems/climber/ClimberCommands.java

@@ -0,0 +1,52 @@
+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(
+                (targetPosition) -> RobotContainer.CLIMBER.setTargetPositionRotations(targetPosition, ClimberConstants.CLIMBER_WEIGHT_SLOT),
+                false,
+                Set.of(RobotContainer.CLIMBER),
+                "Debugging/ClimberTargetPositionRotations"
+        );
+    }
+
+    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),
+                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() {
+        return new StartEndCommand(
+                RobotContainer.CLIMBER::setTargetRetractedState,
+                RobotContainer.CLIMBER::stop,
+                RobotContainer.CLIMBER
+        );
+    }
+
+}

src/main/java/frc/trigon/robot/subsystems/climber/ClimberConstants.java

@@ -0,0 +1,147 @@
+package frc.trigon.robot.subsystems.climber;
+
+import com.ctre.phoenix6.configs.TalonFXConfiguration;
+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 MOTOR_ID = 18;
+    private static final String MOTOR_NAME = "ClimberMasterMotor";
+    static final TalonFXMotor MOTOR = new TalonFXMotor(MOTOR_ID, MOTOR_NAME);
+
+    private static final double GEAR_RATIO = 280;
+    static final double
+            DEFAULT_MAXIMUM_VELOCITY = RobotHardwareStats.isSimulation() ? 80 : 20,
+            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,
+            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 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(
+            MECHANISM_NAME,
+            MAXIMUM_CLIMBER_HEIGHT_METERS,
+            MINIMUM_CLIMBER_HEIGHT_METERS,
+            MECHANISM_COLOR
+    );
+
+    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 ROBOT_WEIGHT_SLOT = 1;
+
+    static {
+        configureMotor();
+    }
+
+    private static void configureMotor() {
+        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() ? 0 : 0;
+        config.Slot0.kI = RobotHardwareStats.isSimulation() ? 0 : 0;
+        config.Slot0.kD = 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.052127 : 0;
+
+        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;
+        config.HardwareLimitSwitch.ReverseLimitAutosetPositionEnable = true;
+        config.HardwareLimitSwitch.ReverseLimitAutosetPositionValue = 0;
+
+        config.MotionMagic.MotionMagicCruiseVelocity = DEFAULT_MAXIMUM_VELOCITY;
+        config.MotionMagic.MotionMagicAcceleration = DEFAULT_MAXIMUM_ACCELERATION;
+        config.MotionMagic.MotionMagicJerk = config.MotionMagic.MotionMagicAcceleration * 10;
+
+        config.CurrentLimits.StatorCurrentLimitEnable = true;
+        config.CurrentLimits.StatorCurrentLimit = 80;
+
+        MOTOR.applyConfiguration(config);
+        MOTOR.setPhysicsSimulation(SIMULATION);
+
+        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);
+        MOTOR.registerSignal(TalonFXSignal.REVERSE_LIMIT, 100);
+    }
+
+    public enum ClimberState {
+        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 extendedSpeedScalar;
+        final double retractedSpeedScalar;
+
+        ClimberState(double targetExtendedPositionMeters, double targetRetractedPositionMeters, double extendedSpeedScalar, double retractedSpeedScalar) {
+            this.targetExtendedPositionMeters = targetExtendedPositionMeters;
+            this.targetRetractedPositionMeters = targetRetractedPositionMeters;
+            this.extendedSpeedScalar = extendedSpeedScalar;
+            this.retractedSpeedScalar = retractedSpeedScalar;
+        }
+    }
+}