SparkSim: MAXMotion, kCos, and Slots

Start with the linked test project at yamg revlib-sim-tests and run ./gradlew test; the failing tests map to the three reported bugs: MAXMotion position tracking, kCos using the setpoint instead of the position, and kV ignored outside closed-loop slot 0. Each bug is reproduced with the same setup (P, I and D at 0 except the gain under test, 12 V bus, iterate every 20 ms, sim.enable()). Done means those tests pass against the documented behaviour, with the SparkSim changes kept separate for each bug.

While building simulation support for SPARKs in YAMS and YAMG I ran into a few problems with SparkSim's closed loop. Each one was tested on its own: P, I and D at 0, only the gain under test set, 12 V bus, iterate() every 20 ms, and the SPARK enabled with sim.enable().

Versions: WPILib / GradleRIO 2027.0.0-alpha-7, REVLib 2027.0.0-alpha-8 (first found on 2027.0.0-alpha-7; alpha-8 gives exactly the same results), Java, desktop simulation. The SPARK MAX and SPARK Flex behave the same.

The snippets below all use this setup:

HAL.initialize();
SparkMax spark = new SparkMax(CANPort.CAN_S0, 1, MotorType.kBrushless);
SparkMaxConfig config = new SparkMaxConfig();
config.closedLoop.pid(0, 0, 0);
// ... the gain under test ...
spark.configure(config, ResetMode.kResetSafeParameters, PersistMode.kNoPersistParameters);
SparkMaxSim sim = new SparkMaxSim(spark, DCMotor.getNEO(1));
sim.enable(); // outputs nothing while disabled otherwise

// "Held still": the output ramps in over ~20 steps, so read it after 50.
for (int i = 0; i < 50; i++) {
  sim.iterate(0, 12, 0.02);
}
double volts = sim.getAppliedOutput() * 12;

1. MAXMotion position doesn't follow its profile in simulation

I ran a NEO through DCMotorSim (J = 0.0005 kg m², 1:1):

config.closedLoop.pid(0.0002, 0, 0);
config.closedLoop.feedForward.kV(1.0 / 473); // a NEO's kV, V/RPM
config.closedLoop.maxMotion.cruiseVelocity(3000).maxAcceleration(6000);
// configure, create the sim, enable...
spark.getClosedLoopController().setSetpoint(100, ControlType.kMAXMotionPositionControl);

DCMotorSim physics = new DCMotorSim(Models.singleJointedArmFromPhysicalConstants(DCMotor.getNEO(1), 0.0005, 1), DCMotor.getNEO(1));
for (int i = 0; i < 250; i++) { // 5 s
  physics.setInputVoltage(sim.getAppliedOutput() * 12);
  physics.update(0.02);
  double rpm = physics.getAngularVelocity() * 60 / (2 * Math.PI);
  sim.iterate(rpm, 12, 0.02);
}

With 6000 RPM/s it should take 0.5 s to reach 3000 RPM and land on 100 rotations at about 2.5 s. What I got:

t	position	velocity
0.5 s	0.7 rot	267 RPM
1.0 s	4.9 rot	731 RPM
2.0 s	25.6 rot	1752 RPM
3.0 s	64.5 rot	2942 RPM
4.0 s	114.5 rot	2873 RPM
5.0 s	157.9 rot	2337 RPM

So it accelerates at roughly 1000 RPM/s rather than 6000, and then runs straight past the goal without stopping (still moving at 158 rotations). Ten times the kP gives almost the same numbers. Raising maxAcceleration to 36000 gets it to cruise faster, but it then overshoots to ~112 rotations and oscillates around the goal.

For comparison, plain kVelocity to 3000 RPM with the same kV settles at 3024 RPM in under 0.5 s, so the motor model and kV are fine. It's the MAXMotion profile in the sim:

spark.getClosedLoopController().setSetpoint(3000, ControlType.kVelocity); // 3024 RPM after 1 s

Because of this we currently have to run a TrapezoidProfile on the SystemCore in simulation and send each step with kPosition.

For reference: the feedforward that does work

Each gain on its own, held still (expected / measured):

mode	gain	expected	measured
kVelocity	kV 0.002 V/RPM at 1000 RPM	2 V	2.0000 V
kVelocity	kS 0.5 V	0.5 V	0.5000 V
kPosition	kS 0.5 V	±0.5 V (sign of the error)	±0.5000 V
kPosition	kG 1 V	1 V	1.0000 V
kMAXMotionPositionControl	kG 1 V	1 V	1.0000 V
kMAXMotionPositionControl	kA 0.001 V/(RPM/s) at 2000 RPM/s	2 V	2.0000 V

2. kCos uses the setpoint, read as degrees, rather than the position

The docs say kCos "is multiplied by the cosine of the absolute position of the mechanism". In the sim it looks like it uses the setpoint, treated as degrees, and ignores where the mechanism actually is:

config.closedLoop.feedForward.kCos(1.0).kCosRatio(1.0);
// configure, create the sim, enable...
sim.setPosition(0.25); // a quarter turn: cos(position) = 0
spark.getClosedLoopController().setSetpoint(0.25, ControlType.kPosition);
// held still for 50 steps: 1.0000 V, which is cos(0.25 degrees), not cos(a quarter turn) = 0

With the motor held still at each position:

motor at	setpoint	output	cos(position, rotations)	cos(setpoint, degrees)
0 rot	0.25	1.0000 V	1	1.0000
0 rot	0.5	1.0000 V	1	1.0000
0 rot	90	0.0000 V	1	0
0 rot	180	-1.0000 V	1	-1
0.25 rot	0.25	1.0000 V	0	1.0000
0.5 rot	0.5	1.0000 V	-1	1.0000

The output always matches cos(setpoint in degrees) and never cos(position). So an arm with kCosRatio = 1 / gearRatio (position in rotations) gets close to the full kG at every angle in simulation. We're working around it by setting kCos to 0 in simulation and sending the gravity as an arbitrary feedforward:

config.closedLoop.feedForward.kCos(RobotBase.isSimulation() ? 0 : kG).kCosRatio(1.0 / gearRatio);
// ...
double gravity = RobotBase.isSimulation() ? kG * Math.cos(armAngleRadians) : 0;
spark.getClosedLoopController().setSetpoint(
    setpointMotorRotations, ControlType.kPosition, ClosedLoopSlot.kSlot0, gravity, ArbFFUnits.kVoltage);

3. kV only works in closed loop slot 0

Found this one while putting the test project together. The same gain in each slot, kVelocity at 1000 RPM, motor held still:

config.closedLoop.feedForward.kV(0.002, ClosedLoopSlot.kSlot1);
// configure, create the sim, enable...
spark.getClosedLoopController().setSetpoint(1000, ControlType.kVelocity, ClosedLoopSlot.kSlot1);
// held still for 50 steps: 0 V; the same in kSlot0 gives 2.0000 V

gain	slot 0	slot 1	slot 2	slot 3	expected
kV 0.002 V/RPM	2.0000 V	0 V	0 V	0 V	2 V
kS 0.5 V	0.5000 V	0.5000 V	0.5000 V	0.5000 V	0.5 V
kP 0.0001 /RPM	1.2000 V	1.3000 V	1.2000 V	1.2000 V	1.2 V

So kV is ignored in slots 1 to 3, and kP in slot 1 comes out about 8% high. I ran into it because a velocity mode on slot 1 only got to 1742 RPM out of 3000.

Test project

Everything above is in a small robot project you can run: https://github.com/Yet-Another-Software-Suite/yamg/tree/revlib-sim-tests/revlib-sim-tests

• ./gradlew simulateJava and a gamepad on port 0 lets you run a slide and an arm with MAXMotion / kCos, or with the workaround, and compare them in AdvantageScope or Glass. The README lists the buttons and what's published.
• ./gradlew test runs a test for each of these. They check the documented behaviour, so the ones that fail are the bugs above.

I haven't been able to check whether a real SPARK does the same; this is all in simulation.