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