Updated LimelightHelper, added CamPose, deleted VisionSubsystem

Author: ProgrammingSR

src/main/java/frc/robot/Constants.java

@@ -9,6 +9,7 @@
 import edu.wpi.first.math.geometry.Pose3d;
 import edu.wpi.first.math.geometry.Rotation3d;
 import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.math.geometry.Translation3d;
 import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
 import edu.wpi.first.math.numbers.N3;
 
@@ -77,7 +78,7 @@ public static final class DriveConstants {
     public static final double kDrivingGearRatio = 8.14; // SDS MK4i's in L1 configuration
 
     // TODO: Tune this PID before running on a robot on the ground
-    public static final double kPModuleTurningController = -0.3;
+    public static final double kPModuleTurningController = 0.3;
 
     public static final SwerveDriveKinematics kDriveKinematics = new SwerveDriveKinematics(
         new Translation2d(kWheelBase / 2, kTrackWidth / 2),
@@ -98,23 +99,20 @@ public static final class DriveConstants {
 
   public static final class VisionConstants {
     // TODO: Update cam pose relative to center of bot
-    public static final Pose3d kCamPose = new Pose3d(0, 0, 0, new Rotation3d(0, 0, 0));
-    public static final double[] kLimelightCamPose = {
-        kCamPose.getX(),
-        kCamPose.getY(),
-        kCamPose.getZ(),
-        kCamPose.getRotation().getX(),
-        kCamPose.getRotation().getY(),
-        kCamPose.getRotation().getZ() };
+    public static final Pose3d kCamPos = new Pose3d(
+      new Translation3d(0.3048,0.254,0),
+      new Rotation3d(0,0,0)
+    );
+
+    public static final String kLimelightName = "limelight-sr";
+
+    // https://docs.limelightvision.io/docs/docs-limelight/pipeline-apriltag/apriltag-robot-localization-megatag2
+    public static final int kIMUMode = 0;
 
     // TODO: Experiment with different std devs in the pose estimator
     public static final Vector<N3> kOdometrySTDDevs = VecBuilder.fill(0.1, 0.1, 0.1);
     public static final Vector<N3> kVisionSTDDevs = VecBuilder.fill(0.5, 0.5, 999999);
 
-    // Field size in meters
-    public static final double kFieldWidth = 8.21055;
-    public static final double kFieldLength = 16.54175;
-
     public static final boolean kUseVision = true;
   }
 

src/main/java/frc/robot/subsystems/DriveSubsystem.java

@@ -81,8 +81,21 @@ public DriveSubsystem() {
     m_headingCorrectionPID.enableContinuousInput(-Math.PI, Math.PI);
 
     // TODO: Name the Limelight "LL1899" and set the camera orientation
-    // TODO: Set a custom crop window for improved performance (the bot only needs to see april tags on the reef)
+    // TODO: Set a custom crop window for improved performance (the bot only needs
+    // to see april tags on the reef)
     m_poseEstimator.setVisionMeasurementStdDevs(VecBuilder.fill(0.5, 0.5, 9999999));
+
+    if (VisionConstants.kUseVision && Robot.isReal()) {
+      LimelightHelpers.setCameraPose_RobotSpace(
+          VisionConstants.kLimelightName,
+          VisionConstants.kCamPos.getX(),
+          VisionConstants.kCamPos.getY(),
+          VisionConstants.kCamPos.getZ(),
+          VisionConstants.kCamPos.getRotation().getX(),
+          VisionConstants.kCamPos.getRotation().getY(),
+          VisionConstants.kCamPos.getRotation().getZ());
+      LimelightHelpers.SetIMUMode(VisionConstants.kLimelightName, VisionConstants.kIMUMode);
+    }
   }
 
   @Override
@@ -101,10 +114,11 @@ public void periodic() {
 
     if (VisionConstants.kUseVision && Robot.isReal()) {
       // Update LimeLight with current robot orientation
-      LimelightHelpers.SetRobotOrientation("LL1899", m_gyro.getAngle(), 0.0, 0.0, 0.0, 0.0, 0.0);
+      LimelightHelpers.SetRobotOrientation(VisionConstants.kLimelightName, m_gyro.getAngle(), 0.0, 0.0, 0.0, 0.0, 0.0);
 
       // Get the pose estimate
-      LimelightHelpers.PoseEstimate limelightMeasurement = LimelightHelpers.getBotPoseEstimate_wpiBlue_MegaTag2("LL1899");
+      LimelightHelpers.PoseEstimate limelightMeasurement = LimelightHelpers
+          .getBotPoseEstimate_wpiBlue_MegaTag2(VisionConstants.kLimelightName);
 
       // Add it to your pose estimator if it is a valid measurement
       if (limelightMeasurement != null && limelightMeasurement.tagCount != 0 && m_gyro.getRate() < 720) {
@@ -113,7 +127,6 @@ public void periodic() {
             limelightMeasurement.timestampSeconds);
       }
     }
-    
 
     m_field.setRobotPose(m_poseEstimator.getEstimatedPosition());
 
@@ -246,7 +259,8 @@ public void setModuleStates(SwerveModuleState[] desiredStates) {
     };
     SmartDashboard.putNumberArray("AdvantageScope Swerve Desired States", logData);
 
-    // Takes the integral of the rotation speed to find the current angle for the simulator
+    // Takes the integral of the rotation speed to find the current angle for the
+    // simulator
     m_gyroAngle += DriveConstants.kDriveKinematics.toChassisSpeeds(desiredStates).omegaRadiansPerSecond
         * Robot.kDefaultPeriod;
   }