Debugging a Ramsete Controller

, , ,

Last updated: 2023-2-24

One of the more interesting (and sometimes more cantankerous) features in the WPILib software library is the Ramsete controller—a controller that can cause a differential drive to follow a path to a particular location and pose in space. While it’s rewarding to get it working, it has a lot of moving parts and there are lots of ways for it to fail.

This year, I found several of them working with team 2051 in the FIRST Robotics competition. For our future reference (and to help any other teams out there), we’re documenting things we learned while debugging our implementation.

Note that this is not a guide on how to set up a Ramsete controller initially; the WPILib guide is already quite good for explaining all the steps you’ll need to follow. There is also a Troubleshooting guide there that is excellent; I would check there first and then use this guide as a supplement.

Preamble out of the way: here’s what we’ve learned so far working with the Ramsete controller.

The fundamentals

Slow is more accurate than fast

If the robot isn’t keeping on course, try driving the course slower. Lower speeds give the robot enough time to follow the commands.

Go slow enough to maintain control authority

Imagine you’re running your motors at 50% and you need to turn. You can do so by speeding one motor up and slowing the other down. Now imagine you’re running them at 99%. Uh-oh… The only way to turn is to slow the opposite motor down, and your turn won’t be as tight.

The term for the ability you have to cause change to your system by changing an input is generally called control authority, and when you run near the top of your motor speed, the Ramsete controller will have a hard time tracking the trajectory because it has fewer options to adjust. To figure out if this is the problem, graph your motor outputs while you run through your trajectory and look for hitting 100% or -100%. You may even want to use NetworkTables to “latch” an alert on your dashboard if you hit the limit during autonomous so you can go back later and figure out why you’re hitting it.

To maintain control authority, try limiting your top speed and top acceleration.

Debug black boxes through divide and conquer

One unfortunate property of Ramsete and the pieces it relies upon is that (unless you copy the implementations out of the source code and make your own) it’s a “black box”—a system where you can observe its inputs and outputs but it’s hard to observe the internal state. Fortunately, there’s an art to debugging a black box that you can take advantage of.

Let’s look at the source code provided in the WPILib docs for setting up the Ramsete command. I’ve added several /// Letter tags to denote sections of the code.

/**
   * Use this to pass the autonomous command to the main {@link Robot} class.
   *
   * @return the command to run in autonomous
   */
  public Command getAutonomousCommand() {
    // Create a voltage constraint to ensure we don't accelerate too fast
    /// A
    var autoVoltageConstraint =
        new DifferentialDriveVoltageConstraint(
            new SimpleMotorFeedforward(
                DriveConstants.ksVolts, /// B
                DriveConstants.kvVoltSecondsPerMeter, /// C
                DriveConstants.kaVoltSecondsSquaredPerMeter), /// D
            DriveConstants.kDriveKinematics,
            10);

    // Create config for trajectory
    TrajectoryConfig config =
        new TrajectoryConfig(
                AutoConstants.kMaxSpeedMetersPerSecond,
                AutoConstants.kMaxAccelerationMetersPerSecondSquared)
            // Add kinematics to ensure max speed is actually obeyed
            .setKinematics(DriveConstants.kDriveKinematics)
            // Apply the voltage constraint
            .addConstraint(autoVoltageConstraint);

    // An example trajectory to follow.  All units in meters.
    Trajectory exampleTrajectory =
        TrajectoryGenerator.generateTrajectory(
            // Start at the origin facing the +X direction
            new Pose2d(0, 0, new Rotation2d(0)),
            // Pass through these two interior waypoints, making an 's' curve path
            List.of(new Translation2d(1, 1), new Translation2d(2, -1)),
            // End 3 meters straight ahead of where we started, facing forward
            new Pose2d(3, 0, new Rotation2d(0)),
            // Pass config
            config);

    RamseteCommand ramseteCommand =
        new RamseteCommand(
            exampleTrajectory,
            m_robotDrive::getPose,
            new RamseteController(AutoConstants.kRamseteB, AutoConstants.kRamseteZeta),
            new SimpleMotorFeedforward(
                DriveConstants.ksVolts,
                DriveConstants.kvVoltSecondsPerMeter,
                DriveConstants.kaVoltSecondsSquaredPerMeter),
            DriveConstants.kDriveKinematics,
            m_robotDrive::getWheelSpeeds,
            new PIDController(DriveConstants.kPDriveVel, 0, 0),
            new PIDController(DriveConstants.kPDriveVel, 0, 0),
            // RamseteCommand passes volts to the callback
            m_robotDrive::tankDriveVolts,
            m_robotDrive);

    // Reset odometry to the starting pose of the trajectory.
    m_robotDrive.resetOdometry(exampleTrajectory.getInitialPose());

    // Run path following command, then stop at the end.
    return ramseteCommand.andThen(() -> m_robotDrive.tankDriveVolts(0, 0));