PID control for a NEO drive train

Technical Programming
1

Hello CD!

I’m writing the PID control for our 2022 competition robot and because I’ve never really touched PID control before I was wondering if someone could let me know whether I’m on the right track:

package frc.robot.subsystems;

import com.revrobotics.CANEncoder;
import com.revrobotics.CANPIDController;
import com.revrobotics.CANSparkMax;
import com.revrobotics.CANSparkMaxLowLevel;
import com.revrobotics.ControlType;
import com.revrobotics.EncoderType;

import edu.wpi.first.wpilibj.ADXRS450_Gyro;
import edu.wpi.first.wpilibj.drive.DifferentialDrive;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import frc.robot.Constants;

public class DriveTrain extends SubsystemBase {

    CANSparkMax leftLeader = new CANSparkMax(1, CANSparkMaxLowLevel.MotorType.kBrushless);
    CANSparkMax rightLeader = new CANSparkMax(3, CANSparkMaxLowLevel.MotorType.kBrushless);
    
    CANSparkMax leftFollower = new CANSparkMax(2, CANSparkMaxLowLevel.MotorType.kBrushless);
    CANSparkMax rightFollower = new CANSparkMax(4, CANSparkMaxLowLevel.MotorType.kBrushless);

    CANEncoder leftEncoder = leftLeader.getEncoder(EncoderType.kHallSensor, 4096);
    CANEncoder rightEncoder = rightLeader.getEncoder(EncoderType.kHallSensor, 4096);

    CANPIDController leftController = leftLeader.getPIDController();
    CANPIDController rightController = rightLeader.getPIDController();

    // Gains
    double kP = 0;
    double kI = 0;
    double kD = 0;
    double kF = 0;
    double kIZone = 0;
    
    ADXRS450_Gyro gyro = new ADXRS450_Gyro();

    DifferentialDrive drive = new DifferentialDrive(leftLeader, rightLeader);

    public DriveTrain() {
        leftFollower.follow(leftLeader, true);
        rightFollower.follow(rightLeader, true);

        leftEncoder.setPositionConversionFactor(Constants.PULSE_PER_REVOLUTION);
        rightEncoder.setPositionConversionFactor(Constants.PULSE_PER_REVOLUTION);

        leftController.setP(kP);
        rightController.setP(kP);
        leftController.setI(kI);
        rightController.setI(kI);
        leftController.setD(kD);
        rightController.setD(kD);
        leftController.setFF(kF);
        rightController.setFF(kF);
        leftController.setIZone(kIZone);
        rightController.setIZone(kIZone);

        leftController.setOutputRange(-1, 1);
        rightController.setOutputRange(-1, 1);
    }

    public void drive(double f, double t) {
        drive.arcadeDrive(f, t);
    }

    public void PIDDrive(double distance) {
        leftController.setReference(distance, ControlType.kPosition);
        rightController.setReference(distance, ControlType.kPosition);
    }

    @Override
    public void periodic() {

    }
}
1 Like
2

I strongly recommend using an external encoder and doing PID control on the RIO rather than using the NEO’s integrated encoder for velocity control. The NEO’s built-in filtering settings add over 100ms of measurement delay to your velocity measurements, which renders feedback control much less effective.

Direct PID-to-position is not ideal for drivetrain control; you want a motion profile of some sort if you’re going to try to do position control.

3 Likes
3

We have some CTRE mag encoders, do you have any thoughts on using those?

4

Those should work fine, though you will need a breakout board (or else a custom splice) to attach them to the RIO iirc (the ribbon cables do not connect to the RIO’s PWM breakouts).

5

Thanks!!

1 Like
6

Could we use the ribbon cables to attach to the Spark Max 10 pin ports?

7

Yes, though you will need to consult the spec sheets to figure out which conductors connect to which pins. When using an external encoder with a brushless motor, I believe the limit switch pins are used for the encoder channels.

You’ll want to modify the default external filtering settings to reduce the amount of phase delay (the default filter is still pretty chonky, but at least with an external encoder the firmware lets you change it).

2 Likes
8

I appreciate it!

1 Like
9

Fair warning: the vendor APIs are a pain to navigate and you’ll probably run into units/sign errors several times before you’re done. If you need real-time help (forums aren’t great for large volumes of messages), pop by the FRC discord server - the programming channels there have a bunch of WPILib and vendor devs (including some from REV) who can help you.

2 Likes
10

Thank you both for this thread. I have two follow- up slightly related questions.

This year, we are using falcons for the first time. Is their internal encoder more reliable, and can a properly tuned feedforward help remedy the latency issue?

11

The default filter for the Falcon is the same as the default filter on all the CTRE motor controllers - it adds about 80ms of phase delay, but is reconfigurable. You should reconfigure it if you want to do effective velocity control.

Accurate feedforward reduces the amount of work the proportional controller has to do, but it won’t make your proportional control itself any more effective.

2 Likes
12

What am I “trading off” for by reducing this delay? There must be some negative to it, or else they wouldn’t have put it at 80ms.

1 Like
13

The usual goal for a low-pass filter is to reduce signal noise.

2 Likes
14

I wonder if it would be crazy to loop two phases of the Neo commutation encoder into the aux encoder input to get the feedback speed boost without additional encoder hardware? Probably only applicable to a shooter, also. You might have to do some digital work to get the three phase signal translated into simple quadrature… does that sound like a useful thing?

15

REV is already working on the issue on their end, I believe, and I’d expect them to have a fix before something like this could be worked to a reliable state.

16

I just tried, for the very first time to get a velocity PID working on a SparkMax and Neo and just really couldn’t get it to work.

Example, I was setting my setpoint to 3000 RPM. I increased my F-gain so the motor was running at about 2800 RPM with no PID values. I started my P increase but there wasn’t a time I wasn’t getting oscillations. Never could hit 3000. If I set P to a point of “acceptable” oscillations, increasing the D, didn’t make anything better. Never hit 3000 and oscillations returned.

Is this the behavior you’d expect given what you know about the SparkMax Neo setup?

17

The p gain that will be stable with the NEO’s built in filtering is very small. What you describe is consistent with that, it seems.

18

Thanks. One more question. I’ve had great success with CTRE/Talon SRX/Falcon velocity PID but it’s looking like my understanding is basic. You had talked about this default filter on the Falcons. Can you elaborate a little because I don’t know what configuration parameter you are talking about or what I would need to do reconfigure it.

19
1 Like
20

Dang, never saw that before in the docs. Thanks so much.

1 Like