The problem of writing a PID Drive

Our team now has a very accurate limelight , but when other team’s robot bump us , our robot will shake and we can’t aim accurately . l think that the PID will help us , so how to accomplish it . l have seen the official wpilib documentation 's driving with a pid drivesubsystem , but l think it doesn’t solve my problem . So, can anybody help me.

What do you know about how PID works? Edit: that was phrased somewhat condescendingly. The motive behind the question is that it’s very hard to write and tune control loops without knowing how they work.

Would creating a button to reset the aim v position be a quick stopgap?

How to accomplish it ?

I would need to see the code and test it to know for sure, but there are a few ways.

  1. cancel the command and create a new one.
  2. send an error of 0 to the command for a bit of time.
  3. stop moving regardless of error.
  4. (most likely) a combination of elements above.

Based on the other posts. I am relatively certain you are running PID.

But may only be using the P loop.

1 Like

My code is below , but l can’t use my xboxcontroller to control it correctly , can you help me with it and tell my whether there is an error in it.

public void PIDDriveCar(double x,double z)

  { ; /* positive is right */

    double leftGoal=x-0.6*z;

    double rghtGoal=x+0.6*z;

    double leftInput=m_leftFront.getSelectedSensorPosition();


    double rghtInput=m_rghtFront.getSelectedSensorVelocity();

    m_diffDrive.tankDrive(leftPID.calculate(leftInput, leftGoal),rghtPID.calculate(rghtInput, rghtGoal));


Is this in Robot container?

If not, how is this inolemented?

Here is how we use the Xbox controller buttons in Robot container.

new JoystickButton(m_driverController, Button.kBumperRight.value)
        .whenPressed(() -> m_robotDrive.setMaxOutput(0.5))
        .whenReleased(() -> m_robotDrive.setMaxOutput(1));

You may not want to use the lambda, but send 0,0 to pidcar. It depends on how that is implemented.

Then do another that reinstates the PID. All of this would be quicker if it happened automatically, but that is a bit more complicated to implement.

Edit: m_drivercobtroller is our xboxcontroller.

I’m not sure what this code is supposed to do. What is x and z? What does multiplying z by 0.6 represent? Why is one side using encoder position and the other side using encoder velocity? What are the PID constants in your PID controller and how are they determined?

I use my drivecmd to use it to drive my car.

So then, if it is part of the drive command, maybe you could create a new drive command or use a method reference to interact directly with this method.

However, you should also look at the questions @Joe_Ross asked. Fixing those issues should help immensley.

x and z are got by my joystick (xboxcontroller), and below is my whole drive subsystem.

/* Copyright (c) 2018 FIRST. All Rights Reserved.                             */
/* Open Source Software - may be modified and shared by FRC teams. The code   */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project.                                                               */


import frc.robot.Robot;
import frc.robot.util.Constants;

import com.kauailabs.navx.frc.AHRS;

import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX;

import com.ctre.phoenix.motorcontrol.NeutralMode;

import edu.wpi.first.wpilibj.Encoder;
import edu.wpi.first.wpilibj.geometry.Pose2d;
import edu.wpi.first.wpilibj.kinematics.DifferentialDriveOdometry;
import edu.wpi.first.wpilibj.kinematics.DifferentialDriveWheelSpeeds;
import edu.wpi.first.wpilibj.controller.PIDController;
import edu.wpi.first.networktables.NetworkTable;
import edu.wpi.first.networktables.NetworkTableInstance;

import edu.wpi.first.wpilibj2.command.SubsystemBase;
import edu.wpi.first.wpiutil.math.MathUtil;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;

 * Add your docs here.
public class Drive extends SubsystemBase {

  public PIDController leftPID;
  public PIDController rghtPID;
  public WPI_TalonFX m_leftFront;
  public WPI_TalonFX m_leftFollower;
  public WPI_TalonFX m_rghtFront;
  public WPI_TalonFX m_rghtFollower;
  public Encoder m_leftEncoder;
  public Encoder m_rghtEncoder;

  public DifferentialDrive m_diffDrive;
  public DifferentialDriveOdometry m_odometry;

  public AHRS m_navx;

  public NetworkTable m_limTable;

  public double tv ;
  public double ta ;
  public double tx ;
  public double ty ;

  public boolean auto = false;
  double LP=0.2,LI=0,LD=0.4;
  double RP=0.2,RI=0,RD=0.4;
  int coast =1;
  int brake = 0;
  public int drivemode;
  public boolean mode = false;

  public AHRS m_ahrs;

  //Speed threshold
  double speed1 =  1;

  public Drive() {
    //here is todo
    leftPID = new PIDController(LP, LI, LD);
    rghtPID = new PIDController(RP, RI, RD);
    m_leftFront = Robot.hardware.m_leftFront;
    m_leftFollower = Robot.hardware.m_leftFollower;
    m_rghtFront = Robot.hardware.m_rghtFront;
    m_rghtFollower = Robot.hardware.m_rghtFollower;
    m_leftEncoder = Robot.hardware.m_leftEncoder;
    m_rghtEncoder = Robot.hardware.m_rghtEncoder;
    m_diffDrive = Robot.hardware.m_diffDrive;
    m_odometry = Robot.hardware.m_odometry;

    m_navx = Robot.hardware.m_navx;

    // Sets the distance per pulse for the encoders

    m_limTable = NetworkTableInstance.getDefault().getTable("limelight");


  public void periodic() {
    // Update the odometry in the periodic block
    m_odometry.update(m_navx.getRotation2d(), m_leftEncoder.getDistance(),

  public void simulationPeriodic() {
    // This method will be called once per scheduler run during simulation

  public double Get_tx(){
    tx = m_limTable.getEntry("tx").getDouble(0);
    SmartDashboard.putNumber("tx", tx);
    return tx;

  public double Get_ty(){
    ty = m_limTable.getEntry("ty").getDouble(0);
    SmartDashboard.putNumber("ty", ty);
    return ty;

  public double Get_ta(){
    ta = m_limTable.getEntry("ta").getDouble(0);
    return ta;

  public double Get_tv(){
    tv = m_limTable.getEntry("tv").getDouble(0);
    return tv;

  public void PIDDriveCar(double x,double z)
  { ; /* positive is right */
    double leftGoal=x-0.6*z;
    double rghtGoal=x+0.6*z;
    double leftInput=2*m_leftFront.getSelectedSensorVelocity();
    double rghtInput=2*m_rghtFront.getSelectedSensorVelocity();
    m_diffDrive.tankDrive(leftPID.calculate(leftInput, leftGoal),rghtPID.calculate(rghtInput, rghtGoal));

  public void DriveCar(double x,double z){

    SmartDashboard.putNumber("x", x);
    SmartDashboard.putNumber("z", z);

    /* get gamepad stick values */
    double forw = -1 * x ; /* positive is forward */
    double turn = +1 * z * 0.65 ; /* positive is right */

    /*drive the robot*/
    if(drivemode == 1){
      tankDriveVolts(1.2 , - 1.2);
    else if(drivemode == 2){
      tankDriveVolts(- 1.2 , 1.2);
    else if(drivemode == 3){
      tankDriveVolts(- 1.2 , - 1.2);
    else if(drivemode == 4){
      tankDriveVolts(1.2 , 1.2);
      m_diffDrive.arcadeDrive(forw, turn);
    SmartDashboard.putNumber("drivemode", drivemode);


public void setLightMode(int mode){

public void setFast(){
    speed1 += 0.1;

    speed1 = MathUtil.clamp(speed1,-1.0,1.0);

public void setSlow(){
      speed1 -= 0.1;
      speed1 = MathUtil.clamp(speed1,0,1.0);

public void enableMotors(boolean on){
  NeutralMode mode;
    mode = NeutralMode.Brake;
    mode = NeutralMode.Coast;

public void setFowardLittle(){
  m_diffDrive.arcadeDrive(0.2 , 0);

public void setBackLittle(){
  m_diffDrive.arcadeDrive(- 0.2 , 0);

public void setRghtLittle(){
  m_diffDrive.arcadeDrive(0 , 0.2);

public void setLeftLittle(){
  m_diffDrive.arcadeDrive(0 , - 0.2);

  * Resets the drive encoders to currently read a position of 0.
public void resetEncoders() {

  * Returns the currently-estimated pose of the robot.
  * @return The pose.
public Pose2d getPose() {
  return m_odometry.getPoseMeters();

  * Returns the current wheel speeds of the robot.
  * @return The current wheel speeds.
public DifferentialDriveWheelSpeeds getWheelSpeeds() {
  return new DifferentialDriveWheelSpeeds(m_leftEncoder.getRate(), m_rghtEncoder.getRate());

  * Resets the odometry to the specified pose.
  * @param pose The pose to which to set the odometry.
public void resetOdometry(Pose2d pose) {
  m_odometry.resetPosition(pose, m_navx.getRotation2d());

  * Controls the left and right sides of the drive directly with voltages.
  * @param leftVolts  the commanded left output
  * @param rightVolts the commanded right output
public void tankDriveVolts(double leftVolts, double rightVolts) {

  * Gets the average distance of the two encoders.
  * @return the average of the two encoder readings
public double getAverageEncoderDistance() {
  return (m_leftEncoder.getDistance() + m_rghtEncoder.getDistance()) / 2.0;

  * Gets the left drive encoder.
  * @return the left drive encoder
public Encoder getLeftEncoder() {
  return m_leftEncoder;

  * Gets the right drive encoder.
  * @return the right drive encoder
public Encoder getRightEncoder() {
  return m_rghtEncoder;

  * Sets the max output of the drive.  Useful for scaling the drive to drive more slowly.
  * @param maxOutput the maximum output to which the drive will be constrained
public void setMaxOutput(double maxOutput) {

  * Zeroes the heading of the robot.
public void zeroHeading() {

  * Returns the heading of the robot.
  * @return the robot's heading in degrees, from -180 to 180
public double getHeading() {
  return m_navx.getRotation2d().getDegrees();

  * Returns the turn rate of the robot.
  * @return The turn rate of the robot, in degrees per second
public double getTurnRate() {
  return -m_navx.getRate();


l use the getSelectedSensorVelocity() in both , there is some error in my former code.l multiple 0.6 to minimize the error and l am new to PID , so can you help me with it?

Seeing the whole subsystem helps a bit.

PID is means of controlling an output based on the input of a sensor. The FRC Docks on PID or This non-FRC Website is a good overview, but both can get a bit technical. When I first started looking into PID, I got annoyed that most of the tutorials I found online did not involve robots, but now, I think looking at those helped me understand how PID actually works. The FRC Docs Video is helpful too.

If I understand correctly, the .6 comes from the limelight tutorial and is the minimum power to drive (this is actually similar to a feedforward, though that seems quite high). If so, it should come after the PID is calculated (applied directly to the motor output, not as a piece of the PID ALgorithm).

Using the driver input during the PID may be where the oscillation error comes into play. A PID takes the input and evaluates an output based on the error, then looks again, and adjusts accordingly. It keeps going until it is stopped. The idea is that it gets you to the setpoint as soon as possible, and keeps you there. This is why it is referred to as a closed-loop. If you begin bringing in other variables, it gets confused and the adjustments it makes do not correctly affect the output.

You may want to take a look at the WPILib PID Implementation docs to understand how PID typically works in FRC.

WPILib Tuning docs help as a starting point to learn how to tune one. Also, I forgot initially, but Tim Winters’ Programming Done Right has a good section on PID. KNow that some of the implementations they discuss is outdated, but it is helpful in understanding what is going on under the hood of a PID controller.

Edit: In my haste this morning, I did not explain the links, so I am going back and doing that.

We’ve been perfecting PID drive for a couple of years now, and its working pretty darn well now. We’re also working on a primitive sort of traction control as well.

Thanks for providing such a good resource. l will dig into it soon. However , l am focus on swerve drive now . And it seems that l also have something uncertain about its PID parameters. Do you have any good resources?

What uncertainties do you have regarding the PID parameters? Are you unsure of how to tune the values?
For me, there isn’t really a algorithmic approach, its just by feel. I increase P until there’s some oscillation, and then start slowly increasing D. For drive PID, we don’t use the integral term.

FYI, there’s a more rigorous way to do it, but you need to mathematically model your robot mechanism.

Given a model, the LinearQuadraticRegulator class finds the gains for an array of P controllers that optimizes a tradeoff between error and control input over time. WPILib has functions for turning characterization gains from frc-characterization or SysId into a model LQR can use. The characterization tools even do the feedback controller synthesis for you if you just care about the gains.


Or Ziegler-Nichols and friends, for many scenarios

1 Like

We’ve tried the Ziegler-Nichols method before. We didn’t really get it to work, at least for drive. Can’t remember if we tried it for any other part of the robot.

Model-based control is the way to go, even if you just use the model to bootstrap reasonable PID gains from hardware specs.

There’s no reason to tune parameters from scratch once you understand the underlying dynamics - and if you don’t understand the underlying dynamics, you should spend the time to learn them (for the simple cases they’re not hard…).

Sorry to bother you again. How do you tune your velocity closed loop. It seems that there isn’t any obvious oscillation in velocity closed loop. So , how to tune the PID parameters of it?

In CTRE docs, it says as below.

So, l need to use tuner to see whether the sensed value is close to the target value. Howevevr , what’s the situation if the P gain is tuned properly? How should l judge it. In position closed loop, it seems that if the oscillation disappers , the P gains is tuned properly. However , in velocity closed loop , does it also adapt ?

Besides , does the turning motor also need a feed-forward gain?