[command] Deprecate RamseteCommand control functionality

[Starlight220]

Resolves #3350

I see nothing inherently wrong with the functionality I'm leaving non-deprecated here. If the idea here is accepted, I'll submit an frc-docs PR updating the tutorial.

[Starlight220]

Can someone please help me fix the error?

[Starlight220]

After discussion with the WPILib team, it seems that RamseteCommand is to be deprecated entirely with a replacement TrajectoryCommand with the following API/distribution of functionality:

class Drivetrain : SubsystemBase {
  // sets targets
  fun setTargetVelocities(left: SIUnit<Ratio<Meters, Seconds>>, right: SIUnit<Ratio<Meters, Seconds>>): Unit
  // odo update
  // setting motors is either here or in [setTargetVelocities] to team choice
  override fun periodic(): Unit
}
class TrajectoryCommand(trajectory: Trajectory, outputVelocity: (SIUnit<Ratio<Meters, Seconds>>, SIUnit<Ratio<Meters, Seconds>>) -> Unit, vararg requirements: Subsystem): CommandBase() {
  // starts timer
  override fun initialize(): Unit
  // samples trajectory and calls [Drivetrain.setTargetVelocities]
  override fun execute(): Unit
  // returns true when timer passes traj length
  override fun isFinished(): Boolean
  // calls 0 only on interrupt like RamseteCommand ?
  override fun end(interrupted: Boolean): Unit
}

[calcmogul]

The name TrajectoryCommand implies it would work for swerve and mecanum too, but that lambda takes left and right velocities that don't make sense for the other drivetrains. Taking a Trajectory.State would work for all of them.

[Starlight220]

In that case, how would it stop on interrupt?

[Starlight220]

Are we dropping the RamseteCommand example and replacing it with TrajectoryCommand or having both?

[prateekma]

There are two problems with this approach:

• targetState.t - m_previousTime will be 1 on the first call to this method, potentially resulting in large positive accelerations.
• targetState.t - m_previousTime will be a large negative value when you start to follow another trajectory because m_previousTime will contain the duration of the previously run trajectory.

[calcmogul]

For reference, LinearPlantInversionFeedforward takes a nominal dt in the ctor, then has Calculate() just take the current velocity and next velocity. The input change caused by dt jitter has been negligible in my experience.

SimpleMotorFeedforward::Calculate() takes a dt because the ctor didn't. I'd recommend just throwing in a nominal dt here to massively simplify things.

Also, the "current velocity, next velocity, dt" overload will be more accurate than the acceleration overload since the acceleration continuously varies throughout the timestep for a linear system (dx/dt = Ax + Bu).

[Starlight220]

Is there a reason that SimpleMotorFeedforward can't be altered to take a dt in the ctor?

Assuming I use the overload that you're suggesting, how would I get the next velocity? Or is it the target velocity (if so, I suggest renaming the parameter- it's confusing)?

[calcmogul]

Is there a reason that SimpleMotorFeedforward can't be altered to take a dt in the ctor?

It could if we're OK with breaking changes in the API.

Assuming I use the overload that you're suggesting, how would I get the next velocity? Or is it the target velocity (if so, I suggest renaming the parameter- it's confusing)?

To compute a feedforward, you use where you wanted to be at the current timestep, where you want to be at the next timestep, and information about your dynamics.

x_{k+1} = Ax_k + Bu_k
r_{k+1} = Ar_k + Bu_k
Bu_k = r_{k+1} - Ar_k
u_k = B^-1 (r_{k+1} - Ar_k)

where x is measured velocity, r is target velocity, _k is the subscript for the current timestep, _{k+1} is the subscript for the next timestep, u is the voltage, and A and B are the system dynamics.

In this case, you can use previous and current target velocity. Both come from your motion profile, whether it's a trapezoid profile or a Trajectory. You'll lag behind by a timestep, but that's OK as long as the controller thinks the previous target velocity is where it's trying to get to.

[Starlight220]

Is it a problem using (previous, current) for FF and (current) for PID?

[calcmogul]

Doing so is technically incorrect, but you wouldn't notice much of a difference unless you're looking for perfect tracking. If it's easy to just use previous, I'd do that as long as we can ensure AtGoal() returns false still when the user sets a new one (instead of it returning true until one cycle later).

[prateekma]

The state-space examples don't work (the robot moves in random directions instead of following the path).

[Starlight220]

And they worked before? Any ideas as to what change is causing it?

[prateekma]

Yes, they were working before. No, I did not have time to go through and debug why it broke.

[prateekma]

Any update on this PR?

[Starlight220]

If I'll have time I'll work through to debug where it broke, though I'm not very good at debugging the trajectory stuff.

[Gold856]

OBE by #6494?