Elevator

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

@@ -21,6 +21,9 @@
 import frc.trigon.robot.poseestimation.poseestimator.PoseEstimator;
 import frc.trigon.robot.subsystems.MotorSubsystem;
 
+import frc.trigon.robot.subsystems.elevator.Elevator;
+import frc.trigon.robot.subsystems.elevator.ElevatorCommands;
+import frc.trigon.robot.subsystems.elevator.ElevatorConstants;
 import frc.trigon.robot.subsystems.arm.Arm;
 import frc.trigon.robot.subsystems.arm.ArmCommands;
 import frc.trigon.robot.subsystems.arm.ArmConstants;
@@ -50,6 +53,7 @@ public class RobotContainer {
     public static final Swerve SWERVE = new Swerve();
     public static final Arm ARM = new Arm();
     public static final Climber CLIMBER = new Climber();
+    public static final Elevator ELEVATOR = new Elevator();
     public static final Intake INTAKE = new Intake();
     public static final Transporter TRANSPORTER = new Transporter();
 
@@ -77,6 +81,7 @@ private void bindDefaultCommands() {
         SWERVE.setDefaultCommand(GeneralCommands.getFieldRelativeDriveCommand());
         ARM.setDefaultCommand(ArmCommands.getSetTargetStateCommand(ArmConstants.ArmState.REST));
         CLIMBER.setDefaultCommand(ClimberCommands.getSetTargetStateCommand(ClimberConstants.ClimberState.REST));
+        ELEVATOR.setDefaultCommand(ElevatorCommands.getSetTargetStateCommand(ElevatorConstants.ElevatorState.REST));
         INTAKE.setDefaultCommand(IntakeCommands.getSetTargetStateCommand(IntakeConstants.IntakeState.REST));
         TRANSPORTER.setDefaultCommand(TransporterCommands.getSetTargetStateCommand(TransporterConstants.TransporterState.REST));
     }

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

@@ -48,7 +48,7 @@ public class ClimberConstants {
             GROUNDED_PID_SLOT = 0,
             ON_CAGE_PID_SLOT = 1;
     private static final double GEAR_RATIO = 37.5;
-    private static final double REVERSE_LIMIT_SENSOR_RESET_POSITION = 0;
+    private static final double REVERSE_LIMIT_RESET_POSITION_ROTATIONS = 0;
     private static final double FORWARD_SOFT_LIMIT_POSITION_ROTATIONS = 3;
     static final boolean FOC_ENABLED = true;
 
@@ -129,7 +129,7 @@ private static void configureMotor() {
         config.SoftwareLimitSwitch.ForwardSoftLimitThreshold = FORWARD_SOFT_LIMIT_POSITION_ROTATIONS;
 
         config.SoftwareLimitSwitch.ReverseSoftLimitEnable = true;
-        config.SoftwareLimitSwitch.ReverseSoftLimitThreshold = REVERSE_LIMIT_SENSOR_RESET_POSITION;
+        config.SoftwareLimitSwitch.ReverseSoftLimitThreshold = REVERSE_LIMIT_RESET_POSITION_ROTATIONS;
 
         config.Feedback.SensorToMechanismRatio = GEAR_RATIO;
 
@@ -147,7 +147,7 @@ private static void configureMotor() {
 
     private static void configureReverseLimitSensor() {
         REVERSE_LIMIT_SENSOR.setSimulationSupplier(REVERSE_LIMIT_SENSORS_SIMULATION_SUPPLIER);
-        REVERSE_LIMIT_SENSOR_BOOLEAN_EVENT.ifHigh(() -> MOTOR.setPosition(REVERSE_LIMIT_SENSOR_RESET_POSITION));
+        REVERSE_LIMIT_SENSOR_BOOLEAN_EVENT.ifHigh(() -> MOTOR.setPosition(REVERSE_LIMIT_RESET_POSITION_ROTATIONS));
     }
 
     public enum ClimberState {

src/main/java/frc/trigon/robot/subsystems/elevator/Elevator.java

@@ -0,0 +1,135 @@
+package frc.trigon.robot.subsystems.elevator;
+
+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.robot.subsystems.MotorSubsystem;
+import org.littletonrobotics.junction.Logger;
+import lib.hardware.phoenix6.talonfx.TalonFXMotor;
+import lib.hardware.phoenix6.talonfx.TalonFXSignal;
+import lib.utilities.Conversions;
+
+public class Elevator extends MotorSubsystem {
+    private final TalonFXMotor masterMotor = ElevatorConstants.MASTER_MOTOR;
+    private final VoltageOut voltageRequest = new VoltageOut(0).withEnableFOC(ElevatorConstants.FOC_ENABLED);
+    private final DynamicMotionMagicVoltage positionRequest = new DynamicMotionMagicVoltage(
+            0,
+            ElevatorConstants.DEFAULT_MAXIMUM_VELOCITY,
+            ElevatorConstants.DEFAULT_MAXIMUM_ACCELERATION,
+            ElevatorConstants.DEFAULT_MAXIMUM_ACCELERATION * 10
+            ).withEnableFOC(ElevatorConstants.FOC_ENABLED);
+    private ElevatorConstants.ElevatorState targetState = ElevatorConstants.ElevatorState.REST;
+
+    public Elevator() {
+        setName("Elevator");
+    }
+
+    @Override
+    public SysIdRoutine.Config getSysIDConfig() {
+        return ElevatorConstants.SYSID_CONFIG;
+    }
+
+    @Override
+    public void setBrake(boolean brake) {
+        masterMotor.setBrake(brake);
+    }
+
+    @Override
+    public void stop() {
+        masterMotor.stopMotor();
+    }
+
+    @Override
+    public void updateLog(SysIdRoutineLog log) {
+        log.motor("Elevator")
+                .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 sysIDDrive(double targetVoltage) {
+        masterMotor.setControl(voltageRequest.withOutput(targetVoltage));
+    }
+
+    @Override
+    public void updateMechanism() {
+        ElevatorConstants.MECHANISM.update(
+                getPositionMeters(),
+                rotationsToMeters(masterMotor.getSignal(TalonFXSignal.CLOSED_LOOP_REFERENCE))
+        );
+
+        Logger.recordOutput("Poses/Components/ElevatorFirstPose", getFirstStageComponentPose());
+        Logger.recordOutput("Poses/Components/ElevatorSecondPose", getSecondStageComponentPose());
+    }
+
+    @Override
+    public void updatePeriodically() {
+        masterMotor.update();
+        Logger.recordOutput("Elevator/CurrentPositionMeters", getPositionMeters());
+    }
+
+    void setTargetState(ElevatorConstants.ElevatorState targetState) {
+        this.targetState = targetState;
+        scalePositionRequestSpeed(targetState.speedScalar);
+        setTargetPositionRotations(metersToRotations(targetState.targetPositionMeters));
+    }
+
+    void setTargetPositionRotations(double targetPositionRotations) {
+        masterMotor.setControl(positionRequest.withPosition(targetPositionRotations));
+    }
+
+    private Pose3d getFirstStageComponentPose() {
+        return calculateComponentPose(ElevatorConstants.ELEVATOR_FIRST_STAGE_VISUALIZATION_ORIGIN_POINT, getFirstStageComponentPoseHeight());
+    }
+
+    private Pose3d getSecondStageComponentPose() {
+        return calculateComponentPose(ElevatorConstants.ELEVATOR_SECOND_STAGE_VISUALIZATION_ORIGIN_POINT, getPositionMeters());
+    }
+
+    private double getFirstStageComponentPoseHeight() {
+        if (isSecondStageComponentLimitReached())
+            return getPositionMeters() - ElevatorConstants.SECOND_ELEVATOR_COMPONENT_EXTENDED_LENGTH_METERS;
+        return 0;
+    }
+
+    private boolean isSecondStageComponentLimitReached() {
+        return getPositionMeters() > ElevatorConstants.SECOND_ELEVATOR_COMPONENT_EXTENDED_LENGTH_METERS;
+    }
+
+    private Pose3d calculateComponentPose(Pose3d originPoint, double currentHeight) {
+        final Transform3d elevatorTransform = new Transform3d(
+                new Translation3d(0, 0, currentHeight),
+                new Rotation3d()
+        );
+        return originPoint.transformBy(elevatorTransform);
+    }
+
+    private void scalePositionRequestSpeed(double speedScalar) {
+        positionRequest.Velocity = ElevatorConstants.DEFAULT_MAXIMUM_VELOCITY * speedScalar;
+        positionRequest.Acceleration = ElevatorConstants.DEFAULT_MAXIMUM_ACCELERATION * speedScalar;
+        positionRequest.Jerk = positionRequest.Acceleration * 10;
+    }
+
+    private double getPositionMeters() {
+        return rotationsToMeters(getPositionRotations());
+    }
+
+    private double rotationsToMeters(double positionsRotations) {
+        return Conversions.rotationsToDistance(positionsRotations, ElevatorConstants.DRUM_DIAMETER_METERS);
+    }
+
+    private double metersToRotations(double positionsMeters) {
+        return Conversions.distanceToRotations(positionsMeters, ElevatorConstants.DRUM_DIAMETER_METERS);
+    }
+
+    private double getPositionRotations() {
+        return masterMotor.getSignal(TalonFXSignal.POSITION);
+    }
+}

src/main/java/frc/trigon/robot/subsystems/elevator/ElevatorCommands.java

@@ -0,0 +1,35 @@
+package frc.trigon.robot.subsystems.elevator;
+
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.StartEndCommand;
+import frc.trigon.robot.RobotContainer;
+import lib.commands.NetworkTablesCommand;
+
+import java.util.Set;
+
+public class ElevatorCommands {
+    public static Command getDebbugingCommand() {
+        return new NetworkTablesCommand(
+                RobotContainer.ELEVATOR::setTargetPositionRotations,
+                false,
+                Set.of(RobotContainer.ELEVATOR),
+                "Debugging/ElevatorTargetPositionRotations"
+        );
+    }
+
+    public static Command getSetTargetStateCommand(ElevatorConstants.ElevatorState targetState) {
+        return new StartEndCommand(
+                () -> RobotContainer.ELEVATOR.setTargetState(targetState),
+                RobotContainer.ELEVATOR::stop,
+                RobotContainer.ELEVATOR
+        );
+    }
+
+    public static Command getSetTargetStateCommand(double targetPositionRotations) {
+        return new StartEndCommand(
+                () -> RobotContainer.ELEVATOR.setTargetPositionRotations(targetPositionRotations),
+                RobotContainer.ELEVATOR::stop,
+                RobotContainer.ELEVATOR
+        );
+    }
+}