1+ package frc .trigon .robot .subsystems .elevator ;
2+
3+ import com .ctre .phoenix6 .controls .DynamicMotionMagicVoltage ;
4+ import com .ctre .phoenix6 .controls .VoltageOut ;
5+ import edu .wpi .first .math .geometry .Pose3d ;
6+ import edu .wpi .first .math .geometry .Rotation3d ;
7+ import edu .wpi .first .math .geometry .Transform3d ;
8+ import edu .wpi .first .math .geometry .Translation3d ;
9+ import edu .wpi .first .units .Units ;
10+ import edu .wpi .first .wpilibj .sysid .SysIdRoutineLog ;
11+ import edu .wpi .first .wpilibj2 .command .sysid .SysIdRoutine ;
12+ import frc .trigon .robot .subsystems .MotorSubsystem ;
13+ import org .littletonrobotics .junction .Logger ;
14+ import lib .hardware .phoenix6 .talonfx .TalonFXMotor ;
15+ import lib .hardware .phoenix6 .talonfx .TalonFXSignal ;
16+ import lib .utilities .Conversions ;
17+
18+ public class Elevator extends MotorSubsystem {
19+ private final TalonFXMotor masterMotor = ElevatorConstants .MASTER_MOTOR ;
20+ private final VoltageOut voltageRequest = new VoltageOut (0 ).withEnableFOC (ElevatorConstants .FOC_ENABLED );
21+ private final DynamicMotionMagicVoltage positionRequest = new DynamicMotionMagicVoltage (
22+ 0 ,
23+ ElevatorConstants .DEFAULT_MAXIMUM_VELOCITY ,
24+ ElevatorConstants .DEFAULT_MAXIMUM_ACCELERATION ,
25+ ElevatorConstants .DEFAULT_MAXIMUM_ACCELERATION * 10
26+ ).withEnableFOC (ElevatorConstants .FOC_ENABLED );
27+ private ElevatorConstants .ElevatorState targetState = ElevatorConstants .ElevatorState .REST ;
28+
29+ public Elevator () {
30+ setName ("Elevator" );
31+ }
32+
33+ @ Override
34+ public SysIdRoutine .Config getSysIDConfig () {
35+ return ElevatorConstants .SYSID_CONFIG ;
36+ }
37+
38+ @ Override
39+ public void setBrake (boolean brake ) {
40+ masterMotor .setBrake (brake );
41+ }
42+
43+ @ Override
44+ public void stop () {
45+ masterMotor .stopMotor ();
46+ }
47+
48+ @ Override
49+ public void updateLog (SysIdRoutineLog log ) {
50+ log .motor ("Elevator" )
51+ .linearPosition (Units .Meters .of (getPositionRotations ()))
52+ .linearVelocity (Units .MetersPerSecond .of (masterMotor .getSignal (TalonFXSignal .VELOCITY )))
53+ .voltage (Units .Volts .of (masterMotor .getSignal (TalonFXSignal .MOTOR_VOLTAGE )));
54+ }
55+
56+ @ Override
57+ public void sysIDDrive (double targetVoltage ) {
58+ masterMotor .setControl (voltageRequest .withOutput (targetVoltage ));
59+ }
60+
61+ @ Override
62+ public void updateMechanism () {
63+ ElevatorConstants .MECHANISM .update (
64+ getPositionMeters (),
65+ rotationsToMeters (masterMotor .getSignal (TalonFXSignal .CLOSED_LOOP_REFERENCE ))
66+ );
67+
68+ Logger .recordOutput ("Poses/Components/ElevatorFirstPose" , getFirstStageComponentPose ());
69+ Logger .recordOutput ("Poses/Components/ElevatorSecondPose" , getSecondStageComponentPose ());
70+ }
71+
72+ @ Override
73+ public void updatePeriodically () {
74+ masterMotor .update ();
75+ Logger .recordOutput ("Elevator/CurrentPositionMeters" , getPositionMeters ());
76+ }
77+
78+ void setTargetState (ElevatorConstants .ElevatorState targetState ) {
79+ this .targetState = targetState ;
80+ scalePositionRequestSpeed (targetState .speedScalar );
81+ setTargetPositionRotations (metersToRotations (targetState .targetPositionMeters ));
82+ }
83+
84+ void setTargetPositionRotations (double targetPositionRotations ) {
85+ masterMotor .setControl (positionRequest .withPosition (targetPositionRotations ));
86+ }
87+
88+ private Pose3d getFirstStageComponentPose () {
89+ return calculateComponentPose (ElevatorConstants .ELEVATOR_FIRST_STAGE_VISUALIZATION_ORIGIN_POINT , getFirstStageComponentPoseHeight ());
90+ }
91+
92+ private Pose3d getSecondStageComponentPose () {
93+ return calculateComponentPose (ElevatorConstants .ELEVATOR_SECOND_STAGE_VISUALIZATION_ORIGIN_POINT , getPositionMeters ());
94+ }
95+
96+ private double getFirstStageComponentPoseHeight () {
97+ if (isSecondStageComponentLimitReached ())
98+ return getPositionMeters () - ElevatorConstants .SECOND_ELEVATOR_COMPONENT_EXTENDED_LENGTH_METERS ;
99+ return 0 ;
100+ }
101+
102+ private boolean isSecondStageComponentLimitReached () {
103+ return getPositionMeters () > ElevatorConstants .SECOND_ELEVATOR_COMPONENT_EXTENDED_LENGTH_METERS ;
104+ }
105+
106+ private Pose3d calculateComponentPose (Pose3d originPoint , double currentHeight ) {
107+ final Transform3d elevatorTransform = new Transform3d (
108+ new Translation3d (0 , 0 , currentHeight ),
109+ new Rotation3d ()
110+ );
111+ return originPoint .transformBy (elevatorTransform );
112+ }
113+
114+ private void scalePositionRequestSpeed (double speedScalar ) {
115+ positionRequest .Velocity = ElevatorConstants .DEFAULT_MAXIMUM_VELOCITY * speedScalar ;
116+ positionRequest .Acceleration = ElevatorConstants .DEFAULT_MAXIMUM_ACCELERATION * speedScalar ;
117+ positionRequest .Jerk = positionRequest .Acceleration * 10 ;
118+ }
119+
120+ private double getPositionMeters () {
121+ return rotationsToMeters (getPositionRotations ());
122+ }
123+
124+ private double rotationsToMeters (double positionsRotations ) {
125+ return Conversions .rotationsToDistance (positionsRotations , ElevatorConstants .DRUM_DIAMETER_METERS );
126+ }
127+
128+ private double metersToRotations (double positionsMeters ) {
129+ return Conversions .distanceToRotations (positionsMeters , ElevatorConstants .DRUM_DIAMETER_METERS );
130+ }
131+
132+ private double getPositionRotations () {
133+ return masterMotor .getSignal (TalonFXSignal .POSITION );
134+ }
135+ }
0 commit comments