fix LL locations

FRC2713 · Mar 15, 2024 · 9fe50f8 · 9fe50f8
1 parent bb36e8a
commit 9fe50f8
Show file tree

Hide file tree

Showing 2 changed files with 9 additions and 18 deletions.
diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java
@@ -68,27 +68,27 @@ public Matrix<N3, N1> toMatrix() {
             .ntTableName("limelight-a")
             .location(
                 new Transform3d(
-                    Units.inchesToMeters(10.072374),
+                    Units.inchesToMeters(-10.072374),
                     Units.inchesToMeters(9.304687),
                     Units.inchesToMeters(24.149765),
                     new Rotation3d(
                         0,
                         Units.degreesToRadians(-14.209976),
-                        Units.degreesToRadians(180 - 160.800285))))
+                        Units.degreesToRadians(180 - 71.409653 + 90))))
             .mountingDirection(MountingDirection.HORIZONTAL_LL3)
             .build();
     public static VisionInfo RIGHT_LIMELIGHT_INFO =
         VisionInfo.builder()
             .ntTableName("limelight-b")
             .location(
                 new Transform3d(
-                    Units.inchesToMeters(-10.012360),
-                    Units.inchesToMeters(9.445222),
+                    Units.inchesToMeters(-10.062783),
+                    Units.inchesToMeters(-9.304855),
                     Units.inchesToMeters(24.185351),
                     new Rotation3d(
                         0,
                         Units.degreesToRadians(-14.209976),
-                        Units.degreesToRadians(-(180 - 160.800285)))))
+                        Units.degreesToRadians(180 - 23.725656))))
             .mountingDirection(MountingDirection.HORIZONTAL_LL3)
             .build();
   }
@@ -219,7 +219,7 @@ public static final class ShooterConstants {
   @UtilityClass
   public static final class DriveConstants {
 
-    public static final double K_JOYSTICK_TURN_DEADZONE = 0.04;
+    public static final double K_JOYSTICK_TURN_DEADZONE = 0.08;
     public static final double WHEEL_DIAMETER = 3.9; // 4.02267; // 3.85;
     public static final double GEAR_RATIO = 6.12;
     public static final double DIST_PER_PULSE =

diff --git a/src/main/java/frc/robot/subsystems/visionIO/VisionIOSim.java b/src/main/java/frc/robot/subsystems/visionIO/VisionIOSim.java
@@ -5,7 +5,6 @@
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Pose3d;
 import edu.wpi.first.math.geometry.Rotation2d;
-import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import frc.robot.Robot;
 import lombok.SneakyThrows;
 import org.photonvision.PhotonCamera;
@@ -45,7 +44,6 @@ public VisionIOSim(VisionInfo info) {
     cameraSim.enableProcessedStream(true);
     cameraSim.enableRawStream(true);
     visionSim.addCamera(cameraSim, this.info.getLocation());
-    SmartDashboard.putData(visionSim.getDebugField());
   }
 
   public VisionInfo getName() {
@@ -55,10 +53,6 @@ public VisionInfo getName() {
   @Override
   public void updateInputs(VisionInputs inputs) {
     visionSim.update(Robot.swerveDrive.getWheelPose());
-    visionSim
-        .getDebugField()
-        .getObject("VisionEstimation")
-        .setPose(Robot.swerveDrive.getWheelPose());
 
     var res = cameraHw.getLatestResult();
 
@@ -71,7 +65,7 @@ public void updateInputs(VisionInputs inputs) {
           PhotonUtils.estimateFieldToRobotAprilTag(
               target.getBestCameraToTarget(),
               layout.getTagPose(target.getFiducialId()).get(),
-              this.info.getLocation());
+              this.info.getLocation().inverse());
 
       inputs.botPoseBlue = robotPose;
       inputs.botPoseBlueTimestamp = imageCaptureTime;
@@ -86,7 +80,7 @@ public void updateInputs(VisionInputs inputs) {
       inputs.tagId = res.getBestTarget().getFiducialId();
 
       inputs.horizontalOffsetFromTarget = target.getYaw();
-      inputs.verticalOffsetFromTarget = target.getPitch();
+      inputs.verticalOffsetFromTarget = -target.getPitch();
 
     } else {
       inputs.botPoseBlue = new Pose3d();
@@ -102,10 +96,7 @@ public void updateInputs(VisionInputs inputs) {
   }
 
   @Override
-  public void setLEDMode(LEDMode mode) {
-    // TODO Auto-generated method stub
-    throw new UnsupportedOperationException("Unimplemented method 'setLEDMode'");
-  }
+  public void setLEDMode(LEDMode mode) {}
 
   @Override
   public void setCameraMode(CameraMode mode) {