PID loops using rate

[geel9]

Hey,

We're trying to get our PID loops to control the motors' rate, as opposed to their distance.

Here is our code:

//Initializes the motors.
    private final SpeedController left = new Jaguar(1);
    private final SpeedController right = new Jaguar(2);
    
    //Initializes the Encoders.
    private final Encoder leftEncoder = new Encoder(1, 2);
    private final Encoder rightEncoder = new Encoder(3, 4);
    
    //Proportional, Integral, and Dervative constants.
    //These values will need to be tuned for your robot.
    private final double Kp = 0.1;
    private final double Ki = 0.001;
    private final double Kd = 0;
    
    //This must be fully initialized in the constructor, after the settings
    //for the encoders have been done.
    private final PIDController leftPID;
    private final PIDController rightPID;

    public PIDDrive() {
        //Sets the distance per pulse in inches.
        leftEncoder.setDistancePerPulse(-1);
        rightEncoder.setDistancePerPulse(-1);

        
        //Starts the encoders.
        leftEncoder.start();
        rightEncoder.start();
        
        //Sets the encoders to use distance for PID.
        //If this is not done, the robot may not go anywhere.
        //It is also possible to use rate, by changing kDistance to kRate.
        leftEncoder.setPIDSourceParameter(Encoder.PIDSourceParameter.kRate);
        rightEncoder.setPIDSourceParameter(Encoder.PIDSourceParameter.kRate);
        
        //Initializes the PID Controllers
        leftPID = new PIDController(Kp, Ki, Kd, leftEncoder, left);
        rightPID = new PIDController(Kp, Ki, Kd, rightEncoder, right);
        
        leftPID.setPercentTolerance(25);
        rightPID.setPercentTolerance(25);
        
        leftPID.setContinuous();
        rightPID.setContinuous();
        
        //Enables the PID Controllers.
        leftPID.enable();
        rightPID.enable();
        

    }

    public static void WriteDriverStationLine(DriverStationLCD.Line line, String text){
        DriverStationLCD.getInstance().println(line, 1, text);
        DriverStationLCD.getInstance().updateLCD();
    }
    /**
     * This function is called once each time the robot enters operator control.
     * Teleop commands are put in here
     */
    public void operatorControl() {
        leftPID.setSetpoint(1000);
        rightPID.setSetpoint(1000);
        
        while(isOperatorControl()){
            
            WriteDriverStationLine(DriverStationLCD.Line.kUser1, "E: " + leftPID.getError());
            WriteDriverStationLine(DriverStationLCD.Line.kUser2, "E2: " + rightPID.getError());
            WriteDriverStationLine(DriverStationLCD.Line.kUser3, "D: " + leftEncoder.getRate());
            WriteDriverStationLine(DriverStationLCD.Line.kUser4, "D2: " + rightEncoder.getRate());
        }

The wheels spin forward, then rapidly switch directions.

[NotInControl]

Originally Posted by geel9

Hey,

We're trying to get our PID loops to control the motors' rate, as opposed to their distance.

Here is our code:

//Initializes the motors.
    private final SpeedController left = new Jaguar(1);
    private final SpeedController right = new Jaguar(2);
    
    //Initializes the Encoders.
    private final Encoder leftEncoder = new Encoder(1, 2);
    private final Encoder rightEncoder = new Encoder(3, 4);
    
    //Proportional, Integral, and Dervative constants.
    //These values will need to be tuned for your robot.
    private final double Kp = 0.1;
    private final double Ki = 0.001;
    private final double Kd = 0;
    
    //This must be fully initialized in the constructor, after the settings
    //for the encoders have been done.
    private final PIDController leftPID;
    private final PIDController rightPID;

    public PIDDrive() {
        //Sets the distance per pulse in inches.
        leftEncoder.setDistancePerPulse(-1);
        rightEncoder.setDistancePerPulse(-1);

        
        //Starts the encoders.
        leftEncoder.start();
        rightEncoder.start();
        
        //Sets the encoders to use distance for PID.
        //If this is not done, the robot may not go anywhere.
        //It is also possible to use rate, by changing kDistance to kRate.
        leftEncoder.setPIDSourceParameter(Encoder.PIDSourceParameter.kRate);
        rightEncoder.setPIDSourceParameter(Encoder.PIDSourceParameter.kRate);
        
        //Initializes the PID Controllers
        leftPID = new PIDController(Kp, Ki, Kd, leftEncoder, left);
        rightPID = new PIDController(Kp, Ki, Kd, rightEncoder, right);
        
        leftPID.setPercentTolerance(25);
        rightPID.setPercentTolerance(25);
        
        leftPID.setContinuous();
        rightPID.setContinuous();
        
        //Enables the PID Controllers.
        leftPID.enable();
        rightPID.enable();
        

    }

    public static void WriteDriverStationLine(DriverStationLCD.Line line, String text){
        DriverStationLCD.getInstance().println(line, 1, text);
        DriverStationLCD.getInstance().updateLCD();
    }
    /**
     * This function is called once each time the robot enters operator control.
     * Teleop commands are put in here
     */
    public void operatorControl() {
        leftPID.setSetpoint(1000);
        rightPID.setSetpoint(1000);
        
        while(isOperatorControl()){
            
            WriteDriverStationLine(DriverStationLCD.Line.kUser1, "E: " + leftPID.getError());
            WriteDriverStationLine(DriverStationLCD.Line.kUser2, "E2: " + rightPID.getError());
            WriteDriverStationLine(DriverStationLCD.Line.kUser3, "D: " + leftEncoder.getRate());
            WriteDriverStationLine(DriverStationLCD.Line.kUser4, "D2: " + rightEncoder.getRate());
        }

The wheels spin forward, then rapidly switch directions.

I would need to study the WPIJ PID class to see what its actually doing... to see if it clips the values, helps with integral windup and other control stuff to give you a complete answer... but off the bat, how did you get your gains?

Your setPoint is 1000 so your first iteration is 1000*0.1 = 100, because PWM only accepts +1 to -1 as inputs to the motor controllers.that instantly drives your motor to full power. I am not sure what the units of your 1000 but if your motor can spin faster than that, then your gains will cause your motor to oscillate... try reducing your gains.

[NotInControl]

Originally Posted by NotInControl

I would need to study the WPIJ PID class to see what its actually doing... to see if it clips the values, helps with integral windup and other control stuff to give you a complete answer... but off the bat, how did you get your gains?

Your setPoint is 1000 so your first iteration is 1000*0.1 = 100, because PWM only accepts +1 to -1 as inputs to the motor controllers.that instantly drives your motor to full power. I am not sure what the units of your 1000 but if your motor can spin faster than that, then your gains will cause your motor to oscillate... try reducing your gains.

Any update?

[pblankenbaker]

My understanding is that the standard PID controller is set up to apply motor speeds to reach a certain distance. In this scenario, the end value goes to 0 when the target is reached.

For a rate controlled system, the end value is not 0 (you don't want your motor power to go to 0 when you achieve the desired rate).

So, what we have been doing is trapping the pidWrite() method (if you extend PIDSource) or usePIDOutput() method (if you extend PIDSubSystem). We then use the value passed in by the PID control loop as a "adjustment" to the current motor power. For example:

    /**
     * Processes the computed PID output value to apply to our system.
     *
     * <p>It is important to remember that the actual PID controller that is
     * making computations and calling our method will be trying to reach a
     * output level of 0.0 when the target is reached. This works well when your
     * PID is used to move to a certain distance (you want your motors to stop
     * when you reach your target).</p>
     *
     * <p>However, our motors are driving a spinning wheel and we want to reach
     * a certain speed (the motor will not be 0.0 when the speed is reached). In
     * order to handle this condition we need to treat the new values provided
     * by the PID controller as ADJUSTMENTS to the current motor power
     * setting.</p>
     *
     * @param power The power level adjustment to apply to the motors.
     */
    protected void usePIDOutput(double power) {
        if (DEBUG) {
            SmartDashboard.putDouble("ShooterPIDAdjust", power);
        }

        // Apply adjustment provided by PID controller to current power output
        // level and make sure its in the range of [0, 1].
        double ratePower = power + leftMotor.get();
        if (ratePower > 1.0) {
            ratePower = 1.0;
        } else if (ratePower < 0) {
            ratePower = 0;
        }

        // Apply the new power level to reach the target rate
        applyPower(ratePower);
    }

Hope that helps.

[Ether]

2013 FRC WPILib (at least for Java) has added a feedforward term to the PIDController to help support using the library PID for speed control.

So this gives you other options.

Instead of integrating the PID output, you can set your feedforward gain so that the feedforward motor command corresponds roughly to what you'd expect is required for the speed you are commanding. Then tune the PID with I only (set P and D to zero) to get the steady-state accuracy.