Spark Max w/ NEO's, PID not getting to Setpoint

I am a complete neophyte when it comes to using anything more than set commands for motors (that’s what CSA’s are for!). At the moment, I am trying to figure out PID on a shooter using a pair of Spark Max’s w/ attached NEOs, direct to shaft, which is shooting power cells great using set commands, but is affected by battery voltage. We want to use PID to set an RPM so that it doesn’t matter what the voltage is, it will just get there.

If I run the motors up using just set commands, they will happily spin along at 5900 rpm when given a set(1). If I attempt the same thing using PID, the REV Spark Max Velocity Control example in particular with very little alteration other than there are 2 motors involved, the motors will go up to ~1820 rpm and stay there when given the same speed command of 1 and max rpm of 5700.

This is completely test code on my personal machine, so no Github, but here is the short code for just testing running the shooter, both with discrete set commands as well as PID. The goal is quite simple. If a shoulder button is pressed, either the left or right motor will turn individually to whatever shooterspeed is discretely set to (a slider on the dashboard that goes from 0 to 1 since we don’t want to run backwards) to visually verify that both motors are going to turn the same direction and not fight each other. If Button 3 is pressed, both motors spin at the same discrete shooterspeed set command. If Button 4 is pressed, it should take that same shooterspeed entry, multiply it by the max rpm variable (defaulted to 5700 and hasn’t been changed during testing), and feed that into the PID command as a target velocity.

Currently using the discrete set commands results in the expected velocity from 0 to 5700/5900 rpm depending on running only 1 or both motors. Using PID, with the numbers given in the REV example, is producing both motors spinning at 1820 rpm. Trying to slow the shooter wheel down does indeed cause the Spark Max’s to feed “moar power” to speed it up, so PID is working somewhat, just not to the 5700 that I believe it ‘should’ be doing when given a shooterspeed of 1.

My understanding of PID, at least what I can understand when reading about it, is that the P-I-D variables affect how the setpoint is achieved, but that it will achieve it eventually. It may be jerky or slow to get there, but it will. Is that not correct? Keep in mind that the below PID values are directly from the REV Velocity Closed-Loop example, with no tuning yet since I can’t get it to get to full speed yet.

Is there something glaring that I’m missing here?

Thanks for looking!

package frc.robot;

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

import edu.wpi.first.wpilibj.Joystick;
import edu.wpi.first.wpilibj.PowerDistributionPanel;
import edu.wpi.first.wpilibj.TimedRobot;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;


public class Robot extends TimedRobot {

  /* Joysticks */
  Joystick gamepad = new Joystick(0);

  /* Power Distribution Panel */
  PowerDistributionPanel pdp = new PowerDistributionPanel();// add pdp functions

  /* Variables */
  int shooterrightid = 12;
  int shooterleftid = 13;
  double shooterspeed = 0;
  public double kP, kI, kD, kIz, kFF, kMaxOutput, kMinOutput, maxRPM;

  /* Shooter Motors */
  CANSparkMax shooterright = new CANSparkMax(shooterrightid, MotorType.kBrushless);
  CANSparkMax shooterleft = new CANSparkMax(shooterleftid, MotorType.kBrushless);

  /* Shooter Encoders */
  CANEncoder shooterrightencoder = new CANEncoder(shooterright);
  CANEncoder shooterleftencoder = new CANEncoder(shooterleft);

  /* Shooter PID */
  CANPIDController shooterrightPID = new CANPIDController(shooterright);
  CANPIDController shooterleftPID = new CANPIDController(shooterleft);

  @Override
  public void robotInit() {
    /* Set up shooter for 2 motors, one on each side, individually no followers currently */
    shooterright.restoreFactoryDefaults();
    shooterleft.restoreFactoryDefaults();
    shooterright.setInverted(false);
    shooterleft.setInverted(true);

    // PID coefficients
    kP = 6e-5; 
    kI = 0;
    kD = 0; 
    kIz = 0; 
    kFF = 0.000015; 
    kMaxOutput = 1; 
    kMinOutput = 0;
    maxRPM = 5700;

    // set PID coefficients
    shooterrightPID.setP(kP);
    shooterrightPID.setI(kI);
    shooterrightPID.setD(kD);
    shooterrightPID.setIZone(kIz);
    shooterrightPID.setFF(kFF);
    shooterrightPID.setOutputRange(kMinOutput, kMaxOutput);

    shooterleftPID.setP(kP);
    shooterleftPID.setI(kI);
    shooterleftPID.setD(kD);
    shooterleftPID.setIZone(kIz);
    shooterleftPID.setFF(kFF);
    shooterleftPID.setOutputRange(kMinOutput, kMaxOutput);

    // display PID coefficients on SmartDashboard
    SmartDashboard.putNumber("P Gain", kP);
    SmartDashboard.putNumber("I Gain", kI);
    SmartDashboard.putNumber("D Gain", kD);
    SmartDashboard.putNumber("I Zone", kIz);
    SmartDashboard.putNumber("Feed Forward", kFF);
    SmartDashboard.putNumber("Max Output", kMaxOutput);
    SmartDashboard.putNumber("Min Output", kMinOutput);

    SmartDashboard.putNumber("Shooter Speed", 0);
    
  }
  
  @Override
  public void robotPeriodic() {
    SmartDashboard.putNumber("Total", pdp.getCurrent(0) + pdp.getCurrent(1) + pdp.getCurrent(2) + pdp.getCurrent(3) + pdp.getCurrent(4) + pdp.getCurrent(5) + pdp.getCurrent(6) + pdp.getCurrent(7) + pdp.getCurrent(8) + pdp.getCurrent(9) + pdp.getCurrent(10) + pdp.getCurrent(11) + pdp.getCurrent(12) + pdp.getCurrent(13) + pdp.getCurrent(14) + pdp.getCurrent(15));
    SmartDashboard.putNumber("Right RPM", shooterrightencoder.getVelocity());
    SmartDashboard.putNumber("Left RPM", shooterrightencoder.getVelocity());
    shooterspeed = SmartDashboard.getNumber("Shooter Speed", 0);

    // read PID coefficients from SmartDashboard
    double p = SmartDashboard.getNumber("P Gain", 0);
    double i = SmartDashboard.getNumber("I Gain", 0);
    double d = SmartDashboard.getNumber("D Gain", 0);
    double iz = SmartDashboard.getNumber("I Zone", 0);
    double ff = SmartDashboard.getNumber("Feed Forward", 0);
    double max = SmartDashboard.getNumber("Max Output", 0);
    double min = SmartDashboard.getNumber("Min Output", 0);

    // if PID coefficients on SmartDashboard have changed, write new values to controller
    if((p != kP)) { shooterrightPID.setP(p); shooterleftPID.setP(p); kP = p; }
    if((i != kI)) { shooterrightPID.setI(i); shooterleftPID.setI(i); kI = i; }
    if((d != kD)) { shooterrightPID.setD(d); shooterleftPID.setD(d); kD = d; }
    if((iz != kIz)) { shooterrightPID.setIZone(iz); shooterleftPID.setIZone(iz); kIz = iz; }
    if((ff != kFF)) { shooterrightPID.setFF(ff); shooterleftPID.setFF(ff); kFF = ff; }
    if((max != kMaxOutput) || (min != kMinOutput)) { 
      shooterrightPID.setOutputRange(min, max); 
      shooterleftPID.setOutputRange(min, max); 
      kMinOutput = min; kMaxOutput = max; 
    }

  }

  @Override
  public void autonomousInit() {
  }
  
  @Override
  public void autonomousPeriodic() {
  }

  @Override    
  public void teleopInit() {
  }

  @Override
  public void teleopPeriodic() {
    if (gamepad.getRawButton(6)) {
      shooterright.set(shooterspeed);
    }
    else if (gamepad.getRawButton(5)) {
      shooterleft.set(shooterspeed);
    }
    else if (gamepad.getRawButton(3)) {
      shooterright.set(shooterspeed);
      shooterleft.set(shooterspeed);
    }
    else if (gamepad.getRawButton(4)) {
      double setPoint = shooterspeed*maxRPM;
      shooterrightPID.setReference(setPoint, ControlType.kVelocity);
      shooterleftPID.setReference(setPoint, ControlType.kVelocity);
      SmartDashboard.putNumber("SetPoint", setPoint);
    }
    else {
      shooterright.set(0);
      shooterleft.set(0);
    }


  }
  
  @Override
  public void disabledInit() {
  }

}

My understanding of PID, at least what I can understand when reading about it, is that the P-I-D variables affect how the setpoint is achieved, but that it will achieve it eventually. It may be jerky or slow to get there, but it will. Is that not correct? Keep in mind that the below PID values are directly from the REV Velocity Closed-Loop example, with no tuning yet since I can’t get it to get to full speed yet.

if your constants are tuned improperly they could prevent you from reaching your setpoint at all–if your P constant is too low, for example, your controller will never supply enough voltage to actually make a difference. too high could cause massive overshoot.

since constants are usually tuned for the system, it’s possible (and likely) that the constants used by rev in example won’t work for your shooter
if the given p constant is too low, it would also explain why your shooter is stuck below your setpoint
if you haven’t already, i would try increasing kP and see if there’s any change

note: your code seems fine at a glance, but i’m not familiar with the rev control libraries so take my input with a grain of salt~

Your PID constants are too small. An F of 0.000015 * 5700 RPM = 0.0855 motor output. A P of 0.00006 * (5700-1800) = .234 motor output for a total output of .32. That’s reasonable for 1800 RPM.

Since you know that at 1 output the RPM is 5700, set F to 1/5700. Then you can tune P from there.

A PID with I will get there eventually, but with just P,D, and F it won’t get there if the PID constants are too low.

I also wouldn’t run two separate PIDs for each motor. If one of them misses encoder pulses the motors could start fighting each other. I would set one as a follower.

In addition, you don’t want to try to shoot at 100% because that gives no headroom for the motors to spin up after a shot. I’d try to design the mechanical part so you can hit the shot at 80% power, so that after the first ball goes through the PID can run at 100% to get the speed back up. The more excess mechanical power you have the faster it will spin up and the more inertia you have the less it will slow down.

5 Likes

Thanks Joe! & Built!

I think I can follow that explanation. We are planning on using 3600 rpm as our initial setpoint, tuning at the competition (is it happens now) with worn balls. We only have the single one from the KOP, not getting any from First Choice, and probably have sent it through our shooter at least 1000 times with no wear on it (I recorded 250 just yesterday and they’ve been doing test sessions for almost 2 weeks now). We were discouraged when it topped out at just over 1800rpm, and that derailed us. I initially tried making one a follower but couldn’t get compilable (didn’t think that was a word) code and planned to revisit it just for the reason you brought up.

I now have something to play with when I get access to the bot tomorrow.

1 Like

This topic was automatically closed 365 days after the last reply. New replies are no longer allowed.