Motion PID Control

Now that our robot has some basic functionality we're going to add some new commands that'll give us more control over how it moves. We'll be using a PID Controller that enables the robot to constantly monitor its current state and adjust its motor speeds as it drives towards its goal. Since we're using PID to control motion we'll refer to this as Motion PID Control, the PID stands for Proportional, Integral, and Derivative. PID control is expained in-depth in the Classical Control module of this training guide. You should read that module before moving on.

Motion PID Control allows our robot to move autonmously. In this module we'll create two new commands that we will test in Autonomous mode from the SendableChooser dropdown menu.

  • DriveDistancePID that will drive the robot a specified distance.

  • TurnToAnglePID that will allow the robot to turn to a specified angle.

PID Controller

A PID controller has been implemented by the WPI library. The controller is used by a PID command to autonomously move the robot from one position to another. The command sets up a feedback loop where it reads the robot's sensors to find its current position and then applies power to each of the motors driving it towards its setpoint. The power output is calculated by the PID controller using the P, I, and D values passed in at the start of the procedure. The measurement source is a function provided by the Drivetrain that constantly measures to robot's current position. The output is another Drivetrain function that sends power to the motors.

PID Controller

Tuning the PID Controller

To get the PID controller to perform properly it will will most likely need to be tuned. The Tuning a PID Controller documentation gives some information on the process. Tuning the PID controller can be done in the Simulator or Shuffleboard. More information can be found in Testing and Tuning PID Loops.

Setting up the Gyro

We have already setup the getHeading() method in the Subsystems module but there are a few of things we need to do in order to setup the gyro as a measurement source.

  • Ensure that the gyro is calibrated, which is done on the Romi Website. Follow the IMU Calibration instructions.

  • Set enableContinuousInput(-180, 180) in your PID turn commands. Rather then using the max and min input range as constraints, it considers them to be the same point and automatically calculates the shortest route to the setpoint.

  • Reset the gyro angles each time we start the program. This is done in the Drivetrain constructor where is calls its own resetGyro() method.

The Java Supplier/Consumer Interface

Normally, you'll be passing parameter values to a function, such as double, and that function will perform some operation on them. Java also provides parameter types of Supplier and Consumer, which are used in a more advanced Java programming paradigm called Functional Interfaces. With Functional Interfaces you do "behavior parameterization" instead of "value parameterization". In other words, you can pass around functionality (i.e. behavior). Let's look at Suppliers and Consumers in more detail and see why we would want to use them.

A Supplier is a method that returns some value. That could be a simple value like a double, but more usefully it's going to be a function that returns a value. This is very useful for supplying a continuous stream of data. Let's say that we want to continuously track where a robot is, then you would assign a function to return its current position. The function that supplies the values is assigned using a lambda expression () ->. A Supplier has only a single method called get(), which "gets" the value from the assigned function when it's invoked.

A Consumer is a method that consumes some value and does some operations on them. In Java terms, a Consumer is similar to a void method. This is useful for doing operations on a continuous stream of data. An example of this could be a motor "consuming" a continuous stream of power inputs. The Consumer's functional method is accept(Object). It can be used as the assignment target for a lambda expression or method reference.

Consumers and Suppliers

In the above diagram a Supplier is providing a continuous stream of data from the measurement method distanceMeters(). The method arcadeDrive() is consuming the calculated power output since it's setup as a Consumer. The two methods are assigned in the PID command's constructor.

Lab - Motion Control PID

This lab builds on the code that you wrote in the Telemetry section of the training guide. In this lab you'll learn about the following Java programming concepts:

  • Java Lambdas A lambda expression is a short block of code which takes in parameters and returns a value. Lambda expressions are similar to functions, but they do not need a name and they can be implemented right in the body of a method.

  • Functional Interfaces specifically the Supplier and Consumer interfaces.

  • The Java keyword super to call superclass (parent) class methods.

There is one task for this lab:

  • DriveDistancePID that will drive the robot a specified distance.

Drive Robot a Specified Distance

To create a PIDCommand in VSCode right click under the commands folder and select Create a new class/command. Then select PIDCommand from the drop down list. Call the command DriveDistancePID. The constructor of the new command is shown in the diagram.

Commands

We're going to modify this command to adapt it to our specific need of driving the robot a specified distance. Here's a pictorial representation of how we need to setup our PID controller.

PID Command

Notice that the PIDCommand is instantiating a PIDController, which is an algorithm that implements the control of our robot. The PIDController class needs to know what its Proportional, Integral, and Derivative values are. We're going to start with the P value set to 1.2 and the I, and D values set to zero. Since these values are constants they should be put in the Constants file. Here's how they should be defined:

// For distances PID
public static final double kPDriveVel = 1.1;
public static final double kIDriveVel = 0.0;
public static final double kDDriveVel = 0.0;

Go back to the DriveDistancePID command and passed these values into the PIDController as parameters. You'll need to import the Constants class:

new PIDController(Constants.kPDriveVel,
                  Constants.kIDriveVel,
                  Constants.kDDriveVel),

We need to pass in the target distance to tell the command how far to drive together with the Drivetrain class. These two parameters are passed in when the DriveDistancePID constructor is called and the Command object is created. We'll add the Drivetrain as a requirement. 

public DriveDistancePID(double targetDistance, Drivetrain drivetrain) {

The targetDistance that you passed in becomes the setpoint for the PID controller, so you can replace the setpoint value 0 with targetDistance. Like so:

// This should return the setpoint (can also be a constant)
() -> targetDistance,

Next, we're going to add in the feedback part of our PID control loop, which in this case is the current distance that the robot has travelled. Remember, that we have a method in the Drivetrain class called getAverageDistanceMeters(). We'll used this as our measurement source for our PID Controller. Use the getAverageDistanceMeters() method in the lambda expression, like this:

// This returns the measurement from the encoders
() -> drivetrain.getAverageDistanceMeters(),

This parameter of the PIDCommand is defined as type Supplier. We're assigning the getAverageDistanceMeters() function to that Supplier, which it will invoke whenever the PID controller needs it. See the The Java Supplier/Consumer Interface section of this module.

Once the controller has calculated how much power is required for the motors the value is output to the DriveTrain's arcadeDrive() method. The output parameter is defined as a Consumer and is assigned to the arcadeDrive() method. Therefore, arcadeDrive() will "consume" the calculated value.

output -> {
  // Use the output here
  drivetrain.arcadeDrive(output, 0);
}

The measurementSource and output setup a looping arrangement which moves the robot towards the setpoint. In our case, the measurement source are the encoders, that are measuring distance, and the output is a power value between 0.0 and 1.0 that is sent to the motors in order to move the robot. Once the setpoint is reached the command will finish.

The full constructor for our DriveDistancePID command is listed below.

public DriveDistancePID(double targetDistance, Drivetrain drivetrain) {
super(
  // The controller that the command will use
  new PIDController(Constants.kPDriveVel,
                    Constants.kIDriveVel,
                    Constants.kDDriveVel),
  // This should return the measurement
  () -> drivetrain.getAverageDistanceMeters(),
  // This should return the setpoint (can also be a constant)
  () -> targetDistance,
  // This uses the output
  output -> {
    // Use the output here
    drivetrain.arcadeDrive(output, 0);
  });

  // Use addRequirements() here to declare subsystem dependencies.
  addRequirements(drivetrain);

  // Configure additional PID options by calling `getController` here.
}

The last thing we need to do is tell the command to finish once it has reached the setpoint. The PID controller has a method called atSetpoint() that returns the boolean value true if the setpoint has been reached. Remember, that our setpoint is the targetDistance that we assigned in the PIDCommand's constructor. This value is returned in the isFinished() method of our DriveDistancePID command.

public boolean isFinished() {
  return getController().atSetpoint();
}

In order to run the command you'll need to add it to the SendableChooser in the RobotContainer class. Have the robot travel for distance of 0.5 meters.

this.chooser.addOption("Drive Distance PID", new DriveDistancePID(0.5, this.drivetrain));

Testing the DriveDistancePID Command

Now connect your laptop to a Romi and test your code. When the Simulator starts we'll need to pull some components onto the dashboard in order to see how the command is functioning. First make sure that you have the dropdown list of commands by selecting NetworkTables->SmartDashboard->SendableChooser. Then from NetworkTables->LiveWindow select the two components shown on the picture below. When you're done your dashboard should show the following four components:

Testing PID Command

Select the DriveDistancePID command and run it in Autonomous mode. You should see your robot move forward. However, there may be a problem! If you look at the /LiveWindow/Drivetrain component you may find that the command is still running. Somehow it didn't finish. Checking the distance travelled you see that it hasn't reached 0.5 meters, it never gets to the setpoint. This is because with just the P parameter set the output value gets so small that it can no longer drive the motors. In order to have it complete you would need to add a value to the I parameter. Try assigning a value of 0.2 to see if the command to finishes.

Another thing that we might want to consider is how close to the setpoint is "good enough"? Maybe if we're within a certain percentage of the setpoint then that would be acceptable. The setTolerance() method sets the position and velocity error which is considered tolerable for use with the setpoint. Place this just after addRequirements() statement.

// Configure additional PID options by calling `getController` here.
getController().setTolerance(0.05, 0.06);

Now test the command again. If the command still doesn't finish try changing the P and I parameters together with the setTolerance() values. This is part where you learn to do PID tuning. Keep on tuning the parameters until you're happy with the results.

For more details on what we've just done read the PID Control through PIDSubsystems and PIDCommands section of the FRC documentation.

When you're happy with how the robot is moving you've now completed this task!

References