@Override
protected void initialize() {
super.initialize();
if (saveGyroHeading != null) {
setpoint = saveGyroHeading.getGyroAngle();
}
pidController.setSetpoint(setpoint);
SmartDashboard.putNumber("Setpoint:", setpoint);
pidController.enable();
log.fine("THIS" + setpoint);
}
@Override
protected void end() {
super.end();
pidController.disable();
}
@Override
protected boolean isFinished() {
return pidController.onTarget();
}