Difference between using a timer and loop period in TrapezoidProfile calculate method

Technical Programming
1

I’m using a TrapezoidProfile to calculate a smooth motion profile, and passing the output to a Spark Max in regular position PID mode.
In the WPILib documentation it says passing in 0.02 as the kDt is equivalent to using an advancing timer, however I can never get the loop period option to work.

Basically this (note that I restart the timer right when the motion starts):

m_smoothMotionState = m_motionProfile.calculate(m_motionTimer.get(), m_currentStateSupplier.get(), m_desiredState);
set(
  m_smoothMotionState.position,
  ControlType.kPosition,
  m_feedforwardSupplier.apply(m_smoothMotionState),
  SparkPIDController.ArbFFUnits.kVoltage
);

versus this:

m_smoothMotionState = m_motionProfile.calculate(0.02, m_currentStateSupplier.get(), m_desiredState);
set(
  m_smoothMotionState.position,
  ControlType.kPosition,
  m_feedforwardSupplier.apply(m_smoothMotionState),
  SparkPIDController.ArbFFUnits.kVoltage
);

When I pass in 0.02, I’ve found that the motion profile doesn’t start the motion, and is perpetually stuck at outputting a very low value. I can nudge the mechanism, then it moves, but otherwise it just sits there. However, with the timer, things seem to work perfectly.

2

You’re passing a point in time that is not advancing? So, the plan is always 0.02s in (a constant value) and the PID just restores things to plan?

3

Yes, the idea is that it calculates what the setpoint is for the next loop.

This theory is explained here:

4

I believe the 2nd parameter to .calculate is meant to be the previous setpoint, rather than the current state, based on when I read the code of the TrapezoidalProfiler recently.

If you look at the example you’ll see this is what they’re doing as well. Passing in m_setpoint.

I think what you’re observing is that the PID controller for the first setpoint isn’t producing enough of an effort to actually advance the measured state off your mechanism, so it constantly calculated the same setpoint. That’s why when you give it a push, it’s enough to get it to advance.
If you feed the previous setpoint instead, then the setpoints will advance regardless of what the mechanism is doing, and the PID will eventually produce a strong enough effort to start it up and get it moving towards the desired setpoint.

If I had to guess, your first method would actually arrive at the goal position twice as fast as intended.
If you’re 1 second into your motion, and the current state is at the setpoint, then .calculate will calculate a state 1 second after the current setpoint, and it’ll place the next setpoint at 2 seconds into the profile.

I think supplying the previous setpoint makes more sense also, the goal is to just provide a smooth, repeatable motion and rely on PID to stay at the moving setpoint. In fact, a “motion profile” is often a precalculated sequence of setpoints which you can upload to a motor controller to execute. The TrapezoidalProfiler in WPILib is nice if you have a moving goal or an arbitrary starting point. If you can’t keep up with the profile, then you need a more aggressive PID or a slower velocity/acceleration constraint.

Alternatively, you could pass in the starting state as the 2nd parameter, and then pass in the current time, and this should produce the exact same sequence of setpoints.

3 Likes
5

Ah I see, I think that makes sense.

1 Like