Limelight程式整合到原程式的流程做

public Pose2d getPose() {

  return m_odometry.getPoseMeters();

}

public void getTurning() {

  SmartDashboard.putNumber("FL", modules[0].getTurningPosition());

  SmartDashboard.putNumber("FR", modules[1].getTurningPosition());

  SmartDashboard.putNumber("BL", modules[2].getTurningPosition());

  SmartDashboard.putNumber("BR", modules[3].getTurningPosition());

}

public void getVelocity() {

  SmartDashboard.putNumber("FL V", modules[0].getDriveVelocity());

  SmartDashboard.putNumber("FR V", modules[1].getDriveVelocity());

  SmartDashboard.putNumber("BL V", modules[2].getDriveVelocity());

  SmartDashboard.putNumber("BR V", modules[3].getDriveVelocity());

}

public void resetOdometry(Pose2d pose) {

  m_odometry.resetPosition(

    Rotation2d.fromDegrees(0),

    new SwerveModulePosition[] {

      modules[0].getPosition(),

      modules[1].getPosition(),

      modules[2].getPosition(),

      modules[3].getPosition()

    },

    pose);

}

public Rotation2d getRotation2d() {

  return gyro.getRotation2d();

}

public double getHeading() {

  return Rotation2d.fromDegrees(gyro.getAngle()).getDegrees();

}

public double getTurnRate() {

  return gyro.getRate() * (DriveConstants.kGyroReversed ? -1.0 : 1.0);

}

public void get() {

  m_poseEstimator.getEstimatedPosition();

}