Robot Characterization with Swerve Drive

Hi all,

5507 mentor/coach here. Since I cannot travel much this summer, I have decided to delve into the robot software. I “rescued” the robot and I am playing with it in my garage. My goal is to be able to better teach the students on all aspects of the robot software. For context, I have a software background and I teach APCSA, but I haven’t spent much time on robot software.

Our robot is a swerve drive (SDS) with Falcon 500 drive motors. Our current gearing is 7:1. So here is out shortened robotconfig.py:

{

    "rightControllerTypes": ["WPI_TalonFX", "WPI_TalonFX"],
    "leftControllerTypes": ["WPI_TalonFX", "WPI_TalonFX"],
    # Note: The first id in the list of ports should be the one with an encoder
    # Ports for the left-side motors
    "leftMotorPorts": [0, 6],
    # Ports for the right-side motors
    "rightMotorPorts": [2, 4],
    # Inversions for the left-side motors
    "leftMotorsInverted": [True, False],
    # Inversions for the right side motors
    "rightMotorsInverted": [True, False],

    # Wheel diameter (in units of your choice - will dictate units of analysis)
    "wheelDiameter": 0.333,
    # If your robot has only one encoder, set all right encoder fields to `None`
    # Encoder edges-per-revolution (*NOT* cycles per revolution!)
    # This value should be the edges per revolution *of the wheels*, and so
    # should take into account gearing between the encoder and the wheels
    "encoderEPR": 14336, # 7:1 gearing so 7*2048

    # Whether the left encoder is inverted
    "leftEncoderInverted": False,
    # Whether the right encoder is inverted:
    "rightEncoderInverted": False,

    "gyroType": "NavX",
    "gyroPort": "SPI.Port.kMXP",
}

The tool outputs this data:

So my questions are as follows:

  1. Is the robotconfig.py set correctly?
  2. Do the Controller Gains seem reasonable?
  3. How do you calculate the kF for PIDF?

Thanks so much,

John Hajel

  1. No idea.
  2. It’s likely. The gains are really small because they include conversion factors from feet to the Talon’s internal units.
  3. kF * setpoint is only relevant to velocity controllers where the formula is kF * velocity. kV is the constant you want for that. If you want to do position control, kF won’t work, and you’ll need to use the Talon’s “arbitrary feedforward” instead.
2 Likes

Your robotconfig.py looks good. A frc-characterization PR that does latency compensation just got merged (the Talon FXes and SRXes have ~82 ms of delay in velocity mode from filtering.) It will almost certainly affect your gains. Running py -3 -m pip install --upgrade --user frc-characterization (you might not need the --user bit if you installed globally) should get everything updated. Then all you need to do is re-run the analyzer select the new Talon FX preset in the feedback analysis pane.

I’d love to hear if the ‘latency compensated’ gains work better than the old ones (I never had a chance to test my PR on much real hardware.)

1 Like

Thanks for the responses. I updated tool and this is my new results for position:

Here is the result for velocity:

Ok, now that I have the kP and kD what do I do with them? I tried them using a PIDCommand but the coefficients were too small by a factor of 100 to make the robot move (Code below). Am I doing something wrong?

public GoToDistance(double distance, SwerveDriveSubsystem drive) {
    super(
        new ProfiledPIDController(0.000361,0.0, 0.000237,   //from characterization tool
            new TrapezoidProfile.Constraints(60,120)),  
        // Close loop on distance
        drive::getInches ,
        // Set reference to target
        distance,  
        // Pipe output to drive robot
        (output,setpoint) -> drive.holonomicDrive(output, 0, 0),
        // Require the drive
        drive);

    getController().setTolerance(6, 10);

  }

If feedforward-only makes the drivetrain follow the desired motion profile, that will prove the encoder conversions and the K_v, K_a pair were correct at least. Then we need to verify the unit conversions for the feedback gains.

First, some context. To compute the feedback gains, frc-characterization creates a state-space model of the form:

\dot{\begin{bmatrix} \text{position} \\ \text{velocity} \end{bmatrix}} = \begin{bmatrix} 0 & 1 \\ 0 & -\frac{K_v}{K_a} \end{bmatrix} \begin{bmatrix} \text{position} \\ \text{velocity} \end{bmatrix} + \begin{bmatrix} 0 \\ \frac{1}{K_a} \end{bmatrix} \begin{bmatrix} \text{voltage} \end{bmatrix}

and finds the LQR for the given error and control effort weights. It has the form:

\begin{bmatrix} \text{voltage} \end{bmatrix} = \begin{bmatrix} K_p & K_d \end{bmatrix} \left( \begin{bmatrix} \text{position setpoint} \\ \text{velocity setpoint} \end{bmatrix} - \begin{bmatrix} \text{position} \\ \text{velocity} \end{bmatrix} \right)

Those gains convert from feet to volts and ft/s to volts respectively.

Now, to use them on Talons, we convert them to the Talon’s native units which are 0..1023 where 0 is 0V and 1023 is 12V (so each gain is multiplied by 1023/12). There’s also funny business with the Talon’s timescale where it uses deciseconds, so we have to compensate for that too. This is all assuming the default unit settings. The Talons added new conversions in 2020 so the output is 0..1 (if you enable it) that multiplies the gains by 1/12 instead.

Overall, getting LQR working with the Talon and Spark MAX internal units has been a massive pain because they don’t stick to anything standard like SI units and seconds. It’s been the single biggest issue when getting the 2020 controls stuff working on people’s robots. If anything, that’s one of the benefits of doing PID on the RIO instead; the units are sane (just your length unit of choice, seconds, and volts).

The second hurdle for teams is gyro direction because vendors aren’t consistent on whether GetAngle() should use North-East-Up (NED) or NWU convention. WPILib calls for NED convention in its Gyro interface, but some vendors don’t follow that. Mathematics/academia in general assumes NWU (the aviation industry is the only one I’ve seen that uses NED instead). We’re hoping that the GetRotation2d() function we’re adding for 2021 helps because the function documentation specifically requires that it use NWU. We’ll see if vendors add it, because not everyone uses the Gyro interface.

4 Likes

Thank you for all of this. It really helps. But my question is more of a practical one. Now that I “characterized” the robot, what do I do with the information? I am using the PID Controller on the RoboRio and the calculated kP and kD are not correct for a PIDController in this example from WPILib (I also made one for GoToLocation).

  public TurnToAngle(double targetAngleDegrees, DriveSubsystem drive) {
    super(
        new PIDController(DriveConstants.kTurnP, DriveConstants.kTurnI, DriveConstants.kTurnD),
        // Close loop on heading
        drive::getHeading,
        // Set reference to target
        targetAngleDegrees,
        // Pipe output to turn robot
        output -> drive.arcadeDrive(0, output),
        // Require the drive
        drive);

This example of the PIDController does not accept a kF argument. I have seen some other examples that do. However, even if I find a PID controller to pass in a kF, what is that value of kF based on the characterization?

https://docs.wpilib.org/en/stable/docs/software/advanced-control/controllers/feedforward.html

1 Like

We decided to remove feedforward from PIDController because feedforward controllers and feedback controllers are in fact separate things, and you can mix-and-match them. u = Kv * velocity (known more commonly as kF * setpoint) only really works for drivetrain velocity systems and flywheels. Making feedforwards separate objects lets us support elevators, drivetrain position systems, and single-jointed arms too.

2 Likes

For debugging IRL controller performance, I find it helpful to plot the position/angle and velocity/angular velocity setpoints, position/angle measurements, and control inputs over time. Do this for both feedforward-only and feedforward+feedback. You can glean a lot of information from these two sets of plots. For example:

  1. Feedforward-only will tell you how well your Kv and Ka reflect the real system. If those are bad, your feedback gains will be bad too.
  2. If the measurements are laggy, you can get a lot of jittering and oscillation. For on-RIO control loops, this can be caused by reading encoders over CAN because the status frames containing the encoder data only show up asynchronously every 10ms at most.
  3. If the control inputs are maxxing out a lot, the reduction in control authority manifests as reduced performance. For drivetrains, this could be caused by poor gearing, a dead/old battery, or just drawing a lot of current at once. PWM applies a percentage of the max bus voltage, and the bus voltage drops under load, which reduces the effective force being applied.
  4. Really noisy measurements can make the controller jitter too. Filtering fixes this.
  5. To get a sense of your disturbance rejection, you can do feedback-only, but keep in mind your best performance will be had with feedforward included because it reacts quicker to desired movements than feedback, which only acts when the error has already grown large.

Feel free to post any plots here, and I can do a more specific analysis. Make sure to include titles, axis labels with units, and a legend for when there’s more than one thing on a plot. I mention this because a lot of people don’t do this stuff and it’s harder to help them. :slightly_smiling_face:

Thank you for the help. At this point, I am going to do a bit more research into trajectory and see how this all fits. Looking at the code samples for Swerve Drive the feedforward is used like this.

m_driveMotor.setVoltage(driveOutput + driveFeedforward);
m_turningMotor.setVoltage(turnOutput + turnFeedforward);

JMH