[pblankenbaker]

If I understand you correctly, you are using the PID loop to maintain a certain rate (how quickly your wheels spin) and not distance (how many times to rotate the wheels).

We've found that the standard PID controller assumes that you are always trying to "move a distance". It tries to get the error to 0 and basis the power value directly on the error. This works well for distance (where you want 0 power when you have 0 error). This doesn't work well for rate based PIDS (where you want the power to settle in to a good non-zero value when the error goes to 0).

Basically, when we run into this situation, we use a wrapper class around our speed controllers that can be used as the parameter to the PIDController constructer. The wrapper class implements PIDOutput and treats each write as an adjustment to the current power setting instead of a direct setting. For example, it would look something like:

rightBackPID =  new PIDController(Kp, Ki, Kd, rightBackEncoder, new RateContollerMotor(rightBack));

Here is a reference implementation (I don't have access to a robot to test it on anymore):

package com.techhounds.robot.subsystems;

import edu.wpi.first.wpilibj.PIDOutput;
import edu.wpi.first.wpilibj.SpeedController;

/**
 * A wrapper around a SpeedController that is used as a PIDOutput for the
 * purpose of controlling the rate at which something spins.
 */
public final class RateControlledMotor implements PIDOutput {

    /**
     * Motor to adjust power on.
     */
    private final SpeedController _Motor;

    /**
     * Construct a new instance and associate a speed controller with the
     * object.
     *
     * @param motor The speed controller that is controlling the motor.
     */
    public RateControlledMotor(SpeedController motor) {
        _Motor = motor;
    }

    /**
     * Apply power value computed by PID to the motor.
     *
     * <p>
     * The standard PID system basis the power output on the amount of "error".
     * This results in the power going to 0 as the error goes to 0. While this
     * works well for a distance based PID (where you want to stop once you get
     * to where you are going). It does not work well for a rate system (where
     * you want to keep spinning at the same rate).</p>
     *
     * <p>
     * Instead of treating the value passed as a new power level, we treat it as
     * an adjustment to the current power level when we apply it.</p>
     *
     * @param output Power value to apply (computed by PID loop). Goes to zero
     * as we reach the desired spin rate.
     */
    public void pidWrite(double output) {
        // Treat new PID computed power output as an "adjustment"
        double rateOutput = _Motor.get() + output;
        rateOutput = Math.min(1.0, rateOutput);
        rateOutput = Math.max(-1.0, rateOutput);
        _Motor.set(rateOutput);
    }

}

Some tips on tuning. If you use this method, we've found that you typically need to set Kd and Kp values to non-zero and that Kd often ends up larger than Kp. Also, it is often useful to use the SmartDashboard when tuning these values. I'd recommend putting the robot on wheels and focusing on tuning one PID. However, I'm not certain if having the weight of the robot off of the wheels will change the desired PID values - but it may at least help you get to a reasonable starting point.

rightBackPID =  new PIDController(Kp, Ki, Kd, rightBackEncoder, new RateContollerMotor(rightBack));
// Enable tuning the Kp, Ki, Kd parameters on the smart dashboard
SmartDashboard.putData("Right Back", rightBackPID);