Vision sim

ascopeAssets/Robot_Offseason/config.json

@@ -1,3 +1,159 @@
 {
-  "name": "offseason"
+  "name": "butch",
+  "sourceUrl": "https://cad.onshape.com/documents/8f873ac3a74d8fd06cda742a/w/88e5df6f1994d26987d1f315/e/eb5a3c451ca2580d6f4169f7",
+  "rotations": [
+    {
+      "axis": "x",
+      "degrees": 90
+    },
+    {
+      "axis": "z",
+      "degrees": 0
+    }
+  ],
+  "position": [
+    0,
+    0,
+    0.04
+  ],
+  "cameras": [
+    {
+      "name": "FrontCam",
+      "position": [
+        0.3,
+        0.0,
+        0.2
+      ],
+      "fov": 59.99999999999999,
+      "rotations": [
+        {
+          "axis": "y",
+          "degrees": -29.999999999999996
+        },
+        {
+          "axis": "z",
+          "degrees": 0.0
+        }
+      ],
+      "resolution": [
+        640,
+        480
+      ]
+    },
+    {
+      "name": "FrontLeftCam",
+      "position": [
+        0.25,
+        0.3,
+        0.2
+      ],
+      "fov": 59.99999999999999,
+      "rotations": [
+        {
+          "axis": "y",
+          "degrees": -30.000000000000004
+        },
+        {
+          "axis": "z",
+          "degrees": 29.999999999999996
+        }
+      ],
+      "resolution": [
+        640,
+        480
+      ]
+    },
+    {
+      "name": "FrontRightCam",
+      "position": [
+        0.25,
+        -0.3,
+        0.2
+      ],
+      "fov": 59.99999999999999,
+      "rotations": [
+        {
+          "axis": "y",
+          "degrees": -29.999999999999996
+        },
+        {
+          "axis": "z",
+          "degrees": -30.000000000000004
+        }
+      ],
+      "resolution": [
+        640,
+        480
+      ]
+    },
+    {
+      "name": "BackLeftCam",
+      "position": [
+        0.25,
+        0.05,
+        0.2
+      ],
+      "fov": 59.99999999999999,
+      "rotations": [
+        {
+          "axis": "y",
+          "degrees": -35.000000000000014
+        },
+        {
+          "axis": "z",
+          "degrees": 150.0
+        }
+      ],
+      "resolution": [
+        640,
+        480
+      ]
+    },
+    {
+      "name": "BackRightCam",
+      "position": [
+        0.25,
+        -0.05,
+        0.2
+      ],
+      "fov": 59.99999999999999,
+      "rotations": [
+        {
+          "axis": "y",
+          "degrees": -35.00000000000001
+        },
+        {
+          "axis": "z",
+          "degrees": -150.0
+        }
+      ],
+      "resolution": [
+        640,
+        480
+      ]
+    }
+  ],
+  "components": [
+    {
+      "zeroedRotations": [
+        {
+          "axis": "x",
+          "degrees": 180
+        },
+        {
+          "axis": "y",
+          "degrees": 60
+        },
+        {
+          "axis": "z",
+          "degrees": 0
+        }
+      ],
+      "zeroedPosition": [
+        0.25,
+        0,
+        -0.02
+      ]
+    }
+  ]
 }

src/main/java/frc/robot/BuildConstants.java

@@ -0,0 +1,17 @@
+package frc.robot;
+
+/** Automatically generated file containing build version information. */
+public final class BuildConstants {
+  public static final String MAVEN_GROUP = "";
+  public static final String MAVEN_NAME = "offseason-robot-code-2024-1";
+  public static final String VERSION = "unspecified";
+  public static final int GIT_REVISION = 534;
+  public static final String GIT_SHA = "bf661f9f8d1233875f154280c54da2e4c3473fed";
+  public static final String GIT_DATE = "2025-01-03 21:20:40 EST";
+  public static final String GIT_BRANCH = "vision-sim";
+  public static final String BUILD_DATE = "2025-01-04 02:29:02 EST";
+  public static final long BUILD_UNIX_TIME = 1735975742416L;
+  public static final int DIRTY = 1;
+
+  private BuildConstants() {}
+}

src/main/java/frc/robot/Constants.java

@@ -6,15 +6,15 @@
 import frc.robot.subsystems.swerve.SwerveConstants.DriveConstants;
 import frc.robot.subsystems.swerve.SwerveConstants.ModuleConstants;
 
+/** Values are statically stored here used globally throughout the code. */
 public final class Constants {
 
   public static final class LogPaths {
     public static final String SYSTEM_PERFORMANCE_PATH = "SystemPerformance/";
     public static final String PHYSICS_SIMULATION_PATH = "MaplePhysicsSimulation/";
-    public static final String APRIL_TAGS_VISION_PATH = "Vision/AprilTags/";
   }
 
-  public static final Mode currentMode = Mode.SIM;
+  public static final Mode CURRENT_MODE = Mode.SIM;
 
   public static enum Mode {
     /** Running on a real robot. */
@@ -133,8 +133,8 @@ public class SimulationConstants {
   public static final class FieldConstants {
     // TODO: Now that I think about it, I'm pretty sure these measurements stay the same every year,
     // so consider setting them in the base code
-    public static final double FIELD_LENGTH_METERS = Units.inchesToMeters(0 - 9);
-    public static final double FIELD_WIDTH_METERS = Units.inchesToMeters(0 - 9);
+    public static final double FIELD_LENGTH_METERS = Units.inchesToMeters(653);
+    public static final double FIELD_WIDTH_METERS = Units.inchesToMeters(325);
   }
 
   public static final class JoystickConstants {

src/main/java/frc/robot/Robot.java

@@ -19,48 +19,48 @@
  */
 public class Robot extends LoggedRobot {
   private Command m_autonomousCommand;
   private RobotContainer m_robotContainer;
 
   public Robot() {
     // Record metadata
     // Logger.recordMetadata("ProjectName", BuildConstants.MAVEN_NAME);
-    // Logger.recordMetadata("BuildDate", BuildConstants.BUILD_DATE);
-    // Logger.recordMetadata("GitSHA", BuildConstants.GIT_SHA);
-    // Logger.recordMetadata("GitDate", BuildConstants.GIT_DATE);
-    // Logger.recordMetadata("GitBranch", BuildConstants.GIT_BRANCH);
-    // switch (BuildConstants.DIRTY) {
-    //   case 0:
-    //     Logger.recordMetadata("GitDirty", "All changes committed");
-    //     break;
-    //   case 1:
-    //     Logger.recordMetadata("GitDirty", "Uncomitted changes");
-    //     break;
-    //   default:
-    //     Logger.recordMetadata("GitDirty", "Unknown");
-    //     break;
-    // }
+    Logger.recordMetadata("BuildDate", BuildConstants.BUILD_DATE);
+    Logger.recordMetadata("GitSHA", BuildConstants.GIT_SHA);
+    Logger.recordMetadata("GitDate", BuildConstants.GIT_DATE);
+    Logger.recordMetadata("GitBranch", BuildConstants.GIT_BRANCH);
+    switch (BuildConstants.DIRTY) {
+      case 0:
+        Logger.recordMetadata("GitDirty", "All changes committed");
+        break;
+      case 1:
+        Logger.recordMetadata("GitDirty", "Uncomitted changes");
+        break;
+      default:
+        Logger.recordMetadata("GitDirty", "Unknown");
+        break;
+    }
 
     // Set up data receivers & replay source
-    switch (Constants.currentMode) {
+    switch (Constants.CURRENT_MODE) {
       case REAL:
         // Running on a real robot, log to a USB stick ("/U/logs")
-        Logger.addDataReceiver(new WPILOGWriter());
+        // Logger.addDataReceiver(new WPILOGWriter());
         Logger.addDataReceiver(new NT4Publisher());
         break;
 
       case SIM:
         // Running a physics simulator, log to NT
         Logger.addDataReceiver(new NT4Publisher());
         break;
 
       case REPLAY:
         // Replaying a log, set up replay source
         setUseTiming(false); // Run as fast as possible
         String logPath = LogFileUtil.findReplayLog();
         Logger.setReplaySource(new WPILOGReader(logPath));
         Logger.addDataReceiver(new WPILOGWriter(LogFileUtil.addPathSuffix(logPath, "_sim")));
         break;
     }
 
     // Start AdvantageKit logger
     Logger.start();

src/main/java/frc/robot/RobotContainer.java

@@ -25,9 +25,10 @@
 import frc.robot.subsystems.swerve.module.ModuleInterface;
 import frc.robot.subsystems.swerve.module.PhysicalModule;
 import frc.robot.subsystems.swerve.module.SimulatedModule;
+import frc.robot.subsystems.vision.PhysicalVision;
+import frc.robot.subsystems.vision.SimulatedVision;
 import frc.robot.subsystems.vision.Vision;
-import frc.robot.subsystems.vision.VisionIO;
-import frc.robot.subsystems.vision.VisionIOReal;
+import frc.robot.subsystems.vision.VisionInterface;
 import java.util.function.DoubleSupplier;
 import org.littletonrobotics.junction.Logger;
 
@@ -51,7 +52,7 @@
   // private final XboxController driverController = new XboxController(0);
 
   public RobotContainer() {
-    switch (Constants.currentMode) {
+    switch (Constants.CURRENT_MODE) {
       case REAL -> {
         /* Real robot, instantiate hardware IO implementations */
 
@@ -67,7 +68,7 @@
                 new PhysicalModule(SwerveConstants.moduleConfigs[1]),
                 new PhysicalModule(SwerveConstants.moduleConfigs[2]),
                 new PhysicalModule(SwerveConstants.moduleConfigs[3]));
-        visionSubsystem = new Vision(new VisionIOReal());
+        visionSubsystem = new Vision(new PhysicalVision());
       }
 
       case SIM -> {
@@ -85,8 +86,8 @@
                 DriveConstants.TRACK_WIDTH + .2,
                 DriveConstants.WHEEL_BASE + .2,
                 SwerveModuleSimulation.getModule(
-                    DCMotor.getFalcon500(1),
-                    DCMotor.getFalcon500(1),
+                    DCMotor.getKrakenX60(1).withReduction(ModuleConstants.DRIVE_GEAR_RATIO),
+                    DCMotor.getFalcon500(1).withReduction(11),
                     60,
                     WHEEL_GRIP.TIRE_WHEEL,
                     ModuleConstants.DRIVE_GEAR_RATIO),
@@ -105,16 +106,17 @@
                 new SimulatedModule(swerveDriveSimulation.getModules()[2]),
                 new SimulatedModule(swerveDriveSimulation.getModules()[3]));
 
-        // TODO: add sim impl
-        visionSubsystem = new Vision(new VisionIO() {});
+        visionSubsystem =
+            new Vision(
+                new SimulatedVision(() -> swerveDriveSimulation.getSimulatedDriveTrainPose()));
 
         SimulatedField.getInstance().resetFieldForAuto();
         resetFieldAndOdometryForAuto(
             new Pose2d(1.3980597257614136, 5.493067741394043, Rotation2d.fromRadians(3.1415)));
       }
 
       default -> {
-        visionSubsystem = new Vision(new VisionIO() {});
+        visionSubsystem = new Vision(new VisionInterface() {});
         /* Replayed robot, disable IO implementations */
 
         /* physics simulations are also not needed */
@@ -141,7 +143,7 @@
       updateFieldSimAndDisplay();
     }
 
-    swerveDrive.periodic();
+    // swerveDrive.periodic();
     swerveDrive.setPose(startingPose);
   }
 
@@ -164,16 +166,16 @@
     DoubleSupplier driverRightStickX = driverController::getRightX;
     DoubleSupplier driverLeftStick[] =
         new DoubleSupplier[] {
-          () -> JoystickUtil.modifyAxisCubedPolar(driverLeftStickX, driverLeftStickY)[0],
-          () -> JoystickUtil.modifyAxisCubedPolar(driverLeftStickX, driverLeftStickY)[1]
+          () -> JoystickUtil.modifyAxisPolar(driverLeftStickX, driverLeftStickY, 3)[0],
+          () -> JoystickUtil.modifyAxisPolar(driverLeftStickX, driverLeftStickY, 3)[1]
         };
 
     DoubleSupplier operatorLeftStickX = operatorController::getLeftX;
     DoubleSupplier operatorRightStickY = operatorController::getRightY;
 
     Trigger driverRightBumper = new Trigger(driverController.rightBumper());
     Trigger driverRightDirectionPad = new Trigger(driverController.pov(90));
     Trigger driverDownDirectionPad = new Trigger(driverController.pov(180));
     Trigger driverLeftDirectionPad = new Trigger(driverController.pov(270));
 
     // // autodrive
@@ -214,7 +216,7 @@
             visionSubsystem,
             driverLeftStick[1],
             driverLeftStick[0],
-            () -> JoystickUtil.modifyAxisCubed(driverRightStickX),
+            () -> JoystickUtil.modifyAxis(driverRightStickX, 3),
             () -> !driverRightBumper.getAsBoolean(),
             () -> driverLeftBumper.getAsBoolean());
     swerveDrive.setDefaultCommand(driveCommand);
@@ -248,9 +250,14 @@
                         swerveDrive.getPose().getX(),
                         swerveDrive.getPose().getY(),
                         Rotation2d.fromDegrees(swerveDrive.getAllianceAngleOffset())))));
+    driverController
+        .x()
+        .onTrue(
+            new InstantCommand(
+                () -> swerveDrive.setPose(swerveDriveSimulation.getSimulatedDriveTrainPose())));
     // // // Reset robot odometry based on vision pose measurement from april tags
-    // driverLeftDirectionPad.onTrue(new InstantCommand(() ->
-    // swerveDrive.resetOdometry(visionSubsystem.getLastSeenPose())));
+    driverLeftDirectionPad.onTrue(
+        new InstantCommand(() -> swerveDrive.setPose(visionSubsystem.getLastSeenPose())));
     // // driverLeftDpad.onTrue(new InstantCommand(() -> swerveDrive.resetOdometry(new
     // Pose2d(15.251774787902832, 5.573054313659668, Rotation2d.fromRadians(3.14159265)))));
     // // driverBButton.whileTrue(new ShootPass(swerveDrive, shooterSubsystem, pivotSubsystem,