PID Speed control with encoders

[pensono]

Hello, I am trying to set up a PID loop where I can tell my drive motors to move the robot at a certain velocity. I am using encoders to measure my current velocity, and I want to set up a PID loop to bridge the two.

I don't know how to set it up, but I will show what I have so far. I've cut it down to show only the left half of the drive train. We call the drive function to give it a throttle amount where 1 is max speed. Could somebody give example code, or correct my mistakes here so I can fix it?

Thanks!

DriveTrain.java:

public class DriveTrain extends Subsystem {
	private Encoder leftEncoder;

	private PIDController leftPID;

	public DriveTrain() {
		VictorSP frontLeft = new VictorSP(DRIVE_PORT_FRONT_LEFT);
		VictorSP rearLeft = new VictorSP(DRIVE_PORT_REAR_LEFT);
		double p = 1.0;
		double i = 0;
		double d = 0;

		leftEncoder = new Encoder(DRIVE_PORT_FRONT_LEFT, DRIVE_PORT_REAR_LEFT);
		leftEncoder.setDistancePerPulse((8 * Math.PI) / 360);
		leftEncoder.setPIDSourceType(PIDSourceType.kRate);

		leftPID = new PIDController(p, i, d, leftEncoder, new DualMotorPID(frontLeft, rearLeft));
		leftPID.setOutputRange(-.5, .5);
		leftPID.setContinuous(true);

		leftPID.enable();
	}

	public void initDefaultCommand() {
		setDefaultCommand(new TeleOpDrive());
	}

	public void drive(double moveValue, double rotateValue) {
		//Copied from RobotDrive::arcadeDrive
		double leftMotorSpeed, rightMotorSpeed;
		
		if (moveValue > 0.0) {
			if (rotateValue > 0.0) {
				leftMotorSpeed = moveValue - rotateValue;
				rightMotorSpeed = Math.max(moveValue, rotateValue);
			} else {
				leftMotorSpeed = Math.max(moveValue, -rotateValue);
				rightMotorSpeed = moveValue + rotateValue;
			}
		} else {
			if (rotateValue > 0.0) {
				leftMotorSpeed = -Math.max(-moveValue, rotateValue);
				rightMotor
Speed = moveValue + rotateValue;
			} else {
				leftMotorSpeed = moveValue - rotateValue;
				rightMotorSpeed = -Math.max(-moveValue, -rotateValue);
			}
		}
		leftPID.setSetpoint(leftMotorSpeed);
	}
}

and DualMotorPID.java This lets us control two mechanically bound motors with one PID

public class DualMotorPID implements PIDOutput{
	private SpeedController motorA;
	private SpeedController motorB;
	
	public DualMotorPID(SpeedController a, SpeedController b){
		motorA = a;
		motorB = b;
	}
		
	@Override
	public void pidWrite(double output) {
		motorA.set(output);
		motorB.set(output);
	}
}

[pblankenbaker]

Not sure if you are still fighting with this issue or not, but here are some things we have found useful when setting up speed/rate control PIDs:

1. Use the SmartDashboard to display the PID control editor (so you can play with your P, I, D and F values).

2. Use the SmartDashboard to display the output power (pidWrite value) that the PIDController has determined, the sensor input value for the PID, the current error and the target value.

3. For a rate controlled PID, we found that wpilib seems to be slightly different this year and that you need to play with the P, D and F values. Rate PIDs are slightly different in nature from position PIDs.

Here is an example of putting out the PID controller for your left side PID (add to the bottom of your DriveTrain() constructor):

SmartDashboard.putData("Left PID", leftPID);

And here is an example of putting out the other smart dashboard values (place at the end of your drive() method):

// Value starts big and goes to zero as your PID reaches the setpoint.
SmartDashboard.putNumber("Left PID Error", leftPID.getError());

// Input (speed) being returned by the encoder (PID input)
// Value should go to the set point you specified
SmartDashboard.putNumber("Left PID Speed", leftEncoder.pidGet());

// Output (power) being applied to motors
SmartDashboard.putNumber("Left PID Output", frontLeft.get());

This should at least give you some information on the smart dashboard to show you what your PID is trying to do. Make sure that the "Left PID Speed" values make sense and are changing.

Hope that helps.