Encoders, Mecanum, and PID

[MichaelB]

How do I set up a velocity PID to work with my existing Mecanum Drive code using the encoders (US Digital E4P-360-250-D-H-D-B) I have on all four wheels? I found some threads on it and understand it conceptually (I think), but actually implementing it leaves me puzzled.

[Ether]

Originally Posted by MichaelB

How do I set up a velocity PID to work with my existing Mecanum Drive code using the encoders (US Digital E4P-360-250-D-H-D-B) I have on all four wheels? I found some threads on it and understand it conceptually (I think), but actually implementing it leaves me puzzled.

Can you articulate in a bit more detail what part you do not understand?

[MichaelB]

Originally Posted by Ether

Can you articulate in a bit more detail what part you do not understand?

I read a post from you (that I can't find anymore) about estimating a maximum speed for the wheels and then scaling that to 1. What I don't know is how to control each wheel individually like that. I tried to get the mecanum code from the DriveTrain class (changed a bit because we're not using a gyro or CAN), but now I'm not sure how to set the proper speeds. Do I just change in mecanumDrive_Cartesian "leftFront.set(...)" to "leftFrontPID.setSetpoint(...)" and et cetera?

public class DriveTrain extends Subsystem {
    // Put methods for controlling this subsystem
    // here. Call these from Commands.
    RobotDrive driveTrain;
    Encoder leftFrontEncoder;
    Encoder leftBackEncoder;
    Encoder rightFrontEncoder;
    Encoder rightBackEncoder;
    
    PIDController leftFrontPID;
    PIDController leftBackPID;
    PIDController rightFrontPID;
    PIDController rightBackPID;
    
    private static final double Kp = 0.3;
    private static final double Ki = 0.0;
    private static final double Kd = 0.0;
    
    public static final int kMaxNumberOfMotors = 4;
    public static Jaguar leftFront;
    public static Jaguar leftBack;
    public static Jaguar rightFront;
    public static Jaguar rightBack;
    
    protected final int m_invertedMotors[] = {1,1,1,1};
    
    public DriveTrain() {
        //super("DriveTrain", Kp, Ki, Kd);
        
        driveTrain = new RobotDrive(RobotMap.fLeftFrontMotorPort, RobotMap.rLeftBackMotorPort, RobotMap.fRightFrontMotorPort, RobotMap.rRightBackMotorPort);
        driveTrain.setSafetyEnabled(false);
        leftFrontEncoder = new Encoder(RobotMap.frontLeftEncoderPort1, RobotMap.frontLeftEncoderPort2, false, Encoder.EncodingType.k2X);
        leftBackEncoder = new Encoder(RobotMap.backLeftEncoderPort1, RobotMap.backLeftEncoderPort2, true, Encoder.EncodingType.k2X);
        rightFrontEncoder = new Encoder(RobotMap.frontRightEncoderPort1, RobotMap.frontRightEncoderPort2, true, Encoder.EncodingType.k2X);
        rightBackEncoder = new Encoder(RobotMap.backRightEncoderPort1, RobotMap.backRightEncoderPort2, true, Encoder.EncodingType.k2X);
        leftFrontEncoder.setDistancePerPulse(2*3*3.14/360/12); //should be in inches/rev? might be 250 instead of 360
        leftBackEncoder.setDistancePerPulse(2*3*3.14/360/12);
        rightFrontEncoder.setDistancePerPulse(2*3*3.14/360/12);
        rightBackEncoder.setDistancePerPulse(2*3*3.14/360/12);
        
        leftFrontEncoder.setSamplesToAverage(100);
        leftBackEncoder.setSamplesToAverage(100);
        rightFrontEncoder.setSamplesToAverage(100);
        rightBackEncoder.setSamplesToAverage(100);
        leftFrontEncoder.start();
        leftFrontEncoder.reset();
        rightFrontEncoder.start();
        rightFrontEncoder.reset();
        leftBackEncoder.start();
        leftBackEncoder.reset();
        rightBackEncoder.start();
        rightBackEncoder.reset();
        
        leftFrontPID =  new PIDController(Kp, Ki, Kd, leftFrontEncoder, leftFront);
        rightFrontPID =  new PIDController(Kp, Ki, Kd, rightFrontEncoder, rightFront);
        leftBackPID =  new PIDController(Kp, Ki, Kd, leftBackEncoder, leftBack);
        rightBackPID =  new PIDController(Kp, Ki, Kd, rightBackEncoder, rightBack);
        
        leftFrontPID.enable();
        leftBackPID.enable();
        rightFrontPID.enable();
        rightBackPID.enable();
    }

    public void initDefaultCommand() {
        // Set the default command for a subsystem here.
        setDefaultCommand(new DriveWithJoystickArcade());
        //setDefaultCommand(new DriveWithJoystick());
        //setDefaultCommand(new DriveWithJoystickPolar());    Doesn't work, don't know why
    }
    
    public void driveWithJoystick(double x, double y, double twist) {
        driveTrain.mecanumDrive_Cartesian(x,twist,-y, 0);
        //System.out.println("X: " + x + "Y: " + y + "Twist: " + twist);
        //System.out.println("Left Front: " + leftFront.get()/360 + " Left Back: " + leftBack.get()/360 + " Right Front: " + rightFront.get()/360 + " Right Back: " + rightBack.get()/360);
        System.out.println("Left Front: " + leftFrontEncoder.getRate() + " Left Back: " + leftBackEncoder.getRate() + " Right Front: " + rightFrontEncoder.getRate() + " Right Back: " + rightBackEncoder.getRate());
    }
    
    public void driveWithJoystickPolar(double magnitude, double direction, double twist) {
        driveTrain.mecanumDrive_Polar(magnitude, direction, twist);
        System.out.println("Mag: " + magnitude + " Dir: " + direction + " Twist: " + twist);
    }
    
    public void drive(double x, double twist){
        driveTrain.arcadeDrive(x,twist);
    }
    
    public void mecanumDrive_Cartesian(double x, double y, double rotation)
    {
        double xIn = x;
        double yIn = y;
        // Negate y for the joystick.
        yIn = -yIn;

        double wheelSpeeds[] = new double[kMaxNumberOfMotors];
        wheelSpeeds[RobotMap.kFrontLeft_val] = xIn + yIn + rotation;
        wheelSpeeds[RobotMap.kFrontRight_val] = -xIn + yIn - rotation;
        wheelSpeeds[RobotMap.kRearLeft_val] = -xIn + yIn + rotation;
        wheelSpeeds[RobotMap.kRearRight_val] = xIn + yIn - rotation;

        normalize(wheelSpeeds);

        leftFront.set(wheelSpeeds[RobotMap.kFrontLeft_val] * m_invertedMotors[RobotMap.kFrontLeft_val] * RobotMap.maxOutput);
        rightFront.set(wheelSpeeds[RobotMap.kFrontRight_val] * m_invertedMotors[RobotMap.kFrontRight_val] * RobotMap.maxOutput);
        leftBack.set(wheelSpeeds[RobotMap.kRearLeft_val] * m_invertedMotors[RobotMap.kRearLeft_val] * RobotMap.maxOutput);
        rightBack.set(wheelSpeeds[RobotMap.kRearRight_val] * m_invertedMotors[RobotMap.kRearRight_val] * RobotMap.maxOutput);
    }
        
    protected static void normalize(double wheelSpeeds[]) {
        double maxMagnitude = Math.abs(wheelSpeeds[0]);
        int i;
        for (i=1; i<kMaxNumberOfMotors; i++) {
            double temp = Math.abs(wheelSpeeds[i]);
            if (maxMagnitude < temp) maxMagnitude = temp;
        }
        if (maxMagnitude > 1.0) {
            for (i=0; i<kMaxNumberOfMotors; i++) {
                wheelSpeeds[i] = wheelSpeeds[i] / maxMagnitude;
            }
        }
    }
}

[Ether]

Yes, you want to take the four wheel speeds that are computed from the driver commands, and use each one as a setpoint for the PID (or other controller) for the corresponding wheel. The encoder signal for each wheel serves as the process variable for that wheel.

As with all closed-loop controllers, you need to make sure that your setpoint and process variable are appropriately scaled and offset so they work together properly.

I'm not a Java guru so I'll leave the Java coding questions to someone else.

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

[Ether]

The wrapper class implements PIDOutput and treats each write as an adjustment to the current power setting instead of a direct setting

Here are 3 other ways to do it.

[MikeE]

Feedforward is implemented in the PIDController class you are using now, see WPILib screensteps

It's a direct scaling factor between setpoint units and and output variable which for a motor controller is [-1, 1]. So if your max speed is N in encoder units then set the feedforward term to 1/N.