WPILib PIDController Question

[astronautlevel]

Hi, our team has been having a lot of trouble using the PIDController class to move an arm on our robot based on an angle sensor. We're not really sure how to use the PIDController, for example, what methods are necessary to initialize the PIDController for it for set motor values correctly? We are using Java and the Command-based Architecture.

Here is what our code is doing (within a Subsystem class):

public static Double setpoint;

private final CANTalon angleMotor = new CANTalon(Ports.SHOOTER_ANGLE_PORT);

PIDController controller;

PIDSource source;

public AnglePIDControllerSubsystem() {
source = new PIDSource() {

@Override
public void setPIDSourceType(PIDSourceType pidSource) {

}

@Override
public double pidGet() {
System.out.println("Getting value: " + currentAngle);
return currentAngle;
}

@Override
public PIDSourceType getPIDSourceType() {
return PIDSourceType.kDisplacement;
}
};
controller = new PIDController(10, 0 ,0, source, angleMotor);
controller.setContinuous(false);
controller.setAbsoluteTolerance(3);
controller.setInputRange(-90, 90);
controller.setOutputRange(-1, 1);
angleMotor.changeControlMode(CANTalon.TalonControl Mode.PercentVbus);
angleMotor.ConfigFwdLimitSwitchNormallyOpen(true);
angleMotor.ConfigRevLimitSwitchNormallyOpen(true);
angleMotor.enableForwardSoftLimit(false);
angleMotor.enableReverseSoftLimit(false);
controller.enable();
}

void setSetpoint(double val) {
controller.setSetpoint(val*10);
System.out.println("Voltage: " + controller.get());
System.out.println("Error: " + controller.getError());
System.out.println("Setpoint: " + controller.getSetpoint());
System.out.println("On target: " + controller.onTarget());
}

The default command for this subsystem is calling setSetpoint each cycle.

Has anyone else had success with the PIDController?

[pblankenbaker]

It's a bit difficult to tell what issues you are having without seeing all of the code (in particular I could not tell how you were reading the sensor and determining the current angle), but here are some suggestions/comments.

If you haven't taken a look at the WPIlib docs on PIDController yet, check out: http://wpilib.screenstepslive.com/s/4485/m/13809/l/241879-operating-the-robot-with-feedback-from-sensors-pid-control

If your class extends PIDSubsystem instead of Subsystem, then you should probably remove your current PIDController object and refer to the WPIlib docs mentioned above.

If your class extends Subsystem, then having your own PIDController object (controller) makes sense.

Your P value of 10 looks way big for an initial starting point. The P value is multiplied by the error to come up with a motor power value in your PID. Hence a 3 degree error would result in a motor power of 30.0 given your 10.0 P multiplier. I would suggest you start with a much smaller value (like 0.01) and work your way up. Using the SmartDashboard PID control can be very useful for this (enabling this is described later).

Once you get close on your P value, try adding some D to reduce oscillation.

Make sure you look for inversion of source or power (if your shooter heads in the wrong direction, it typically means that the motor power or sensor reading needs to be inverted).

Your pidGet() method looks something like the following:


@Override
public double pidGet() {
System.out.println("Getting value: " + currentAngle);
return currentAngle;
}


Hopefully the currentAngle field is being continuously updated. Otherwise, I would suggest refactoring so that you are sure you are taking a fresh reading from your sensor and converting it to degrees:


@Override
public double pidGet() {
return readAngle();
}

// Later, in your subsystem add the readAngle() command ...

/**
* Reads voltage from potentiometer and converts to shooter angle in degrees.
*/
double readAngle() {
// Example of reading voltage from AnalogInput
double volts = anglePot.get();
// Convert voltage to degrees (you will need to determine the conversion
// constants shown below based on your robot)
double degs = volts / VOLTS_PER_DEG + DEG_OFS;
return degs;
}


Next, it is often helpful to put the PIDController to the dashboard so you can play with the values. Add the following line after "controller.enable()", to put a control on the smart dashboard that you can use to play with P, I and D values as you tune your PID:


// Temporarily add widget to dashboard to play with PID settings
SmartDashboard.putData("Angle PID", controller);


Along those lines, it is sometimes easier to work with SmartDashboard information than System.out logging. You may want to add a method that you periodically call on your dashboard when debugging your PID:


void updateDashboard() {
SmartDashboard.putNumber("Angle Degs", readAngle());
// Comment following when done tuning PID
SmartDashboard.putNumber("Angle Volts", anglePot.get());
SmartDashboard.putNumber("Angle Error", controller.getError());
SmartDashboard.putNumber("Angle Setpoint", controller.getSetpoint());
SmartDashboard.putBoolean("Angle On target", controller.onTarget());
SmartDashboard.putNumber("Angle Power", angleMotor.get());
}



"Angle Degs" - shows what the PIDController is currently using to determine the error.
"Angle Error" - shows how far off the PIDController things it is (this should go to zero).
"Angle Power" - shows the last power setting applied to the motor.
"Angle On Target" - shows when the PIDController things the current angle is good enough.