PIDSubsystem getMeasurement() not being called?

I’m trying to get the new PIDSubsystem to run, following the docs on the PIDSubsystem example page. I do have the PIDSubsystem wrapped in a minimal Command class; maybe that’s the problem, but all the command is doing is calling enable() on the subsystem.

From the logging, it looks like the getMeasuremente() method isn’t being called. I can see logs for the method’s execute() and the PIDSubsystem’s useOutput() methods being called. We’re trying to center on the Limelight, so the intent was to use the Limelight’s xOffset = 0 as the setpoint. The useOutput() method is being called with 0 for both output and setpoint.

Also, FYI, we have basically the same logic working with an inline PIDCommand, so we know we’re getting data from the Limelight and the system in general is working.

Here’s the command:

public class LimelightCenterPID extends CommandBase {

  private final PIDDrivetrain drivetrain;

  NetworkTable limelight = NetworkTableInstance.getDefault().getTable("limelight");
  NetworkTableEntry xOffset = limelight.getEntry("tx");
  NetworkTableEntry yOffset = limelight.getEntry("ty");
  NetworkTableEntry targetArea = limelight.getEntry("ta");
  

  /**
   * Creates a new LimelightCenter.
   */
  public LimelightCenterPID(PIDDrivetrain drivetrain) {
    this.drivetrain = drivetrain;

    // Use addRequirements() here to declare subsystem dependencies.
    addRequirements(drivetrain);
  }

  // Called when the command is initially scheduled.
  @Override
  public void initialize() {
    drivetrain.enable();
  }

  // Called every time the scheduler runs while the command is scheduled.
  @Override
  public void execute() {
    System.out.println("LimelightCenterPID execute");
  }

  // Called once the command ends or is interrupted.
  @Override
  public void end(boolean interrupted) {
    drivetrain.disable();
  }

  // Returns true when the command should end.
  @Override
  public boolean isFinished() {
    return drivetrain.atSetpoint();
  }

}

Here’s the PIDSubsystem (only relevant bits)

public class PIDDrivetrain extends PIDSubsystem {

  public static final double Kp = 0.0225;
  public static final double Ki = 0.001;
  public static final double Kd = 0.00275;

  private WPI_TalonSRX frontRight;
  private WPI_TalonSRX middleRight;
  private WPI_TalonSRX rearRight;
  public WPI_TalonSRX frontLeft; // This is being used as the encoder
  private WPI_TalonSRX middleLeft;
  private WPI_TalonSRX rearLeft;

  NetworkTable limelight;

  /**
   * Creates a new Drivetrain.
   */
  public PIDDrivetrain() {

    super(new PIDController(Kp, Ki, Kd));

    frontRight = new WPI_TalonSRX(5);
    middleRight = new WPI_TalonSRX(8);
    rearRight = new WPI_TalonSRX(9);
    frontLeft = new WPI_TalonSRX(2);
    middleLeft = new WPI_TalonSRX(3);
    rearLeft = new WPI_TalonSRX(4);

    
    limelight = NetworkTableInstance.getDefault().getTable("limelight");

    setSetpoint(0.0);
  }

  public boolean atSetpoint() {
    return getController().atSetpoint();
  }

  private void log(String msg) {
    System.out.println(msg);
  }

  @Override
  protected double getMeasurement() {
    NetworkTableEntry xOffset = limelight.getEntry("tx");
    double xOffsetDouble = xOffset.getDouble(0.0);
    log("xOffset : " + xOffsetDouble);
    return xOffsetDouble;
  }

  @Override
  protected void useOutput(double output, double setpoint) {
    log("PIDSubsystem useOutput and setpoint : " + output + ", " + setpoint);
    drive(output, -output);
  }

  @Override
  public void periodic() {
    // This method will be called once per scheduler run 
 }

  public void drive(double leftPower, double rightPower) {
    frontLeft.set(ControlMode.PercentOutput, leftPower);
    middleLeft.set(ControlMode.PercentOutput, leftPower);
    rearLeft.set(ControlMode.PercentOutput, leftPower);
    frontRight.set(ControlMode.PercentOutput, -rightPower);
    middleRight.set(ControlMode.PercentOutput, -rightPower);
    rearRight.set(ControlMode.PercentOutput, -rightPower);
  }

}

Are these motors in the same gearbox? If so, might I suggest follower mode?

If useOutput is being called, it seems impossible that getMeasurement isn’t, unless you’ve overridden the periodic method without calling super. To wit:

  @Override
  public void periodic() {
    if (m_enabled) {
      useOutput(m_controller.calculate(getMeasurement(), m_setpoint), m_setpoint);
    }
  }

Even if they aren’t in the same gearbox, is there a case where you’d want the output of an entire side of the robot not being the same?

I would definitely use follower mode, that way for more advanced controls (like this PID) you only have one motor controller to deal with.

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