Skip to content

Commit 6da6524

Browse files
judsonjamesjudsonjames
committed
Add Flywheel simulation support; Add TalonFX and TalonFXS SimState; Motion Magic Velocity Voltage; Velocity Tuning
1 parent 7d3df30 commit 6da6524

11 files changed

Lines changed: 372 additions & 46 deletions

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

Lines changed: 6 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -60,7 +60,6 @@ public RobotContainer() {
6060
// new ModuleIOSim(),
6161
// new ModuleIOSim(),
6262
// new ModuleIOSim());
63-
6463
break;
6564

6665
case REPLAY:
@@ -105,7 +104,7 @@ private void configureButtonBindings() {
105104
// Set up the controls for the Turret Here
106105
controller.b().onTrue(superStructure.setTreeCommand(SuperStructureModes.R180));
107106
controller.leftBumper().onTrue(superStructure.setModeCommand(SuperStructureModes.L90));
108-
controller.rightBumper().onTrue(superStructure.runTVelocity(1));
107+
// controller.rightBumper().onTrue(superStructure.runTVelocity(1));
109108
// if (controller.isConnected()) {
110109
// controller.x().onTrue(superStructure.TreeRotate());
111110
// } else if (!controller.isConnected()) {
@@ -119,5 +118,10 @@ private void configureButtonBindings() {
119118
.whileTrue(superStructure.turretManualCommand(() -> controller.getLeftXSquared() * 6))
120119
.onTrue(superStructure.setModeCommand(SuperStructureModes.MANUAL));
121120
// controller.start().and(superStructure.isTurretAligned()).onTrue(superStructure.shootCommand());
121+
122+
controller.povUp().onTrue(superStructure.setFlywheelVelocity(10));
123+
controller.povDown().onTrue(superStructure.setFlywheelVelocity(-10));
124+
controller.povLeft().onTrue(superStructure.setFlywheelVoltage(-6));
125+
controller.povRight().onTrue(superStructure.setFlywheelVoltage(6));
122126
}
123127
}

‎src/main/java/frc/robot/subsystems/flywheel/Flywheel.java‎

Lines changed: 90 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,90 @@
1+
package frc.robot.subsystems.flywheel;
2+
3+
import static edu.wpi.first.units.Units.RotationsPerSecond;
4+
import static edu.wpi.first.units.Units.RotationsPerSecondPerSecond;
5+
6+
import edu.wpi.first.math.system.plant.LinearSystemId;
7+
import edu.wpi.first.math.util.Units;
8+
import edu.wpi.first.wpilibj.RobotBase;
9+
import edu.wpi.first.wpilibj.simulation.FlywheelSim;
10+
import frc.robot.subsystems.rollers.single.SingleRoller;
11+
import frc.robot.subsystems.rollers.single.SingleRollerIO;
12+
import java.util.Optional;
13+
import org.littletonrobotics.junction.Logger;
14+
15+
/** Simulation support for a single motor that only spins. */
16+
public class Flywheel extends SingleRoller {
17+
/**
18+
* Radius of flywheel, necessary for velocity calculations In the event that you use differently
19+
* sized flywheels, they should be separate Flywheel objects with their own respected
20+
* measurements.
21+
*/
22+
private final double radiusMeters;
23+
24+
private double lastPositionRotations = 0.0;
25+
26+
private final Optional<FlywheelSim> flywheelSim;
27+
28+
public Flywheel(String name, SingleRollerIO io, double radiusMeters) {
29+
super(name, io);
30+
this.radiusMeters = radiusMeters;
31+
32+
if (RobotBase.isSimulation()) {
33+
flywheelSim =
34+
Optional.of(
35+
new FlywheelSim(
36+
LinearSystemId.createFlywheelSystem(
37+
FlywheelConstants.gearbox,
38+
FlywheelConstants.moi,
39+
FlywheelConstants.reduction),
40+
FlywheelConstants.gearbox));
41+
} else {
42+
flywheelSim = Optional.empty();
43+
}
44+
}
45+
46+
@Override
47+
public void periodic() {
48+
super.periodic();
49+
50+
Logger.recordOutput(
51+
name + "/PositionDeltaRotations", inputs.positionRotations - lastPositionRotations);
52+
53+
lastPositionRotations = inputs.positionRotations;
54+
55+
// Flywheel Simulation, if present
56+
flywheelSim.ifPresent(
57+
flywheelSim -> {
58+
flywheelSim.setInputVoltage(inputs.appliedVoltage);
59+
flywheelSim.setAngularVelocity(Units.rotationsToRadians(inputs.velocityRotationsPerSec));
60+
61+
String logRoot = name + "/PhysicalSim";
62+
Logger.recordOutput(
63+
logRoot + "/VelocityRotPerSec",
64+
flywheelSim.getAngularVelocity().in(RotationsPerSecond));
65+
Logger.recordOutput(
66+
logRoot + "/OmegaRotPerSecSq",
67+
flywheelSim.getAngularAcceleration().in(RotationsPerSecondPerSecond));
68+
Logger.recordOutput(logRoot + "/TorqueNwtMeters", flywheelSim.getTorqueNewtonMeters());
69+
Logger.recordOutput(logRoot + "/CurrentDrawAmps", flywheelSim.getCurrentDrawAmps());
70+
Logger.recordOutput(logRoot + "/InputVoltage", flywheelSim.getInputVoltage());
71+
});
72+
}
73+
74+
public void setVelocity(double velocityMetersPerSec) {
75+
// CTRE has documentation to support this
76+
// https://v6.docs.ctr-electronics.com/en/latest/docs/api-reference/device-specific/talonfx/closed-loop-requests.html#converting-from-meters
77+
78+
// We devide distance measured by circumference to "unrawp"
79+
double unwrappedRadiusMeters = (2 * Math.PI * radiusMeters);
80+
81+
// Handle w = v / 2πr
82+
double velocityRotationsPerSec = velocityMetersPerSec / unwrappedRadiusMeters;
83+
84+
io.setVelocity(velocityRotationsPerSec);
85+
}
86+
87+
public void setVoltage(double volts) {
88+
io.runVolts(volts);
89+
}
90+
}

‎src/main/java/frc/robot/subsystems/flywheel/FlywheelConstants.java‎

Lines changed: 46 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,46 @@
1+
package frc.robot.subsystems.flywheel;
2+
3+
import com.ctre.phoenix6.configs.MotionMagicConfigs;
4+
import com.ctre.phoenix6.configs.Slot0Configs;
5+
import edu.wpi.first.math.geometry.Rotation2d;
6+
import edu.wpi.first.math.system.plant.DCMotor;
7+
8+
public class FlywheelConstants {
9+
public static final int canId = 6;
10+
11+
public static final DCMotor gearbox = DCMotor.getKrakenX60Foc(1);
12+
// public static final double reduction = 25.0 * (216.0 / 24.0); // for REAL turret
13+
public static final double reduction = 1; // for minion motor
14+
public static final double moi = 0.000002;
15+
16+
public static final boolean invert = true;
17+
public static final double currentLimitAmps = 30;
18+
19+
public static final boolean isBrakeMode = true;
20+
21+
// Tune this as needed
22+
public static final Rotation2d intialPosition = Rotation2d.fromDegrees(0);
23+
public static final Rotation2d minimumPosition = Rotation2d.fromDegrees(-90);
24+
public static final Rotation2d maximumPosition = Rotation2d.fromDegrees(90);
25+
26+
// https://docs.wpilib.org/en/stable/docs/software/advanced-controls/introduction/tuning-vertical-arm.html#combined-feedforward-and-feedback-control
27+
public static final Slot0Configs gains =
28+
new Slot0Configs()
29+
// feedforward
30+
.withKS(0.05)
31+
.withKV(0.05)
32+
.withKA(0.0)
33+
// feedback
34+
.withKP(1.0)
35+
.withKI(0.0)
36+
.withKD(0.0);
37+
38+
public static final MotionMagicConfigs mmConfig =
39+
new MotionMagicConfigs()
40+
.withMotionMagicCruiseVelocity(5.0 * reduction)
41+
// .withMotionMagicCruiseVelocity(0.01 * reduction)
42+
.withMotionMagicAcceleration(4.5 * reduction)
43+
.withMotionMagicJerk(20.0 * reduction);
44+
45+
public static final boolean foc = true;
46+
}

‎src/main/java/frc/robot/subsystems/rollers/single/SingleRollerIO.java‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -26,7 +26,7 @@ public default void updateInputs(SingleRollerIOInputs inputs) {}
2626
public default void runVolts(double volts) {}
2727

2828
/** Run roller at set Velocity */
29-
public default void setVelocity(double velocity) {}
29+
public default void setVelocity(double velocityMetersPerSec) {}
3030

3131
/** Set goal position of roller to position */
3232
public default void setGoal(double positionRotations) {}

‎src/main/java/frc/robot/subsystems/rollers/single/SingleRollerIOTalonFX.java‎

Lines changed: 23 additions & 14 deletions
Original file line numberDiff line numberDiff line change
@@ -5,6 +5,7 @@
55
import com.ctre.phoenix6.configs.MotionMagicConfigs;
66
import com.ctre.phoenix6.configs.Slot0Configs;
77
import com.ctre.phoenix6.configs.TalonFXConfiguration;
8+
import com.ctre.phoenix6.controls.MotionMagicVelocityVoltage;
89
import com.ctre.phoenix6.controls.MotionMagicVoltage;
910
import com.ctre.phoenix6.controls.NeutralOut;
1011
import com.ctre.phoenix6.controls.VoltageOut;
@@ -19,24 +20,26 @@
1920
import frc.robot.Constants;
2021

2122
public class SingleRollerIOTalonFX implements SingleRollerIO {
22-
private final TalonFX talon;
23-
private final double reduction;
23+
protected final TalonFX talon;
24+
protected final double reduction;
2425

25-
private final StatusSignal<Angle> position;
26-
private final StatusSignal<AngularVelocity> velocity;
27-
private final StatusSignal<Voltage> voltage;
28-
private final StatusSignal<Current> supplyCurrentAmps;
29-
private final StatusSignal<Current> torqueCurrentAmps;
30-
private final StatusSignal<Temperature> tempCelsius;
26+
protected final StatusSignal<Angle> position;
27+
protected final StatusSignal<AngularVelocity> velocity;
28+
protected final StatusSignal<Voltage> voltage;
29+
protected final StatusSignal<Current> supplyCurrentAmps;
30+
protected final StatusSignal<Current> torqueCurrentAmps;
31+
protected final StatusSignal<Temperature> tempCelsius;
3132

32-
private final StatusSignal<Double> positionSetpointRotations;
33-
private final StatusSignal<Double> velocitySetpointRotationsPerSec;
33+
protected final StatusSignal<Double> positionSetpointRotations;
34+
protected final StatusSignal<Double> velocitySetpointRotationsPerSec;
3435

35-
private final MotionMagicVoltage mmVoltage;
36-
private final VoltageOut voltageOut;
37-
private final NeutralOut neutralOut = new NeutralOut();
36+
protected final MotionMagicVoltage mmVoltage;
37+
protected final MotionMagicVelocityVoltage mmVelocityVoltage;
3838

39-
private double positionGoalRotations = 0;
39+
protected final VoltageOut voltageOut;
40+
protected final NeutralOut neutralOut = new NeutralOut();
41+
42+
protected double positionGoalRotations = 0;
4043

4144
public SingleRollerIOTalonFX(
4245
int canId,
@@ -53,6 +56,7 @@ public SingleRollerIOTalonFX(
5356

5457
voltageOut = new VoltageOut(0.0).withUpdateFreqHz(0).withEnableFOC(foc);
5558
mmVoltage = new MotionMagicVoltage(0.0).withUpdateFreqHz(0).withEnableFOC(foc);
59+
mmVelocityVoltage = new MotionMagicVelocityVoltage(0).withSlot(0).withEnableFOC(foc);
5660

5761
position = talon.getPosition();
5862
velocity = talon.getVelocity();
@@ -124,6 +128,11 @@ public void runVolts(double volts) {
124128
talon.setControl(voltageOut.withOutput(volts));
125129
}
126130

131+
@Override
132+
public void setVelocity(double velocityRotationsPerSec) {
133+
talon.setControl(mmVelocityVoltage.withVelocity(velocityRotationsPerSec * reduction));
134+
}
135+
127136
@Override
128137
public void setGoal(double positionRotations) {
129138
positionGoalRotations = positionRotations * reduction;

‎src/main/java/frc/robot/subsystems/rollers/single/SingleRollerIOTalonFXS.java‎

Lines changed: 23 additions & 22 deletions
Original file line numberDiff line numberDiff line change
@@ -6,14 +6,13 @@
66
import com.ctre.phoenix6.configs.Slot0Configs;
77
import com.ctre.phoenix6.configs.TalonFXSConfiguration;
88
import com.ctre.phoenix6.controls.DutyCycleOut;
9+
import com.ctre.phoenix6.controls.MotionMagicVelocityVoltage;
910
import com.ctre.phoenix6.controls.MotionMagicVoltage;
1011
import com.ctre.phoenix6.controls.NeutralOut;
11-
import com.ctre.phoenix6.controls.VelocityVoltage;
1212
import com.ctre.phoenix6.controls.VoltageOut;
1313
import com.ctre.phoenix6.hardware.TalonFXS;
1414
import com.ctre.phoenix6.signals.InvertedValue;
1515
import com.ctre.phoenix6.signals.NeutralModeValue;
16-
import edu.wpi.first.math.util.Units;
1716
import edu.wpi.first.units.measure.Angle;
1817
import edu.wpi.first.units.measure.AngularVelocity;
1918
import edu.wpi.first.units.measure.Current;
@@ -22,25 +21,27 @@
2221
import frc.robot.Constants;
2322

2423
public class SingleRollerIOTalonFXS implements SingleRollerIO {
25-
private final TalonFXS talon;
26-
private final double reduction;
24+
protected final TalonFXS talon;
25+
protected final double reduction;
2726

28-
private final StatusSignal<Angle> position;
29-
private final StatusSignal<AngularVelocity> velocity;
30-
private final StatusSignal<Voltage> voltage;
31-
private final StatusSignal<Current> supplyCurrentAmps;
32-
private final StatusSignal<Current> torqueCurrentAmps;
33-
private final StatusSignal<Temperature> tempCelsius;
27+
protected final StatusSignal<Angle> position;
28+
protected final StatusSignal<AngularVelocity> velocity;
29+
protected final StatusSignal<Voltage> voltage;
30+
protected final StatusSignal<Current> supplyCurrentAmps;
31+
protected final StatusSignal<Current> torqueCurrentAmps;
32+
protected final StatusSignal<Temperature> tempCelsius;
3433

35-
private final StatusSignal<Double> positionSetpointRotations;
36-
private final StatusSignal<Double> velocitySetpointRotationsPerSec;
34+
protected final StatusSignal<Double> positionSetpointRotations;
35+
protected final StatusSignal<Double> velocitySetpointRotationsPerSec;
3736

38-
private final MotionMagicVoltage mmVoltage;
39-
private final VoltageOut voltageOut;
40-
private final DutyCycleOut dutyCycle;
41-
private final NeutralOut neutralOut = new NeutralOut();
37+
protected final MotionMagicVoltage mmVoltage;
38+
protected final MotionMagicVelocityVoltage mmVelocityVoltage;
4239

43-
private double positionGoalRotations = 0;
40+
protected final VoltageOut voltageOut;
41+
protected final DutyCycleOut dutyCycle;
42+
protected final NeutralOut neutralOut = new NeutralOut();
43+
44+
protected double positionGoalRotations = 0;
4445

4546
public SingleRollerIOTalonFXS(
4647
int canId,
@@ -58,6 +59,7 @@ public SingleRollerIOTalonFXS(
5859
voltageOut = new VoltageOut(0.0).withUpdateFreqHz(0).withEnableFOC(foc);
5960
dutyCycle = new DutyCycleOut(0).withUpdateFreqHz(0).withEnableFOC(foc);
6061
mmVoltage = new MotionMagicVoltage(0.0).withUpdateFreqHz(0).withEnableFOC(foc);
62+
mmVelocityVoltage = new MotionMagicVelocityVoltage(0).withSlot(0).withEnableFOC(foc);
6163

6264
position = talon.getPosition();
6365
velocity = talon.getVelocity();
@@ -134,15 +136,14 @@ public void runVolts(double volts) {
134136
// talon.setControl(dutyCycle.withOutput(velocity * reduction));
135137
// }
136138
@Override
137-
public void setVelocity(double velocity) {
139+
public void setVelocity(double velocityRotationsPerSec) {
138140
// Convert degrees/sec to rotations/sec
139-
double velocityRadPerSec = Units.degreesToRadians(velocity);
140-
double velocityRotations = velocityRadPerSec / (2.0 * Math.PI);
141-
VelocityVoltage velocityVoltage = new VelocityVoltage(0).withSlot(0);
141+
// double velocityRadPerSec = Units.degreesToRadians(velocity);
142+
// double velocityRotations = velocityRadPerSec / (2.0 * Math.PI);
142143

143144
// double ffVolts = feedforward.calculate(getVelocity(), acceleration);
144145
// motor.setControl(velocityRequest.withVelocity(velocityRotations).withFeedForward(ffVolts));
145-
talon.setControl(velocityVoltage.withVelocity(velocityRotations));
146+
talon.setControl(mmVelocityVoltage.withVelocity(velocityRotationsPerSec / reduction));
146147
}
147148

148149
@Override

‎src/main/java/frc/robot/subsystems/rollers/single/SingleRollerIOTalonFXSSim.java‎

Lines changed: 68 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,68 @@
1+
package frc.robot.subsystems.rollers.single;
2+
3+
import com.ctre.phoenix6.configs.MotionMagicConfigs;
4+
import com.ctre.phoenix6.configs.Slot0Configs;
5+
import com.ctre.phoenix6.controls.PositionVoltage;
6+
import com.ctre.phoenix6.controls.VelocityVoltage;
7+
import edu.wpi.first.math.system.plant.DCMotor;
8+
import edu.wpi.first.math.system.plant.LinearSystemId;
9+
import edu.wpi.first.math.util.Units;
10+
import edu.wpi.first.wpilibj.simulation.DCMotorSim;
11+
import frc.robot.Constants;
12+
13+
public class SingleRollerIOTalonFXSSim extends SingleRollerIOTalonFXS {
14+
private final DCMotorSim motorSim;
15+
16+
/** Motion Magic does _not_ work in Sim, so we use Position Voltage in the meantime */
17+
private final PositionVoltage positionVoltage;
18+
19+
/** Motion Magic does _not_ work in Sim, so we use Velocity Voltage in the meantime */
20+
private final VelocityVoltage velocityVoltage;
21+
22+
public SingleRollerIOTalonFXSSim(
23+
int canId,
24+
double reduction,
25+
double currentLimitAmps,
26+
boolean invert,
27+
boolean isBrakeMode,
28+
boolean foc,
29+
Slot0Configs gains,
30+
MotionMagicConfigs mmConfig,
31+
DCMotor dcMotor,
32+
double moi) {
33+
super(canId, reduction, currentLimitAmps, invert, isBrakeMode, foc, gains, mmConfig);
34+
35+
motorSim =
36+
new DCMotorSim(LinearSystemId.createDCMotorSystem(dcMotor, 0.00002, reduction), dcMotor);
37+
velocityVoltage = new VelocityVoltage(0).withSlot(0).withEnableFOC(foc);
38+
positionVoltage = new PositionVoltage(0).withSlot(0).withEnableFOC(foc);
39+
}
40+
41+
@Override
42+
public void setGoal(double positionRotations) {
43+
positionGoalRotations = positionRotations * reduction;
44+
talon.setControl(positionVoltage.withPosition(positionGoalRotations));
45+
}
46+
47+
@Override
48+
public void setVelocity(double velocityRotationsPerSec) {
49+
talon.setControl(velocityVoltage.withVelocity(velocityRotationsPerSec * reduction));
50+
}
51+
52+
@Override
53+
public void updateInputs(SingleRollerIOInputs inputs) {
54+
// Still do everything as expected for TalonFXS
55+
super.updateInputs(inputs);
56+
57+
// Update motor output from sim state
58+
motorSim.setInput(talon.getSimState().getMotorVoltage());
59+
motorSim.update(Constants.loopPeriodSecs);
60+
61+
// (WIP) Take DCMotorSim configs and set them to TalonFXS Sim
62+
talon.getSimState().setRawRotorPosition(motorSim.getAngularPositionRotations() / reduction);
63+
talon
64+
.getSimState()
65+
.setRotorVelocity(
66+
Units.radiansToRotations(motorSim.getAngularVelocityRadPerSec() / reduction));
67+
}
68+
}

0 commit comments

Comments
 (0)