Merge branch 'main' into note-detection-fieldrelative

StuyPulse · Feb 10, 2024 · a2379df · a2379df
2 parents 34f6f8a + 0848677
commit a2379df
Show file tree

Hide file tree

Showing 4 changed files with 35 additions and 6 deletions.
diff --git a/src/main/java/com/stuypulse/robot/RobotContainer.java b/src/main/java/com/stuypulse/robot/RobotContainer.java
@@ -110,7 +110,11 @@ private void configureButtonBindings() {
             .whileTrue(new IntakeAcquire())
             .onFalse(new IntakeStop());
 
-        driver.getRightButton().whileTrue(new SwerveDriveDriveToChain());
+        // driver.getBottomButton().whileTrue(new SwerveDriveWithAiming(Field.getFiducial(8).getPose().toPose2d(), driver));
+        driver.getLeftButton().whileTrue(new SwerveDriveToScore());
+        driver.getRightButton().whileTrue(new SwerveDriveDriveToScore(driver));
+        // driver.getBottomButton().whileTrue(AutoBuilder.pathfindToPose(new Pose2d(), new PathConstraints(3, 4, Units.degreesToRadians(540), Units.degreesToRadians(720)), 0, 0));
+
     }
 
     /**************/

diff --git a/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveToAutoStart.java b/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveToAutoStart.java
@@ -88,5 +88,4 @@ public void end(boolean interupted) {
         swerve.stop();
         targetPose2d.setPose(Double.NaN, Double.NaN, new Rotation2d(Double.NaN));
     }
-
-}
+}
diff --git a/src/main/java/com/stuypulse/robot/constants/Motors.java b/src/main/java/com/stuypulse/robot/constants/Motors.java
@@ -61,7 +61,7 @@ public CANSparkMaxConfig(
         }
 
         public CANSparkMaxConfig(boolean inverted, IdleMode idleMode, int currentLimitAmps) {
-            this(inverted, idleMode, currentLimitAmps, 0.0);
+            this(inverted, idleMode, currentLimitAmps, 0.05);
         }
 
         public CANSparkMaxConfig(boolean inverted, IdleMode idleMode) {
@@ -95,7 +95,7 @@ public CANSparkFlexConfig(
         }
 
         public CANSparkFlexConfig(boolean inverted, IdleMode idleMode, int currentLimitAmps) {
-            this(inverted, idleMode, currentLimitAmps, 0.0);
+            this(inverted, idleMode, currentLimitAmps, 0.05);
         }
 
         public CANSparkFlexConfig(boolean inverted, IdleMode idleMode) {

diff --git a/src/main/java/com/stuypulse/robot/util/OV2311Camera.java b/src/main/java/com/stuypulse/robot/util/OV2311Camera.java
@@ -112,6 +112,28 @@ private Pose3d getRobotPose() {
             new Transform3d(offset.getTranslation(), offset.getRotation()).inverse());
     }
 
+    private boolean isValidData(VisionData data) {
+        for (long id : data.tids) {
+            boolean found = false;
+            for (Fiducial f : Field.FIDUCIALS) {
+                if (f.getID() == id) {
+                    found = true;
+                    break;
+                }
+            }
+
+            if (!found) {
+                return false;
+            }
+        }
+
+        if (Double.isNaN(data.robotPose.getX()) || data.robotPose.getX() < 0  || data.robotPose.getX() > Field.WIDTH) return false;
+        if (Double.isNaN(data.robotPose.getY()) || data.robotPose.getY() < 0  || data.robotPose.getY() > Field.HEIGHT) return false;
+        if (Double.isNaN(data.robotPose.getZ()) || data.robotPose.getZ() < -1 || data.robotPose.getZ() > 1) return false;
+
+        return true;
+    }
+
     public Optional<VisionData> getVisionData() {
         updateData();
 
@@ -123,7 +145,11 @@ public Optional<VisionData> getVisionData() {
 
         LogPose3d.logPose3d("Vision/Robot Pose", getRobotPose());
 
-        return Optional.of(new VisionData(rawIdData, offset, getRobotPose(), timestamp));
+        VisionData data = new VisionData(rawIdData, offset, getRobotPose(), timestamp);
+
+        if (!isValidData(data)) return Optional.empty();
+
+        return Optional.of(data);
     }
 
     public void setLayout(Fiducial[] layout) {
-Original file line number
+Diff line change
@@ Expand Up / @@ -88,5 +88,4 @@ public void end(boolean interupted) { @@
             swerve.stop();
             targetPose2d.setPose(Double.NaN, Double.NaN, new Rotation2d(Double.NaN));
         }
-    }
+    }