Implement Vision

simgui-ds.json

@@ -91,7 +91,8 @@
   ],
   "robotJoysticks": [
     {
-      "guid": "030000005e040000120b000009050000"
+      "guid": "78696e70757401000000000000000000",
+      "useGamepad": true
     }
   ]
 }

src/main/java/org/team5924/frc2026/Constants.java

@@ -16,6 +16,12 @@
 
 package org.team5924.frc2026;
 
+import java.util.ArrayList;
+import java.util.List;
+
+import org.photonvision.simulation.SimCameraProperties;
+import org.team5924.frc2026.util.VisionUtil;
+
 import com.ctre.phoenix6.configs.CANdiConfiguration;
 import com.ctre.phoenix6.configs.ClosedLoopRampsConfigs;
 import com.ctre.phoenix6.configs.CurrentLimitsConfigs;
@@ -28,6 +34,11 @@
 import com.ctre.phoenix6.signals.NeutralModeValue;
 import com.ctre.phoenix6.signals.S1CloseStateValue;
 import com.ctre.phoenix6.signals.S2CloseStateValue;
+
+import edu.wpi.first.apriltag.AprilTagFieldLayout;
+import edu.wpi.first.apriltag.AprilTagFields;
+import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.math.util.Units;
 import edu.wpi.first.wpilibj.RobotBase;
 
 /**
@@ -230,6 +241,22 @@ public final class Indexer { //TODO: update these later
             .withInverted(InvertedValue.CounterClockwise_Positive)
             .withNeutralMode(NeutralModeValue.Brake));
   }
+
+  public static final class Field { // TODO: update all of these when new field released
+      public static final double FIELD_BORDER_MARGIN = 0.5;
+      public static final AprilTagFields FIELD_TYPE = AprilTagFields.k2025ReefscapeWelded; // TODO: update to new field
+      public static final AprilTagFieldLayout field = AprilTagFieldLayout.loadField(FIELD_TYPE);
+      public static final double FIELD_WIDTH = field.getFieldWidth();
+      public static final double FIELD_LENGTH = field.getFieldLength();
+
+      public static final double faceLength = Units.inchesToMeters(36.792600);
+      public static final double fieldWidth = field.getFieldWidth();
+      public static final Translation2d blueCenter =
+          new Translation2d(Units.inchesToMeters(176.745), Units.inchesToMeters(158.5));
+
+      public static final Translation2d redCenter =
+          new Translation2d(Units.inchesToMeters(514.13), Units.inchesToMeters(158.5));
+  }
 }
 
 

src/main/java/org/team5924/frc2026/RobotContainer.java

@@ -38,11 +38,18 @@
 import org.team5924.frc2026.subsystems.drive.ModuleIO;
 import org.team5924.frc2026.subsystems.drive.ModuleIOTalonFX;
 import org.team5924.frc2026.subsystems.drive.ModuleIOTalonFXSim;
+import org.team5924.frc2026.subsystems.vision.Vision;
+import org.team5924.frc2026.subsystems.vision.VisionConstants;
+import org.team5924.frc2026.subsystems.vision.VisionIO;
+import org.team5924.frc2026.subsystems.vision.VisionIOPhotonVision;
+import org.team5924.frc2026.subsystems.vision.VisionIOPhotonVisionSim;
 
 public class RobotContainer {
   // Subsystems
   private final Drive drive;
   private SwerveDriveSimulation driveSimulation = null;
+
+  private Vision vision;
   // private final ExampleSystem exampleSystem;
   // private final ExampleRoller exampleRoller;
 
@@ -66,6 +73,17 @@ public RobotContainer() {
                 new ModuleIOTalonFX(TunerConstants.BackLeft),
                 new ModuleIOTalonFX(TunerConstants.BackRight),
                 (pose) -> {});
+        vision =
+            new Vision(
+                drive::addVisionMeasurement,
+                // new VisionIOPhotonVision(
+                //     VisionConstants.FRONT_LEFT_NAME, VisionConstants.FRONT_LEFT_TRANSFORM),
+                new VisionIOPhotonVision(
+                    VisionConstants.FRONT_RIGHT_NAME, VisionConstants.FRONT_RIGHT_TRANSFORM),
+                new VisionIOPhotonVision(
+                    VisionConstants.BACK_LEFT_NAME, VisionConstants.BACK_LEFT_TRANSFORM),
+                new VisionIOPhotonVision(
+                    VisionConstants.BACK_RIGHT_NAME, VisionConstants.BACK_RIGHT_TRANSFORM));
         // exampleSystem = new ExampleSystem(new ExampleSystemIOTalonFX());
         // exampleRoller = new ExampleRoller(new ExampleRollerIOKrakenFOC());
         break;
@@ -83,13 +101,29 @@ public RobotContainer() {
                 new ModuleIOTalonFXSim(TunerConstants.BackLeft, driveSimulation.getModules()[2]),
                 new ModuleIOTalonFXSim(TunerConstants.BackRight, driveSimulation.getModules()[3]),
                 driveSimulation::setSimulationWorldPose);
-        // vision = new Vision(drive,
-        // new VisionIOPhotonVisionSim(
-        // camera0Name, robotToCamera0,
-        // driveSimulation::getSimulatedDriveTrainPose),
-        // new VisionIOPhotonVisionSim(
-        // camera0Name, robotToCamera0,
-        // driveSimulation::getSimulatedDriveTrainPose);
+        vision =
+        new Vision(
+            drive::addVisionMeasurement,
+            new VisionIOPhotonVisionSim(
+                VisionConstants.FRONT_LEFT_NAME,
+                VisionConstants.FRONT_LEFT_TRANSFORM,
+                VisionConstants.SIM_THRIFTYCAM_PROPERTIES,
+                drive::getPose),
+            new VisionIOPhotonVisionSim(
+                VisionConstants.FRONT_RIGHT_NAME,
+                VisionConstants.FRONT_RIGHT_TRANSFORM,
+                VisionConstants.SIM_THRIFTYCAM_PROPERTIES,
+                drive::getPose),
+            new VisionIOPhotonVisionSim(
+                VisionConstants.BACK_LEFT_NAME,
+                VisionConstants.BACK_LEFT_TRANSFORM,
+                VisionConstants.SIM_THRIFTYCAM_PROPERTIES,
+                drive::getPose),
+            new VisionIOPhotonVisionSim(
+                VisionConstants.BACK_RIGHT_NAME,
+                VisionConstants.BACK_RIGHT_TRANSFORM,
+                VisionConstants.SIM_THRIFTYCAM_PROPERTIES,
+                drive::getPose));
         // exampleSystem = new ExampleSystem(new ExampleSystemIOSim());
         // exampleRoller = new ExampleRoller(new ExampleRollerIOSim());
         break;
@@ -104,6 +138,13 @@ public RobotContainer() {
                 new ModuleIO() {},
                 new ModuleIO() {},
                 (pose) -> {});
+        vision =
+            new Vision(
+                drive::addVisionMeasurement,
+                new VisionIO() {},
+                new VisionIO() {},
+                new VisionIO() {},
+                new VisionIO() {});
         // exampleSystem = new ExampleSystem(new ExampleSystemIO() {});
         // exampleRoller = new ExampleRoller(new ExampleRollerIO() {});
         // vision = new Vision(drive, new VisionIO() {}, new VisionIO() {});
@@ -141,21 +182,24 @@ public RobotContainer() {
    */
   private void configureButtonBindings() {
     // Default command, normal field-relative drive
-    if (Constants.currentMode == Constants.Mode.SIM) {
-      drive.setDefaultCommand(
-          DriveCommands.joystickDrive(
-              drive,
-              () -> -driveController.getLeftY(),
-              () -> -driveController.getRawAxis(0),
-              () -> -driveController.getRawAxis(2)));
-    } else {
-      drive.setDefaultCommand(
-          DriveCommands.joystickDrive(
-              drive,
-              () -> -driveController.getLeftY(),
-              () -> -driveController.getLeftX(),
-              () -> -driveController.getRightX()));
-    }
+
+    // TODO: uncomment this out!
+    // if (Constants.currentMode == Constants.Mode.SIM) {
+    //   drive.setDefaultCommand(
+    //       DriveCommands.joystickDrive(
+    //           drive,
+    //           () -> -driveController.getLeftY(),
+    //           () -> -driveController.getRawAxis(0),
+    //           () -> -driveController.getRawAxis(2)));
+    // } else {
+    drive.setDefaultCommand(
+        DriveCommands.joystickDrive(
+            drive,
+            () -> -driveController.getLeftY(),
+            () -> -driveController.getLeftX(),
+            () -> -driveController.getRightX()));
+    // }
+
     // [driver] SLOW MODE YIPE
     driveController
         .y()

src/main/java/org/team5924/frc2026/RobotState.java

@@ -20,7 +20,6 @@
 import edu.wpi.first.math.geometry.Rotation2d;
 import lombok.Getter;
 import lombok.Setter;
-import org.littletonrobotics.junction.AutoLogOutput;
 import org.team5924.frc2026.subsystems.exampleSystem.ExampleSystem.ExampleSystemState;
 import org.team5924.frc2026.subsystems.rollers.exampleRoller.ExampleRoller.ExampleRollerState;
 import org.team5924.frc2026.subsystems.rollers.hopperAgitator.Hopper.HopperState;
@@ -40,10 +39,8 @@ public static RobotState getInstance() {
   }
 
   // Pose Estimation Members
-  @AutoLogOutput(key = "RobotState/OdometryPose")
-  @Getter
-  @Setter
-  private Pose2d odometryPose = new Pose2d();
+  // @AutoLogOutput(key = "RobotState/OdometryPose")
+  @Getter @Setter private Pose2d odometryPose = new Pose2d();
 
   @Getter @Setter private Rotation2d yawPosition = new Rotation2d();
   @Getter @Setter private double yawVelocityRadPerSec = 0.0;

src/main/java/org/team5924/frc2026/subsystems/drive/Drive.java

@@ -304,6 +304,7 @@ public void periodic() {
 
     // Update RobotState
     RobotState.getInstance().setOdometryPose(getPose());
+    Logger.recordOutput("RobotState/OdometryPose", getPose());
 
     // prevents error spam
     if (!gyroInputs.connected && wasGyroConnected) {

src/main/java/org/team5924/frc2026/subsystems/rollers/indexer/IndexerIOTalonFX.java

@@ -24,14 +24,14 @@ public class IndexerIOTalonFX extends GenericRollerSystemIOKrakenFOC<IndexerIOIn
 
   // private final DigitalInput beamBreak = new DigitalInput(Constants.Indexer.BEAM_BREAK_ID);
   private class IndexerInverseTalonFX extends GenericRollerSystemIOKrakenFOC<IndexerIOInputs> {
-    public IndexerInverseTalonFX () {
+    public IndexerInverseTalonFX() {
       super(
-        Constants.Indexer.CAN_ID_INVERSE,
-        Constants.Indexer.BUS,
-        Constants.Indexer.CONFIG,
-        Constants.Indexer.REDUCTION_INVERSE);
+          Constants.Indexer.CAN_ID_INVERSE,
+          Constants.Indexer.BUS,
+          Constants.Indexer.CONFIG,
+          Constants.Indexer.REDUCTION_INVERSE);
     }
-  } 
+  }
 
   // This is the other motor on indexer, the one that pushes up balls to shooter
   private final IndexerInverseTalonFX indexerInverse;
@@ -54,7 +54,6 @@ public void updateInputs(IndexerIOInputs inputs) {
   @Override
   public void runVolts(double volts) {
     super.runVolts(volts);
-    indexerInverse.runVolts(-1*volts);
+    indexerInverse.runVolts(-1 * volts);
   }
-
 }