Multiple PID Loops - Java

[tomy]

This year my team is using robot build to start our base code in java. We want to have multiple pid loops for our drive train using three sensors; encoders, gyro and ultra sonic sensor. What is the best way to do this in a command based robot?

[mikets]

We don't do command-based but our library does support a PidDrive class that takes 3 PID controllers so it can shift power between the wheels from reading various sensors.
https://github.com/trc492/Frc2016Fir...cPidDrive.java

[Brian Selle]

I would recommend making a single PID loop with multiple modes. Use commands to switch the mode and set distance, angle, etc.

[tomy]

Originally Posted by Brian Selle

I would recommend making a single PID loop with multiple modes. Use commands to switch the mode and set distance, angle, etc.

So you would do something like this with more sensors?

public class DriveTrain extends PIDSubsystem {


    private final AnalogGyro analogGyro1 = RobotMap.driveTrainAnalogGyro1;
    private final SpeedController speedController1 = RobotMap.driveTrainSpeedController1;
    private final SpeedController speedController2 = RobotMap.driveTrainSpeedController2;
    private final SpeedController speedController3 = RobotMap.driveTrainSpeedController3;
    private final SpeedController speedController4 = RobotMap.driveTrainSpeedController4;
    private final RobotDrive robotDrive41 = RobotMap.driveTrainRobotDrive41;


    public DriveTrain() {
 
        super("DriveTrain", 1.0, 0.0, 0.0);
        setAbsoluteTolerance(0.2);
        getPIDController().setContinuous(false);
        LiveWindow.addActuator("Drive Train", "PIDSubsystem Controller", getPIDController());


    }

    public void initDefaultCommand() {

    }

    protected double returnPIDInput(double sensor) {

        return sensor;

    }

    protected void usePIDOutput(double output) {

        speedController1.pidWrite(output);

    }
    
    public double returnGyro(){
    	return analogGyro1.pidGet();
    }
}

[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

[Brian Selle]

Originally Posted by tomy

So you would do something like this with more sensors?

Do you intend to use the sensors independently or coupled together? For example, would you like a drive-to-distance-PID that uses encoders for distance + a gyro to help go straight? Or perhaps you are looking to drive straight until the ultrasonic sensor trips? Some of these cases are easily handled by the default WPILib PID implementation, others not so much.

For theses cases, we typically use a separate control loop thread and switch between modes. The update method might look something like this:

	public void controlLoopUpdate() {
		if (controlMode == DriveTrainControlMode.JOYSTICK) {
			driveWithJoystick();
		}
		else if (!isFinished) {
			if (controlMode == DriveTrainControlMode.MP_STRAIGHT) {
				isFinished = mpStraightController.controlLoopUpdate(getGyroAngleDeg()); 
			}
			else if (controlMode == DriveTrainControlMode.MP_TURN) {
				isFinished = mpTurnController.controlLoopUpdate(getGyroAngleDeg()); 
			}
			else if (controlMode == DriveTrainControlMode.PID_TURN) {
				isFinished = pidTurnController.controlLoopUpdate(getGyroAngleDeg()); 
			}
		}
	}

For more detail, you can look at the Drivetrain subsystem in our 2016 code: https://github.com/Team3310/FRC-2016