Merge pull request #59 from Spectrum3847/54-update-vision-files

54 update vision files, merging vision in and we can fix from here

Author: SpectrumFRC3847

.vscode/launch.json

@@ -4,18 +4,31 @@
   // For more information, visit: https://go.microsoft.com/fwlink/?linkid=830387
   "version": "0.2.0",
   "configurations": [
-
+    {
+      "type": "java",
+      "name": "Main",
+      "request": "launch",
+      "mainClass": "frc.robot.Main",
+      "projectName": "2025-SpectrumBeta"
+    },
+    {
+      "type": "java",
+      "name": "Main",
+      "request": "launch",
+      "mainClass": "frc.robot.Main",
+      "projectName": "2025-Spectrum"
+    },
     {
       "type": "wpilib",
       "name": "WPILib Desktop Debug",
       "request": "launch",
-      "desktop": true,
+      "desktop": true
     },
     {
       "type": "wpilib",
       "name": "WPILib roboRIO Debug",
       "request": "launch",
-      "desktop": false,
+      "desktop": false
     }
   ]
 }

.vscode/spellright.dict

@@ -142,3 +142,6 @@ eToSratio
 Ncoder
 Dpad
 Limelights
+Robotand
+vx
+vy

src/main/java/frc/reefscape/Field.java

@@ -66,40 +66,42 @@ public static class Reef {
         public static final double faceToZoneLine =
                 Units.inchesToMeters(12); // Side of the reef to the inside of the reef zone line
 
+        @Getter
         public static final Pose2d[] centerFaces =
                 new Pose2d[6]; // Starting facing the driver station in clockwise order
+
         public static final List<Map<ReefHeight, Pose3d>> branchPositions =
                 new ArrayList<>(); // Starting at the right branch facing the driver station in
         // clockwise
 
         static {
             // Initialize faces
-            centerFaces[0] =
+            centerFaces[0] = // reef id 18
                     new Pose2d(
                             Units.inchesToMeters(144.003),
                             Units.inchesToMeters(158.500),
                             Rotation2d.fromDegrees(180));
-            centerFaces[1] =
+            centerFaces[1] = // reef id 19
                     new Pose2d(
                             Units.inchesToMeters(160.373),
                             Units.inchesToMeters(186.857),
                             Rotation2d.fromDegrees(120));
-            centerFaces[2] =
+            centerFaces[2] = // reef id 20
                     new Pose2d(
                             Units.inchesToMeters(193.116),
                             Units.inchesToMeters(186.858),
                             Rotation2d.fromDegrees(60));
-            centerFaces[3] =
+            centerFaces[3] = // reef id 21
                     new Pose2d(
                             Units.inchesToMeters(209.489),
                             Units.inchesToMeters(158.502),
                             Rotation2d.fromDegrees(0));
-            centerFaces[4] =
+            centerFaces[4] = // reef id 22
                     new Pose2d(
                             Units.inchesToMeters(193.118),
                             Units.inchesToMeters(130.145),
                             Rotation2d.fromDegrees(-60));
-            centerFaces[5] =
+            centerFaces[5] = // reef id 17
                     new Pose2d(
                             Units.inchesToMeters(160.375),
                             Units.inchesToMeters(130.144),

src/main/java/frc/robot/pilot/Pilot.java

@@ -40,6 +40,8 @@ public class Pilot extends Gamepad {
     public final Trigger actionReady = rightBumper.and(teleop);
     public final Trigger score = actionReady.not().and(teleop);
 
+    // vision Drive
+    public final Trigger visionAim = Y.and(noFn, teleop);
     // Drive Triggers
     public final Trigger upReorient = upDpad.and(fn, teleop);
     public final Trigger leftReorient = leftDpad.and(fn, teleop);

src/main/java/frc/robot/swerve/Swerve.java

@@ -223,7 +223,7 @@ Command applyRequest(Supplier<SwerveRequest> requestSupplier) {
         return run(() -> this.setControl(requestSupplier.get())).ignoringDisable(true);
     }
 
-    private ChassisSpeeds getCurrentRobotChassisSpeeds() {
+    public ChassisSpeeds getCurrentRobotChassisSpeeds() {
         return getKinematics().toChassisSpeeds(getState().ModuleStates);
     }
 

src/main/java/frc/robot/swerve/SwerveStates.java

@@ -55,6 +55,9 @@ protected static void setStates() {
         pilot.leftReorient.onTrue(log(reorientLeft()));
         pilot.downReorient.onTrue(log(reorientBack()));
         pilot.rightReorient.onTrue(log(reorientRight()));
+
+        // // vision aim
+        pilot.visionAim.onTrue(log(visionReefDrive()));
     }
 
     /** Pilot Commands ************************************************************************ */
@@ -88,6 +91,14 @@ protected static Command fpvDrive() {
                 .withName("Swerve.PilotFPVDrive");
     }
 
+    protected static Command visionReefDrive() {
+        return drive(
+                        pilot::getDriveFwdPositive,
+                        pilot::getDriveLeftPositive,
+                        () -> Robot.getVision().getAdjustedThetaToReefFace())
+                .withName("Swerve.VisionDrive");
+    }
+
     protected static Command pilotAimDrive(DoubleSupplier targetDegrees) {
         return aimDrive(pilot::getDriveFwdPositive, pilot::getDriveLeftPositive, targetDegrees)
                 .withName("Swerve.PilotAimDrive");