From 3ea5b9d652faa1db1dc33e7fc165acece07c97f3 Mon Sep 17 00:00:00 2001 From: Veselin Dikov Date: Sat, 3 Jan 2026 14:35:31 -0800 Subject: [PATCH 1/3] Annotate some points of repetition in the ElevatorIO implementations for future refactoring. --- .../subsystems/elevator/ElevatorIOReal.java | 21 +++++++++++++++++++ .../subsystems/elevator/ElevatorIOSim.java | 4 ++++ 2 files changed, 25 insertions(+) diff --git a/src/main/java/frc/robot/subsystems/elevator/ElevatorIOReal.java b/src/main/java/frc/robot/subsystems/elevator/ElevatorIOReal.java index a8cae27..4d74dc6 100644 --- a/src/main/java/frc/robot/subsystems/elevator/ElevatorIOReal.java +++ b/src/main/java/frc/robot/subsystems/elevator/ElevatorIOReal.java @@ -17,11 +17,15 @@ public class ElevatorIOReal implements ElevatorIO { + // TODO(vdikov): Looks like IOReal and IOSim could share the motor instance. private TalonFX motor; + // TODO(vdikov): Ideally, IOSim and IOReal work off the same motorConfig + // (though PID values might have to be kept separately) private TalonFXConfiguration motorConfig; private final PositionVoltage positionControl = new PositionVoltage(0); + // TODO(vdikov): this definitely should be moved to Elevator. private Angle currentMotorSetpoint = Rotations.of(0); public ElevatorIOReal() { @@ -43,6 +47,23 @@ public ElevatorIOReal() { // Config motor inversion. motorConfig.withMotorOutput(new MotorOutputConfigs().withInverted(InvertedValue.Clockwise_Positive)); + // TODO(vdikov): Discuss with Stefan if we should adopt this as a more + // self-contained way to configure the motor. + // motorConfig = new TalonFXConfiguration() + // .withSlot0( + // new Slot0Configs() // Motor PID and gain values. + // .withKP(ELEVATOR_kP) + // .withKI(ELEVATOR_kI) + // .withKD(ELEVATOR_kD) + // .withKS(ELEVATOR_kS) + // .withKV(ELEVATOR_kV) + // .withKA(ELEVATOR_kA) + // .withKG(ELEVATOR_kG)) + // .withMotorOutput( + // new MotorOutputConfigs() + // .withInverted(InvertedValue.Clockwise_Positive) // Invert motor rotation. + // ); + motor.getConfigurator().apply(motorConfig); } diff --git a/src/main/java/frc/robot/subsystems/elevator/ElevatorIOSim.java b/src/main/java/frc/robot/subsystems/elevator/ElevatorIOSim.java index 7c3dd18..be6420b 100644 --- a/src/main/java/frc/robot/subsystems/elevator/ElevatorIOSim.java +++ b/src/main/java/frc/robot/subsystems/elevator/ElevatorIOSim.java @@ -32,13 +32,17 @@ public class ElevatorIOSim implements ElevatorIO { true, 0.00); + // TODO(vdikov): Looks like IOReal and IOSim could share the motor instance. private TalonFX motor; private TalonFXSimState motorSim; + // TODO(vdikov): Ideally, IOSim and IOReal work off the same motorConfig + // (though PID values might have to be kept separately) private TalonFXConfiguration motorConfig; // Used for actually moving the motor to a given position with PID applied to a voltage input. private final PositionVoltage positionControl = new PositionVoltage(0); + // TODO(vdikov): This shold definitely move to Elevator. private Angle currentMotorSetpoint = Rotations.of(0); public ElevatorIOSim() { From 6d1a615af00e3a10f60f1ebece9125d472e3d319 Mon Sep 17 00:00:00 2001 From: Veselin Dikov Date: Sat, 3 Jan 2026 15:22:31 -0800 Subject: [PATCH 2/3] Refactor Elevator{IO} to avoid code repetition --- .../robot/subsystems/elevator/Elevator.java | 37 ++++++++++-- .../robot/subsystems/elevator/ElevatorIO.java | 9 ++- .../subsystems/elevator/ElevatorIOReal.java | 58 ++----------------- .../subsystems/elevator/ElevatorIOSim.java | 36 ++---------- 4 files changed, 51 insertions(+), 89 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/elevator/Elevator.java b/src/main/java/frc/robot/subsystems/elevator/Elevator.java index e4771f7..4214ee1 100644 --- a/src/main/java/frc/robot/subsystems/elevator/Elevator.java +++ b/src/main/java/frc/robot/subsystems/elevator/Elevator.java @@ -4,11 +4,18 @@ import static edu.wpi.first.units.Units.Rotations; import static edu.wpi.first.units.Units.Volts; +import com.ctre.phoenix6.configs.MotorOutputConfigs; +import com.ctre.phoenix6.configs.Slot0Configs; +import com.ctre.phoenix6.configs.TalonFXConfiguration; +import com.ctre.phoenix6.hardware.TalonFX; +import com.ctre.phoenix6.signals.InvertedValue; +import com.ctre.phoenix6.signals.NeutralModeValue; import edu.wpi.first.units.measure.Angle; import edu.wpi.first.units.measure.Distance; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.Constants; import frc.robot.subsystems.SimulationVisualizer; import org.littletonrobotics.junction.Logger; @@ -25,6 +32,25 @@ public class Elevator extends SubsystemBase { * @param io The hardware implementation for the elevator, either sim or real. */ public Elevator(ElevatorIO io) { + var motorConfig = new TalonFXConfiguration() + .withSlot0( + new Slot0Configs() // Motor PID and gain values. + .withKP(ElevatorConstants.ELEVATOR_kP) + .withKI(ElevatorConstants.ELEVATOR_kI) + .withKD(ElevatorConstants.ELEVATOR_kD) + .withKS(ElevatorConstants.ELEVATOR_kS) + .withKV(ElevatorConstants.ELEVATOR_kV) + .withKA(ElevatorConstants.ELEVATOR_kA) + .withKG(ElevatorConstants.ELEVATOR_kG)) + .withMotorOutput( + new MotorOutputConfigs() + .withInverted(InvertedValue.Clockwise_Positive) // Invert motor rotation. + ); + var motor = new TalonFX(Constants.ELEVATOR_ID); + motor.setNeutralMode(NeutralModeValue.Brake); + motor.getConfigurator().apply(motorConfig); + + io.setMotor(motor); this.io = io; } @@ -40,8 +66,11 @@ public void periodic() { // instead. Logger.processInputs("Elevator", replayedInputs); Logger.recordOutput( - "Elevator/Desired Carriage Position", + "Elevator/Carriage Setpoint (inches)", currentElevatorSetpoint.getPosition().in(Inches)); + Logger.recordOutput( + "Elevator/Motor Setpoint (rotations)", + currentElevatorSetpoint.getPositionAngle().in(Rotations)); } @Override @@ -49,9 +78,9 @@ public void simulationPeriodic() { SimulationVisualizer.getInstance().updateElevatorHeight(Inches.of(replayedInputs.carriagePositionInches)); } - public void setElevatorPosition(ElevatorHeight height) { - currentElevatorSetpoint = height; - io.setMotorSetpoint(height.getPositionAngle()); + public void setElevatorPosition(ElevatorHeight heightSetpoint) { + currentElevatorSetpoint = heightSetpoint; + io.setMotorSetpoint(heightSetpoint.getPositionAngle()); } public Command setElevatorPositionCommand(ElevatorHeight height) { diff --git a/src/main/java/frc/robot/subsystems/elevator/ElevatorIO.java b/src/main/java/frc/robot/subsystems/elevator/ElevatorIO.java index 99b74dd..e7efa29 100644 --- a/src/main/java/frc/robot/subsystems/elevator/ElevatorIO.java +++ b/src/main/java/frc/robot/subsystems/elevator/ElevatorIO.java @@ -3,6 +3,7 @@ import static edu.wpi.first.units.Units.Meters; import static edu.wpi.first.units.Units.Rotations; +import com.ctre.phoenix6.hardware.TalonFX; import edu.wpi.first.units.measure.Angle; import edu.wpi.first.units.measure.Distance; import edu.wpi.first.units.measure.Voltage; @@ -13,6 +14,7 @@ public interface ElevatorIO { @AutoLog class ElevatorIOInputs { public double carriagePositionInches = 0.0; + // public double motorSetpointRotations = 0.0; public double motorRotations = 0.0; public double motorVelocityRotsPerSecond = 0.0; public double motorCurrent = 0.0; @@ -20,7 +22,12 @@ class ElevatorIOInputs { } /** - * Used to update Advantage kit autologged input data, as well as any other necessary states (like in sim) + * Sets the TalonFX motor configuraiton in the IO implementation. + */ + default void setMotor(TalonFX motor) {} + + /** + * Updates Advantage kit autologged input data, as well as any other necessary states (like in sim) * @param inputs The "struct" (data class) to handle hardware inputs. */ default void updateState(ElevatorIOInputs inputs) {} diff --git a/src/main/java/frc/robot/subsystems/elevator/ElevatorIOReal.java b/src/main/java/frc/robot/subsystems/elevator/ElevatorIOReal.java index 4d74dc6..4c22d6b 100644 --- a/src/main/java/frc/robot/subsystems/elevator/ElevatorIOReal.java +++ b/src/main/java/frc/robot/subsystems/elevator/ElevatorIOReal.java @@ -3,68 +3,24 @@ import static edu.wpi.first.units.Units.*; import static frc.robot.subsystems.elevator.ElevatorConstants.*; -import com.ctre.phoenix6.configs.MotorOutputConfigs; -import com.ctre.phoenix6.configs.TalonFXConfiguration; import com.ctre.phoenix6.controls.PositionVoltage; import com.ctre.phoenix6.hardware.TalonFX; -import com.ctre.phoenix6.signals.InvertedValue; -import com.ctre.phoenix6.signals.NeutralModeValue; import edu.wpi.first.units.measure.Angle; import edu.wpi.first.units.measure.Distance; import edu.wpi.first.units.measure.Voltage; -import frc.robot.Constants; -import org.littletonrobotics.junction.Logger; public class ElevatorIOReal implements ElevatorIO { // TODO(vdikov): Looks like IOReal and IOSim could share the motor instance. private TalonFX motor; - // TODO(vdikov): Ideally, IOSim and IOReal work off the same motorConfig - // (though PID values might have to be kept separately) - private TalonFXConfiguration motorConfig; - private final PositionVoltage positionControl = new PositionVoltage(0); + private final PositionVoltage positionControl = new PositionVoltage(Rotations.of(0)); - // TODO(vdikov): this definitely should be moved to Elevator. - private Angle currentMotorSetpoint = Rotations.of(0); + public ElevatorIOReal() {} - public ElevatorIOReal() { - motor = new TalonFX(Constants.ELEVATOR_ID); - motor.setNeutralMode(NeutralModeValue.Brake); - - motorConfig = new TalonFXConfiguration(); - - // Config PID - var slot0PIDConfig = motorConfig.Slot0; - slot0PIDConfig.kP = ELEVATOR_kP; - slot0PIDConfig.kI = ELEVATOR_kI; - slot0PIDConfig.kD = ELEVATOR_kD; - slot0PIDConfig.kS = ELEVATOR_kS; - slot0PIDConfig.kV = ELEVATOR_kV; - slot0PIDConfig.kA = ELEVATOR_kA; - slot0PIDConfig.kG = ELEVATOR_kG; - - // Config motor inversion. - motorConfig.withMotorOutput(new MotorOutputConfigs().withInverted(InvertedValue.Clockwise_Positive)); - - // TODO(vdikov): Discuss with Stefan if we should adopt this as a more - // self-contained way to configure the motor. - // motorConfig = new TalonFXConfiguration() - // .withSlot0( - // new Slot0Configs() // Motor PID and gain values. - // .withKP(ELEVATOR_kP) - // .withKI(ELEVATOR_kI) - // .withKD(ELEVATOR_kD) - // .withKS(ELEVATOR_kS) - // .withKV(ELEVATOR_kV) - // .withKA(ELEVATOR_kA) - // .withKG(ELEVATOR_kG)) - // .withMotorOutput( - // new MotorOutputConfigs() - // .withInverted(InvertedValue.Clockwise_Positive) // Invert motor rotation. - // ); - - motor.getConfigurator().apply(motorConfig); + @Override + public void setMotor(TalonFX motor) { + this.motor = motor; } @Override @@ -75,14 +31,10 @@ public void updateState(ElevatorIOInputs inputs) { inputs.motorVelocityRotsPerSecond = motor.getVelocity().getValueAsDouble(); inputs.motorCurrent = motor.getStatorCurrent().getValueAsDouble(); inputs.motorVoltage = motor.getMotorVoltage().getValueAsDouble(); - - // Remember to explicitly state the unit. - Logger.recordOutput("Elevator/motor/SetpointRotations", currentMotorSetpoint.in(Rotations)); } @Override public void setMotorSetpoint(Angle setpoint) { - currentMotorSetpoint = setpoint; motor.setControl(positionControl.withPosition(setpoint)); } diff --git a/src/main/java/frc/robot/subsystems/elevator/ElevatorIOSim.java b/src/main/java/frc/robot/subsystems/elevator/ElevatorIOSim.java index be6420b..de70f34 100644 --- a/src/main/java/frc/robot/subsystems/elevator/ElevatorIOSim.java +++ b/src/main/java/frc/robot/subsystems/elevator/ElevatorIOSim.java @@ -2,13 +2,8 @@ import static edu.wpi.first.units.Units.*; -import com.ctre.phoenix6.configs.MotorOutputConfigs; -import com.ctre.phoenix6.configs.Slot0Configs; -import com.ctre.phoenix6.configs.TalonFXConfiguration; import com.ctre.phoenix6.controls.PositionVoltage; import com.ctre.phoenix6.hardware.TalonFX; -import com.ctre.phoenix6.signals.InvertedValue; -import com.ctre.phoenix6.signals.NeutralModeValue; import com.ctre.phoenix6.sim.TalonFXSimState; import edu.wpi.first.math.system.plant.DCMotor; import edu.wpi.first.math.util.Units; @@ -16,7 +11,6 @@ import edu.wpi.first.units.measure.Distance; import edu.wpi.first.units.measure.Voltage; import edu.wpi.first.wpilibj.simulation.ElevatorSim; -import frc.robot.Constants; import org.littletonrobotics.junction.Logger; public class ElevatorIOSim implements ElevatorIO { @@ -35,33 +29,16 @@ public class ElevatorIOSim implements ElevatorIO { // TODO(vdikov): Looks like IOReal and IOSim could share the motor instance. private TalonFX motor; private TalonFXSimState motorSim; - // TODO(vdikov): Ideally, IOSim and IOReal work off the same motorConfig - // (though PID values might have to be kept separately) - private TalonFXConfiguration motorConfig; // Used for actually moving the motor to a given position with PID applied to a voltage input. private final PositionVoltage positionControl = new PositionVoltage(0); - // TODO(vdikov): This shold definitely move to Elevator. - private Angle currentMotorSetpoint = Rotations.of(0); + public ElevatorIOSim() {} - public ElevatorIOSim() { - motor = new TalonFX(Constants.ELEVATOR_ID); - motor.setNeutralMode(NeutralModeValue.Brake); - - motorSim = motor.getSimState(); - - motorConfig = new TalonFXConfiguration(); - - motorConfig.withSlot0(new Slot0Configs() - .withKP(ElevatorConstants.ELEVATOR_kP) - .withKI(ElevatorConstants.ELEVATOR_kI) - .withKD(ElevatorConstants.ELEVATOR_kD)); - - // This needs to be CounterClockwise, else sim won't work propper, though it is clockwise on the bot. - motorConfig.withMotorOutput(new MotorOutputConfigs().withInverted(InvertedValue.CounterClockwise_Positive)); - - motor.getConfigurator().apply(motorConfig); + @Override + public void setMotor(TalonFX motor) { + this.motorSim = motor.getSimState(); + this.motor = motor; } @Override @@ -84,8 +61,6 @@ private void updateSim() { // Logs to "Real Outputs" NT Logger.recordOutput("Simulated Elevator/motorSim/Voltage", motorSim.getMotorVoltage()); - // Explicitly stating the unit prevents confusion when looking at the value in adv. scope. - Logger.recordOutput("Simulated Elevator/motor/SetpointRotations", currentMotorSetpoint.in(Rotations)); Logger.recordOutput("Simulated Elevator/elevatorSim/hitsUpperLimit", elevatorSim.hasHitUpperLimit()); Logger.recordOutput("Simulated Elevator/elevatorSim/hitsLowerLimit", elevatorSim.hasHitLowerLimit()); @@ -103,7 +78,6 @@ private void updateSim() { @Override public void setMotorSetpoint(Angle setpoint) { - currentMotorSetpoint = setpoint; motor.setControl(positionControl.withPosition(setpoint)); } From 69ef7779fe88c9a6b9bab63f6d2e165920fb99a6 Mon Sep 17 00:00:00 2001 From: Veselin Dikov Date: Sat, 3 Jan 2026 16:26:03 -0800 Subject: [PATCH 3/3] Make elevationSim work with inverted motor to be consistent with REAL robot motor config --- .../robot/subsystems/elevator/Elevator.java | 25 +++++++++--------- .../subsystems/elevator/ElevatorIOSim.java | 26 ++++++++++--------- 2 files changed, 26 insertions(+), 25 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/elevator/Elevator.java b/src/main/java/frc/robot/subsystems/elevator/Elevator.java index 4214ee1..ce5c784 100644 --- a/src/main/java/frc/robot/subsystems/elevator/Elevator.java +++ b/src/main/java/frc/robot/subsystems/elevator/Elevator.java @@ -32,20 +32,19 @@ public class Elevator extends SubsystemBase { * @param io The hardware implementation for the elevator, either sim or real. */ public Elevator(ElevatorIO io) { + var slot0Config = new Slot0Configs() // Motor PID and gain values. + .withKP(ElevatorConstants.ELEVATOR_kP) + .withKI(ElevatorConstants.ELEVATOR_kI) + .withKD(ElevatorConstants.ELEVATOR_kD) + .withKS(ElevatorConstants.ELEVATOR_kS) + .withKV(ElevatorConstants.ELEVATOR_kV) + .withKA(ElevatorConstants.ELEVATOR_kA) + .withKG(ElevatorConstants.ELEVATOR_kG); var motorConfig = new TalonFXConfiguration() - .withSlot0( - new Slot0Configs() // Motor PID and gain values. - .withKP(ElevatorConstants.ELEVATOR_kP) - .withKI(ElevatorConstants.ELEVATOR_kI) - .withKD(ElevatorConstants.ELEVATOR_kD) - .withKS(ElevatorConstants.ELEVATOR_kS) - .withKV(ElevatorConstants.ELEVATOR_kV) - .withKA(ElevatorConstants.ELEVATOR_kA) - .withKG(ElevatorConstants.ELEVATOR_kG)) - .withMotorOutput( - new MotorOutputConfigs() - .withInverted(InvertedValue.Clockwise_Positive) // Invert motor rotation. - ); + .withSlot0(slot0Config) + // .withMotorOutput(new MotorOutputConfigs().withInverted(InvertedValue.CounterClockwise_Positive)); + // Invert motor rotation. + .withMotorOutput(new MotorOutputConfigs().withInverted(InvertedValue.Clockwise_Positive)); var motor = new TalonFX(Constants.ELEVATOR_ID); motor.setNeutralMode(NeutralModeValue.Brake); motor.getConfigurator().apply(motorConfig); diff --git a/src/main/java/frc/robot/subsystems/elevator/ElevatorIOSim.java b/src/main/java/frc/robot/subsystems/elevator/ElevatorIOSim.java index de70f34..4f9dad1 100644 --- a/src/main/java/frc/robot/subsystems/elevator/ElevatorIOSim.java +++ b/src/main/java/frc/robot/subsystems/elevator/ElevatorIOSim.java @@ -24,14 +24,13 @@ public class ElevatorIOSim implements ElevatorIO { ElevatorConstants.ELEVATOR_MIN_HEIGHT.in(Meter), ElevatorConstants.ELEVATOR_MAX_HEIGHT.in(Meter), true, - 0.00); + ElevatorConstants.ELEVATOR_MIN_HEIGHT.in(Meter)); - // TODO(vdikov): Looks like IOReal and IOSim could share the motor instance. private TalonFX motor; private TalonFXSimState motorSim; // Used for actually moving the motor to a given position with PID applied to a voltage input. - private final PositionVoltage positionControl = new PositionVoltage(0); + private final PositionVoltage positionControl = new PositionVoltage(Rotations.of(0)); public ElevatorIOSim() {} @@ -55,25 +54,28 @@ public void updateState(ElevatorIOInputs inputs) { private void updateSim() { motorSim.setSupplyVoltage(Volts.of(12)); + double motorInverted = -1.0; // -1 for inverted, 1 for forward motor. // Apply the voltage to the sim elevator that we apply to the sim motor. - elevatorSim.setInputVoltage(motorSim.getMotorVoltage()); + // Negating the sim motor value since it is set to use negative value when pushing + // the cartrage UP. + elevatorSim.setInputVoltage(motorInverted * motorSim.getMotorVoltage()); + elevatorSim.update(0.02); // Same update cycle as an actual robot, 20 ms. // Logs to "Real Outputs" NT Logger.recordOutput("Simulated Elevator/motorSim/Voltage", motorSim.getMotorVoltage()); + Logger.recordOutput("Simulated Elevator/elevatorSim/position (meters)", elevatorSim.getPositionMeters()); Logger.recordOutput("Simulated Elevator/elevatorSim/hitsUpperLimit", elevatorSim.hasHitUpperLimit()); Logger.recordOutput("Simulated Elevator/elevatorSim/hitsLowerLimit", elevatorSim.hasHitLowerLimit()); - elevatorSim.update(0.02); // Same update cycle as an actual robot, 20 ms. - - motorSim.setRawRotorPosition(getMotorRotations(elevatorSim.getPositionMeters())); + motorSim.setRawRotorPosition(motorInverted * getMotorRotations(elevatorSim.getPositionMeters())); // angular velocity = linear velocity / radius, taken also from 5414 - motorSim.setRotorVelocity( - ((elevatorSim.getVelocityMetersPerSecond() / ElevatorConstants.ELEVATOR_SPOOL_RADIUS.in(Meters)) - // radians/sec to rotations/sec - / (2.0 * Math.PI)) - * ElevatorConstants.MOTOR_TO_ELEVATOR_GEARING); + motorSim.setRotorVelocity(motorInverted + * ((elevatorSim.getVelocityMetersPerSecond() / ElevatorConstants.ELEVATOR_SPOOL_RADIUS.in(Meters)) + // radians/sec to rotations/sec + / (2.0 * Math.PI)) + * ElevatorConstants.MOTOR_TO_ELEVATOR_GEARING); } @Override