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();

    //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.                                                               */
/*----------------------------------------------------------------------------*/

package frc.robot.subsystems.drive;

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.drive.DifferentialDrive;
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 {

  //motor
  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;

  //Limelight
  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;

  //navX
  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_leftEncoder.setDistancePerPulse(Constants.DriveConstants.kEncoderDistancePerPulse);
    m_rghtEncoder.setDistancePerPulse(Constants.DriveConstants.kEncoderDistancePerPulse);
    
    resetEncoders();

    m_limTable = NetworkTableInstance.getDefault().getTable("limelight");
    m_diffDrive.setMaxOutput(speed1);

  }

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

  @Override
  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();
    //m_leftFront.getSelectedSensorPosition();
    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);
    }
    else{
      m_diffDrive.arcadeDrive(forw, turn);
    }
    SmartDashboard.putNumber("drivemode", drivemode);


}

public void setLightMode(int mode){
  m_limTable.getEntry("ledMode").setNumber(mode);
}

public void setFast(){
    speed1 += 0.1;

    speed1 = MathUtil.clamp(speed1,-1.0,1.0);
    m_diffDrive.setMaxOutput(speed1);
   
}

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

public void enableMotors(boolean on){
  NeutralMode mode;
  if(on){
    mode = NeutralMode.Brake;
  }
  else{
    mode = NeutralMode.Coast;
  }
  m_leftFront.setNeutralMode(mode);
  m_leftFollower.setNeutralMode(mode);
  m_rghtFront.setNeutralMode(mode);
  m_rghtFollower.setNeutralMode(mode);
  //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() {
  m_leftEncoder.reset();
  m_rghtEncoder.reset();
}

/**
  * 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) {
  resetEncoders();
  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) {
  m_leftFront.setVoltage(leftVolts);
  m_rghtFront.setVoltage(-rightVolts);
  m_diffDrive.feed();
}

/**
  * 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) {
  m_diffDrive.setMaxOutput(maxOutput);
}

/**
  * Zeroes the heading of the robot.
  */
public void zeroHeading() {
  m_navx.reset();
}

/**
  * 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.