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