Skip to content
This repository was archived by the owner on Jan 17, 2026. It is now read-only.

Commit e6ddb8e

Browse files
spacey-sootyspacey-sooty
committed
[elevator] Rewrite sim code, use motion magic, refactor some things
Signed-off-by: Jade Turner <spacey-sooty@proton.me>
1 parent 9a010a6 commit e6ddb8e

File tree

6 files changed

+116
-49
lines changed

6 files changed

+116
-49
lines changed

‎src/main/java/org/curtinfrc/frc2025/Constants.java‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -15,7 +15,7 @@
1515
* (log replay from a file).
1616
*/
1717
public final class Constants {
18-
public static final RobotType robotType = RobotType.COMPBOT;
18+
public static final RobotType robotType = RobotType.SIMBOT;
1919
public static final double ROBOT_X = 0.705;
2020
public static final double ROBOT_Y = 0.730;
2121
public static final double FIELD_LENGTH = aprilTagLayout.getFieldLength();

‎src/main/java/org/curtinfrc/frc2025/Robot.java‎

Lines changed: 20 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -273,6 +273,26 @@ public Robot() {
273273
drive,
274274
drive::logTrajectory);
275275

276+
autoChooser.addCmd(
277+
"L3 No end", () -> elevator.goToSetpoint(ElevatorSetpoints.L3, intake.backSensor.negate()));
278+
279+
autoChooser.addCmd(
280+
"L2 No end", () -> elevator.goToSetpoint(ElevatorSetpoints.L2, intake.backSensor.negate()));
281+
282+
autoChooser.addCmd(
283+
"L3 End",
284+
() ->
285+
elevator
286+
.goToSetpoint(ElevatorSetpoints.L3, intake.backSensor.negate())
287+
.until(elevator.atSetpoint));
288+
289+
autoChooser.addCmd(
290+
"L2 End",
291+
() ->
292+
elevator
293+
.goToSetpoint(ElevatorSetpoints.L2, intake.backSensor.negate())
294+
.until(elevator.atSetpoint));
295+
276296
autoChooser.addRoutine("Test Path", () -> Autos.path("Test Path", factory, drive));
277297
autoChooser.addRoutine(
278298
"Three Piece Left", () -> Autos.threePieceLeft(factory, drive, ejector, elevator, intake));

‎src/main/java/org/curtinfrc/frc2025/subsystems/elevator/Elevator.java‎

Lines changed: 17 additions & 10 deletions
Original file line numberDiff line numberDiff line change
@@ -29,14 +29,7 @@ public class Elevator extends SubsystemBase {
2929
new Trigger(() -> setpoint != ElevatorSetpoints.BASE && setpoint != ElevatorSetpoints.L1);
3030

3131
public final Trigger toZero = new Trigger(() -> inputs.hominSensor);
32-
public final Trigger atSetpoint =
33-
new Trigger(
34-
() -> {
35-
var refrence = setpoint.setpoint / (Math.PI * 2 * 0.03055) * 8.1818;
36-
var current = inputs.positionRotations;
37-
38-
return (refrence - current) < 0.85248701367;
39-
});
32+
public final Trigger atSetpoint;
4033

4134
@AutoLogOutput(key = "Climber/ElevatorAtSetpoint")
4235
public final Trigger atClimbSetpoint = new Trigger(climbPID::atSetpoint);
@@ -51,6 +44,20 @@ public Elevator(ElevatorIO io) {
5144
this.io = io;
5245
pid.setTolerance(tolerance);
5346
climbPID.setTolerance(tolerance);
47+
48+
atSetpoint =
49+
new Trigger(
50+
() -> {
51+
var referencePos = io.metresToRotations(setpoint.setpoint);
52+
var posTolerance = io.metresToRotations(0.005);
53+
var currentPos = inputs.positionRotations;
54+
55+
var velTolerance = io.metresToRotations(0.3);
56+
var currentVel = inputs.angularVelocityRotationsPerSecond;
57+
58+
return Math.abs(referencePos - currentPos) < posTolerance
59+
&& Math.abs(0 - currentVel) < velTolerance;
60+
});
5461
}
5562

5663
@Override
@@ -100,7 +107,7 @@ public Command goToClimberSetpoint(ElevatorSetpoints point, BooleanSupplier safe
100107
setpoint = point;
101108
var out =
102109
climbPID.calculate(
103-
io.positionRotationsToMetres(inputs.positionRotations), setpoint.setpoint);
110+
io.rotationsToMetres(inputs.positionRotations), setpoint.setpoint);
104111
io.setVoltage(MathUtil.clamp(out, -4, 4));
105112
}),
106113
Commands.none(),
@@ -120,7 +127,7 @@ public Pose3d getHeight() {
120127
return new Pose3d(
121128
0,
122129
0,
123-
io.positionRotationsToMetres(inputs.positionRotations),
130+
io.rotationsToMetres(inputs.positionRotations),
124131
new Rotation3d(Math.PI / 2, 0, Math.PI / 2));
125132
}
126133
}

‎src/main/java/org/curtinfrc/frc2025/subsystems/elevator/ElevatorIO.java‎

Lines changed: 3 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -9,7 +9,7 @@ public static class ElevatorIOInputs {
99
public double appliedVolts;
1010
public double currentAmps;
1111
public double positionRotations;
12-
public double angularVelocityRotationsPerMinute;
12+
public double angularVelocityRotationsPerSecond;
1313
public boolean hominSensor;
1414
}
1515

@@ -21,11 +21,11 @@ public default void setPosition(double positionMetres) {}
2121

2222
public default void zero() {}
2323

24-
public default double positionRotationsToMetres(double rotations) {
24+
public default double rotationsToMetres(double rotations) {
2525
return rotations;
2626
}
2727

28-
public default double positionMetresToRotations(double metres) {
28+
public default double metresToRotations(double metres) {
2929
return metres;
3030
}
3131
}

‎src/main/java/org/curtinfrc/frc2025/subsystems/elevator/ElevatorIOComp.java‎

Lines changed: 39 additions & 20 deletions
Original file line numberDiff line numberDiff line change
@@ -4,14 +4,12 @@
44

55
import com.ctre.phoenix6.BaseStatusSignal;
66
import com.ctre.phoenix6.StatusSignal;
7-
import com.ctre.phoenix6.configs.CurrentLimitsConfigs;
8-
import com.ctre.phoenix6.configs.MotorOutputConfigs;
9-
import com.ctre.phoenix6.configs.Slot0Configs;
107
import com.ctre.phoenix6.configs.TalonFXConfiguration;
118
import com.ctre.phoenix6.controls.Follower;
12-
import com.ctre.phoenix6.controls.PositionVoltage;
9+
import com.ctre.phoenix6.controls.MotionMagicVoltage;
1310
import com.ctre.phoenix6.controls.VoltageOut;
1411
import com.ctre.phoenix6.hardware.TalonFX;
12+
import com.ctre.phoenix6.signals.GravityTypeValue;
1513
import com.ctre.phoenix6.signals.InvertedValue;
1614
import com.ctre.phoenix6.signals.NeutralModeValue;
1715
import edu.wpi.first.units.measure.Angle;
@@ -26,17 +24,9 @@ public class ElevatorIOComp implements ElevatorIO {
2624
private static final double gearing = 8.1818;
2725
private static final int ID = 7;
2826
private static final int FOLLOWER_ID = 40;
29-
private static final TalonFXConfiguration config =
30-
new TalonFXConfiguration()
31-
.withMotorOutput(
32-
new MotorOutputConfigs()
33-
.withInverted(InvertedValue.CounterClockwise_Positive)
34-
.withNeutralMode(NeutralModeValue.Brake))
35-
.withSlot0(new Slot0Configs().withKP(0.4))
36-
.withCurrentLimits(
37-
new CurrentLimitsConfigs().withSupplyCurrentLimit(30).withStatorCurrentLimit(60));
38-
39-
private final TalonFX motor = new TalonFX(ID);
27+
private final TalonFXConfiguration config;
28+
29+
protected final TalonFX motor = new TalonFX(ID);
4030
private final TalonFX follower = new TalonFX(FOLLOWER_ID);
4131

4232
private DigitalInput reset = new DigitalInput(0);
@@ -47,10 +37,39 @@ public class ElevatorIOComp implements ElevatorIO {
4737
private final StatusSignal<AngularVelocity> velocity = motor.getVelocity();
4838

4939
private final VoltageOut voltageRequest = new VoltageOut(0).withEnableFOC(true);
50-
private final PositionVoltage positionRequest = new PositionVoltage(0).withEnableFOC(true);
40+
private final MotionMagicVoltage positionRequest = new MotionMagicVoltage(0).withEnableFOC(true);
5141
private final Follower followRequest = new Follower(ID, false);
5242

5343
public ElevatorIOComp() {
44+
config = new TalonFXConfiguration();
45+
var output = config.MotorOutput;
46+
output.Inverted = InvertedValue.CounterClockwise_Positive;
47+
output.NeutralMode = NeutralModeValue.Brake;
48+
49+
var slot0 = config.Slot0;
50+
slot0.GravityType = GravityTypeValue.Elevator_Static;
51+
// we lie about units cause of how the dimensional analysis works out
52+
// rotations = metres / value
53+
// rotations * value = metres
54+
// V/rotations = V/metres/value
55+
// V/rotations = V*value/metres
56+
// so rotationsToMetres does the correct thing here
57+
slot0.kG = 0.09;
58+
slot0.kP = rotationsToMetres(318.70);
59+
slot0.kD = rotationsToMetres(1.33);
60+
slot0.kV = rotationsToMetres(5.29);
61+
slot0.kV = rotationsToMetres(0.01);
62+
63+
var currentLimits = config.CurrentLimits;
64+
currentLimits.StatorCurrentLimitEnable = true;
65+
currentLimits.StatorCurrentLimit = 60;
66+
currentLimits.SupplyCurrentLimitEnable = true;
67+
currentLimits.SupplyCurrentLimit = 30;
68+
69+
var mmConfig = config.MotionMagic;
70+
mmConfig.MotionMagicCruiseVelocity = metresToRotations(2.24);
71+
mmConfig.MotionMagicAcceleration = metresToRotations(49.00);
72+
5473
tryUntilOk(5, () -> motor.getConfigurator().apply(config));
5574
tryUntilOk(5, () -> follower.getConfigurator().apply(config));
5675
BaseStatusSignal.setUpdateFrequencyForAll(20.0, velocity, voltage, current, position);
@@ -66,7 +85,7 @@ public void updateInputs(ElevatorIOInputs inputs) {
6685
inputs.appliedVolts = voltage.getValueAsDouble();
6786
inputs.currentAmps = current.getValueAsDouble();
6887
inputs.positionRotations = position.getValueAsDouble();
69-
inputs.angularVelocityRotationsPerMinute = velocity.getValueAsDouble();
88+
inputs.angularVelocityRotationsPerSecond = velocity.getValueAsDouble();
7089
inputs.hominSensor = reset.get();
7190
}
7291

@@ -78,7 +97,7 @@ public void setVoltage(double volts) {
7897

7998
@Override
8099
public void setPosition(double positionMetres) {
81-
positionRequest.withPosition(positionMetresToRotations(positionMetres));
100+
positionRequest.withPosition(metresToRotations(positionMetres));
82101
motor.setControl(positionRequest);
83102
}
84103

@@ -88,12 +107,12 @@ public void zero() {
88107
}
89108

90109
@Override
91-
public double positionRotationsToMetres(double rotations) {
110+
public double rotationsToMetres(double rotations) {
92111
return rotations * Math.PI * 2 * pulleyRadiusMeters / gearing;
93112
}
94113

95114
@Override
96-
public double positionMetresToRotations(double metres) {
115+
public double metresToRotations(double metres) {
97116
return metres / (Math.PI * 2 * pulleyRadiusMeters) * gearing;
98117
}
99118
}

‎src/main/java/org/curtinfrc/frc2025/subsystems/elevator/ElevatorIOSim.java‎

Lines changed: 36 additions & 15 deletions
Original file line numberDiff line numberDiff line change
@@ -1,28 +1,49 @@
11
package org.curtinfrc.frc2025.subsystems.elevator;
22

3-
import static org.curtinfrc.frc2025.subsystems.elevator.ElevatorConstants.kA;
4-
import static org.curtinfrc.frc2025.subsystems.elevator.ElevatorConstants.kV;
5-
63
import edu.wpi.first.math.system.plant.DCMotor;
4+
import edu.wpi.first.wpilibj.Notifier;
5+
import edu.wpi.first.wpilibj.RobotController;
76
import edu.wpi.first.wpilibj.simulation.ElevatorSim;
87

9-
public class ElevatorIOSim implements ElevatorIO {
8+
public class ElevatorIOSim extends ElevatorIOComp {
109
private final ElevatorSim elevatorSim =
11-
new ElevatorSim(kV, kA, DCMotor.getNEO(1), 0, 0.6, true, 0);
12-
private double volts = 0;
10+
new ElevatorSim(5.29, 0.01, DCMotor.getKrakenX60Foc(2), 0, 0.7, true, 0, 0.001, 0.01);
1311

14-
@Override
15-
public void updateInputs(ElevatorIOInputs inputs) {
12+
private double lastSimTime;
13+
private final Notifier notifier;
14+
15+
public ElevatorIOSim() {
16+
super();
17+
lastSimTime = RobotController.getFPGATime();
18+
19+
notifier =
20+
new Notifier(
21+
() -> {
22+
final double currentTime = RobotController.getFPGATime();
23+
double deltaTime = currentTime - lastSimTime;
24+
lastSimTime = currentTime;
25+
26+
updateSimState(deltaTime);
27+
});
28+
notifier.startPeriodic(0.001);
29+
}
30+
31+
private void updateSimState(double deltaTime) {
32+
var simState = motor.getSimState();
33+
elevatorSim.setInput(simState.getMotorVoltage());
1634
elevatorSim.update(0.02);
17-
// ignore velocity cause we dont care
18-
inputs.currentAmps = elevatorSim.getCurrentDrawAmps();
19-
inputs.appliedVolts = volts;
20-
inputs.positionRotations = elevatorSim.getPositionMeters();
35+
var rotorPosition = metresToRotations(elevatorSim.getPositionMeters());
36+
simState.setRawRotorPosition(rotorPosition);
37+
var rotorVel = metresToRotations(elevatorSim.getVelocityMetersPerSecond());
38+
simState.setRotorVelocity(rotorVel);
2139
}
2240

2341
@Override
24-
public void setVoltage(double volts) {
25-
this.volts = volts;
26-
elevatorSim.setInputVoltage(volts);
42+
public void updateInputs(ElevatorIOInputs inputs) {
43+
super.updateInputs(inputs);
44+
final double currentTime = RobotController.getFPGATime();
45+
double deltaTime = currentTime - lastSimTime;
46+
lastSimTime = currentTime;
47+
updateSimState(deltaTime);
2748
}
2849
}

0 commit comments

Comments
 (0)