Multiple PID Loops - Java

[pblankenbaker]

If you keep the drive subsystem simple (only providing motor control and sensor reading values), you can move the PID control into each type of command you want to create.

For example, assuming your drive subsystem provides methods to read/write the power to the left and right side of the robot and the ability to read the distance from the left and right side encoders, you could create a drive distance command shown below where you can set how far you want the left and right side to move (set them to the same value to move forward or backward, set them to different values for "interesting" moves).

NOTE: This code is only roughed in to demonstrate this approach, you will need to adjust/tweak in order to get it working on your robot.

package org.usfirst.frc.team868.robot.commands;

import edu.wpi.first.wpilibj.PIDController;
import edu.wpi.first.wpilibj.PIDOutput;
import edu.wpi.first.wpilibj.PIDSource;
import edu.wpi.first.wpilibj.PIDSourceType;
import edu.wpi.first.wpilibj.command.Command;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;

import org.usfirst.frc.team868.robot.subsystems.DriveSubsystem;

/**
 * PID command to drive the left and right side of a drive train a specific
 * distance using a PIDController.
 */
public class DriveDistanceCommand extends Command {
  // You will need to adjust your PID constants
  private static final double Kp = 0.02;

  private static final double Ki = 0.02;

  private static final double Kd = 0.00;

  // Set to false once you are done tuning the PID
  private static final boolean DEBUG = true;

  // PID controllers for each side of drive train
  private PIDController leftPID;

  private PIDController rightPID;

  // How far each side should go (in meters)
  private double leftDist;

  private double rightDist;

  // Drive subsystem that will expose motor control and distrance traveled
  private DriveSubsystem drive;

  // Used for diagnostic output to see how far each side was driven
  private double leftStart;

  private double rightStart;

  /**
   * Command to use PID control to drive a fixed distance.
   *
   * @param drive
   *          Drive subsystem to use.
   * @param leftM
   *          How far the left side of the drive should travel (meters).
   * @param rightM
   *          How far the right side of the drive should travel (meters).
   */
  public DriveDistanceCommand(DriveSubsystem drive, double leftM, double rightM) {
    this.drive = drive;
    requires(drive);

    // How far to travel
    leftDist = leftM;
    rightDist = rightM;

    //
    // Define PIDSource based on distance to travel
    //
    PIDSource leftSource = new PIDSource() {
      @Override
      public void setPIDSourceType(PIDSourceType pidSource) {
      }

      @Override
      public PIDSourceType getPIDSourceType() {
        // Distance type PID
        return PIDSourceType.kDisplacement;
      }

      @Override
      public double pidGet() {
        return drive.getLeftDist();
      }
    };

    PIDSource rightSource = new PIDSource() {
      @Override
      public void setPIDSourceType(PIDSourceType pidSource) {
      }

      @Override
      public PIDSourceType getPIDSourceType() {
        // Distance type PID
        return PIDSourceType.kDisplacement;
      }

      @Override
      public double pidGet() {
        return drive.getRightDist();
      }
    };

    //
    // Define PID outputs to set drive power
    //
    PIDOutput leftOut = new PIDOutput() {
      @Override
      public void pidWrite(double output) {
        drive.setLeftPower(output);
      }
    };

    PIDOutput rightOut = new PIDOutput() {
      @Override
      public void pidWrite(double output) {
        drive.setRightPower(output);
      }
    };

    // Initialize PID controllers
    leftPID = new PIDController(Kp, Ki, Kd, leftSource, leftOut);
    rightPID = new PIDController(Kp, Ki, Kd, rightSource, rightOut);

    // If debugging PID, then pollute dash board with some tuning values
    if (DEBUG) {
      SmartDashboard.putData("Left PID", leftPID);
      SmartDashboard.putData("Right PID", rightPID);
    }
  }

  @Override
  protected void initialize() {
    // Save distance at start (I don't like zeroing encoder counts - but this is
    // an option as well)
    leftStart = drive.getLeftDist();
    rightStart = drive.getRightDist();

    leftPID.setSetpoint(leftDist + leftStart);
    rightPID.setSetpoint(rightDist + rightStart);

    leftPID.enable();
    rightPID.enable();
  }

  @Override
  protected void execute() {
  }

  @Override
  protected boolean isFinished() {
    double closeEnough = 0.05; // 5 cm
    double leftErr = leftPID.getError();
    double rightErr = rightPID.getError();

    boolean leftIsClose = Math.abs(leftPID.getError()) <= closeEnough;
    boolean rightIsClose = Math.abs(rightPID.getError()) <= closeEnough;

    // If debugging PID, then pollute dash board with some tuning values
    if (DEBUG) {
      SmartDashboard.putNumber("Left Dist", drive.getLeftDist() - leftStart);
      SmartDashboard.putNumber("Right Dist", drive.getRightDist() - rightStart);
      SmartDashboard.putNumber("Left Power", drive.getLeftPower());
      SmartDashboard.putNumber("Right Power", drive.getRightPower());

      SmartDashboard.putNumber("Left Err", leftErr);
      SmartDashboard.putNumber("Right Err", rightErr);
      SmartDashboard.putBoolean("Left is Close", leftIsClose);
      SmartDashboard.putBoolean("Right Is Close", rightIsClose);
    }

    return leftIsClose && rightIsClose;
  }

  /** Shutdown PIDs and stop motors when ended/interrupted. */
  @Override
  protected void end() {
    leftPID.disable();
    rightPID.disable();
    drive.setLeftPower(0);
    drive.setRightPower(0);
  }

  /** Shutdown PIDs and stop motors when ended/interrupted. */
  @Override
  protected void interrupted() {
    end();
  }
}

Using this approach, you could create additional PID type drive commands (controlled by gyro, vision targetting, etc). The nice thing about this approach is that things are loosely coupled (someone adjusting a gyro controlled command is less likely to break your drive by distance command as the source code is cleanly separated).

Hope that helps,
Paul