small things

DeepBlueRobotics · Jun 13, 2024 · 7d21c52 · 7d21c52
1 parent a51458e
commit 7d21c52
Show file tree

Hide file tree

Showing 3 changed files with 4 additions and 4 deletions.
diff --git a/src/main/java/org/carlmontrobotics/Constants.java b/src/main/java/org/carlmontrobotics/Constants.java
@@ -383,7 +383,7 @@ public static final class Limelightc {
 		public static final double STD_DEV_Y_METERS = 0.7; // uncertainty of 0.7 meters on the field
 		public static final int STD_DEV_HEADING_RADS = 9999999; // (gyro) heading standard deviation, set extremely high
 																// to represent unreliable heading
-		public static final int MAX_TRUSTED_ANG_VEL_DEGSPSEC = 720; // maximum trusted angular velocity
+		public static final int MAX_TRUSTED_ANG_VEL_DEG_PER_SEC = 720; // maximum trusted angular velocity
 
 		public static final double ERROR_TOLERANCE_RAD = 0.1; // unused
 		public static final double HORIZONTAL_FOV_DEG = 0; // unused

diff --git a/src/main/java/org/carlmontrobotics/commands/AlignToApriltagMegatag2.java b/src/main/java/org/carlmontrobotics/commands/AlignToApriltagMegatag2.java
@@ -31,7 +31,7 @@ public AlignToApriltagMegatag2(Drivetrain drivetrain, Limelight limelight) {
 
         rotationPID.enableContinuousInput(-180, 180);
         Rotation2d targetAngle = Rotation2d.fromDegrees(drivetrain.getHeading())
-                .plus(Rotation2d.fromDegrees(limelight.getRotateAngleRadMT2()));
+                .plus(Rotation2d.fromRadians(limelight.getRotateAngleRadMT2()));
         rotationPID.setSetpoint(MathUtil.inputModulus(targetAngle.getDegrees(), -180, 180));
         rotationPID.setTolerance(positionTolerance[2], velocityTolerance[2]);
         SendableRegistry.addChild(this, rotationPID);
@@ -41,7 +41,7 @@ public AlignToApriltagMegatag2(Drivetrain drivetrain, Limelight limelight) {
     @Override
     public void execute() {
         Rotation2d targetAngle = Rotation2d.fromDegrees(drivetrain.getHeading())
-                .plus(Rotation2d.fromDegrees(limelight.getRotateAngleRadMT2()));
+                .plus(Rotation2d.fromRadians(limelight.getRotateAngleRadMT2()));
         rotationPID.setSetpoint(MathUtil.inputModulus(targetAngle.getDegrees(), -180, 180));
         if (teleopDrive == null)
             drivetrain.drive(0, 0, rotationPID.calculate(drivetrain.getHeading()));

diff --git a/src/main/java/org/carlmontrobotics/subsystems/Limelight.java b/src/main/java/org/carlmontrobotics/subsystems/Limelight.java
@@ -125,7 +125,7 @@ public void updateMT2Odometry() {
     LimelightHelpers.PoseEstimate visionPoseEstimate = LimelightHelpers
         .getBotPoseEstimate_wpiBlue_MegaTag2(SHOOTER_LL_NAME);
 
-    if (Math.abs(drivetrain.getGyroRate()) > MAX_TRUSTED_ANG_VEL_DEGSPSEC) { // degrees per second
+    if (Math.abs(drivetrain.getGyroRate()) > MAX_TRUSTED_ANG_VEL_DEG_PER_SEC) { // degrees per second
       rejectVisionUpdate = true;
     }