wtf why does it work 😭

TitaniumTigers4829 · Jan 4, 2025 · bf661f9 · bf661f9
1 parent 79a886e
commit bf661f9
Show file tree

Hide file tree

Showing 12 changed files with 142 additions and 108 deletions.
diff --git a/src/main/java/frc/robot/BuildConstants.java b/src/main/java/frc/robot/BuildConstants.java
@@ -3,15 +3,15 @@
 /** 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";
+  public static final String MAVEN_NAME = "offseason-robot-code-2024-1";
   public static final String VERSION = "unspecified";
-  public static final int GIT_REVISION = 464;
-  public static final String GIT_SHA = "e85bebc1ff41f2682e3d4ccbe67f1f5ce0f8a0f3";
-  public static final String GIT_DATE = "2024-12-08 17:15:05 EST";
-  public static final String GIT_BRANCH = "add-aquila-constants";
-  public static final String BUILD_DATE = "2024-12-08 17:29:19 EST";
-  public static final long BUILD_UNIX_TIME = 1733696959570L;
-  public static final int DIRTY = 0;
+  public static final int GIT_REVISION = 533;
+  public static final String GIT_SHA = "79a886e543767e4320fa8f9f31ac613afa9377c3";
+  public static final String GIT_DATE = "2025-01-02 19:18:09 EST";
+  public static final String GIT_BRANCH = "vision-sim";
+  public static final String BUILD_DATE = "2025-01-03 21:17:04 EST";
+  public static final long BUILD_UNIX_TIME = 1735957024697L;
+  public static final int DIRTY = 1;
 
   private BuildConstants() {}
 }
diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java
@@ -24,21 +24,21 @@ public class Robot extends LoggedRobot {
   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.CURRENT_MODE) {

diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
@@ -8,7 +8,6 @@
 import edu.wpi.first.wpilibj2.command.InstantCommand;
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
 import edu.wpi.first.wpilibj2.command.button.Trigger;
-import frc.robot.Constants.SimulationConstants;
 import frc.robot.commands.drive.DriveCommand;
 import frc.robot.extras.simulation.field.SimulatedField;
 import frc.robot.extras.simulation.mechanismSim.swerve.GyroSimulation;
@@ -87,8 +86,8 @@ public RobotContainer() {
                 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),
@@ -145,7 +144,7 @@ private void resetFieldAndOdometryForAuto(Pose2d robotStartingPoseAtBlueAlliance
     }
 
     // swerveDrive.periodic();
-    swerveDrive.resetPosition(startingPose);
+    swerveDrive.setPose(startingPose);
   }
 
   public void teleopInit() {
@@ -246,7 +245,7 @@ private void configureButtonBindings() {
     driverRightDirectionPad.onTrue(
         new InstantCommand(
             () ->
-                swerveDrive.resetPosition(
+                swerveDrive.setPose(
                     new Pose2d(
                         swerveDrive.getPose().getX(),
                         swerveDrive.getPose().getY(),
@@ -255,11 +254,10 @@ private void configureButtonBindings() {
         .x()
         .onTrue(
             new InstantCommand(
-                () ->
-                    swerveDrive.resetPosition(swerveDriveSimulation.getSimulatedDriveTrainPose())));
+                () -> swerveDrive.setPose(swerveDriveSimulation.getSimulatedDriveTrainPose())));
     // // // Reset robot odometry based on vision pose measurement from april tags
     driverLeftDirectionPad.onTrue(
-        new InstantCommand(() -> swerveDrive.resetPosition(visionSubsystem.getLastSeenPose())));
+        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,
@@ -308,7 +306,7 @@ private void configureButtonBindings() {
   public Command getAutonomousCommand() {
     // Resets the pose factoring in the robot side
     // This is just a failsafe, pose should be reset at the beginning of auto
-    swerveDrive.resetPosition(
+    swerveDrive.setPose(
         new Pose2d(
             swerveDrive.getPose().getX(),
             swerveDrive.getPose().getY(),

diff --git a/src/main/java/frc/robot/commands/drive/DriveCommandBase.java b/src/main/java/frc/robot/commands/drive/DriveCommandBase.java
@@ -4,6 +4,7 @@
 
 import edu.wpi.first.wpilibj2.command.Command;
 import frc.robot.extras.interpolators.MultiLinearInterpolator;
+import frc.robot.extras.util.TimeUtil;
 import frc.robot.subsystems.swerve.SwerveDrive;
 import frc.robot.subsystems.vision.Vision;
 import frc.robot.subsystems.vision.VisionConstants;
@@ -70,7 +71,7 @@ public void calculatePoseFromLimelight(Limelight limelight) {
 
       swerveDrive.addPoseEstimatorVisionMeasurement(
           vision.getPoseFromAprilTags(limelight),
-          Logger.getTimestamp() - vision.getLatencySeconds(limelight));
+          TimeUtil.getLogTimeSeconds() - vision.getLatencySeconds(limelight));
     }
 
     Logger.recordOutput(

diff --git a/src/main/java/frc/robot/extras/util/TimeUtil.java b/src/main/java/frc/robot/extras/util/TimeUtil.java
@@ -1,5 +1,6 @@
 package frc.robot.extras.util;
 
+import edu.wpi.first.wpilibj.RobotController;
 import org.littletonrobotics.junction.Logger;
 
 public class TimeUtil {
@@ -26,6 +27,6 @@ public static double getLogTimeSeconds() {
   }
 
   public static double getRealTimeSeconds() {
-    return Logger.getRealTimestamp() / 1_000_000.0;
+    return RobotController.getFPGATime() / 1_000_000.0;
   }
 }
diff --git a/src/main/java/frc/robot/subsystems/swerve/SwerveDrive.java b/src/main/java/frc/robot/subsystems/swerve/SwerveDrive.java
@@ -39,7 +39,7 @@ public class SwerveDrive extends SubsystemBase {
 
   private Rotation2d rawGyroRotation;
   private final SwerveModulePosition[] lastModulePositions;
-  private final SwerveDrivePoseEstimator odometry;
+  private final SwerveDrivePoseEstimator poseEstimator;
 
   private final SwerveSetpointGenerator setpointGenerator =
       new SwerveSetpointGenerator(
@@ -86,10 +86,10 @@ public SwerveDrive(
           new SwerveModulePosition(),
           new SwerveModulePosition()
         };
-    this.odometry =
+    this.poseEstimator =
         new SwerveDrivePoseEstimator(
             DriveConstants.DRIVE_KINEMATICS,
-            getGyroRotation2d(),
+            rawGyroRotation,
             lastModulePositions,
             new Pose2d(),
             VecBuilder.fill(
@@ -99,7 +99,7 @@ public SwerveDrive(
                 VisionConstants.VISION_Y_POS_TRUST,
                 VisionConstants.VISION_ANGLE_TRUST));
 
-    this.odometryThread = OdometryThread.createInstance(DeviceCANBus.CANIVORE);
+    this.odometryThread = OdometryThread.createInstance(DeviceCANBus.RIO);
     this.odometryThreadInputs = new OdometryThreadInputsAutoLogged();
     this.odometryThread.start();
 
@@ -123,7 +123,7 @@ public SwerveDrive(
    */
   public void addPoseEstimatorVisionMeasurement(
       Pose2d visionMeasurement, double currentTimeStampSeconds) {
-    odometry.addVisionMeasurement(visionMeasurement, currentTimeStampSeconds);
+    poseEstimator.addVisionMeasurement(visionMeasurement, currentTimeStampSeconds);
   }
 
   /**
@@ -136,7 +136,7 @@ public void addPoseEstimatorVisionMeasurement(
    */
   public void setPoseEstimatorVisionConfidence(
       double xStandardDeviation, double yStandardDeviation, double thetaStandardDeviation) {
-    odometry.setVisionMeasurementStdDevs(
+    poseEstimator.setVisionMeasurementStdDevs(
         VecBuilder.fill(xStandardDeviation, yStandardDeviation, thetaStandardDeviation));
   }
 
@@ -160,7 +160,6 @@ public void runCharacterization(double volts) {
   }
 
   /** Returns the average drive velocity in rotations/sec. */
-  // TODO: fix method
   public double getCharacterizationVelocity() {
     double velocity = 0.0;
     for (SwerveModule module : swerveModules) {
@@ -256,7 +255,7 @@ public void zeroHeading() {
    */
   @AutoLogOutput(key = "Odometry/Odometry")
   public Pose2d getPose() {
-    return odometry.getEstimatedPosition();
+    return poseEstimator.getEstimatedPosition();
   }
 
   /** Returns a Rotation2d for the heading of the robot */
@@ -319,7 +318,7 @@ public void addPoseEstimatorSwerveMeasurement() { // int timestampIndex
       rawGyroRotation = rawGyroRotation.plus(new Rotation2d(twist.dtheta));
     }
 
-    odometry.updateWithTime(
+    poseEstimator.updateWithTime(
         // odometryThreadInputs.measurementTimeStamps[timestampIndex],
         Logger.getTimestamp(), rawGyroRotation, modulePositions);
   }
@@ -373,7 +372,7 @@ private SwerveModulePosition[] getModulePositions() {
    *
    * @param pose pose to set
    */
-  public void resetPosition(Pose2d pose) {
-    odometry.resetPosition(getGyroRotation2d(), getModulePositions(), pose);
+  public void setPose(Pose2d pose) {
+    poseEstimator.resetPosition(rawGyroRotation, getModulePositions(), pose);
   }
 }
diff --git a/src/main/java/frc/robot/subsystems/swerve/setpointGen/SwerveSetpointGenerator.java b/src/main/java/frc/robot/subsystems/swerve/setpointGen/SwerveSetpointGenerator.java
@@ -130,11 +130,7 @@ public SwerveSetpoint generateSetpoint(
             vars.prevSpeeds.vxMetersPerSecond + vars.minS * vars.dx,
             vars.prevSpeeds.vyMetersPerSecond + vars.minS * vars.dy,
             vars.prevSpeeds.omegaRadiansPerSecond + vars.minS * vars.dtheta);
-    retSpeeds.discretize(
-        retSpeeds.vxMetersPerSecond,
-        retSpeeds.vyMetersPerSecond,
-        retSpeeds.omegaRadiansPerSecond,
-        dt);
+    retSpeeds = ChassisSpeeds.discretize(retSpeeds, dt);
 
     double chassisAccelX = (retSpeeds.vxMetersPerSecond - vars.prevSpeeds.vxMetersPerSecond) / dt;
     double chassisAccelY = (retSpeeds.vyMetersPerSecond - vars.prevSpeeds.vyMetersPerSecond) / dt;

diff --git a/src/main/java/frc/robot/subsystems/vision/PhysicalVision.java b/src/main/java/frc/robot/subsystems/vision/PhysicalVision.java
@@ -61,6 +61,8 @@ public void updateInputs(VisionInputs inputs) {
         inputs.limelightCalculatedPose[limelight.getId()] = getPoseFromAprilTags(limelight);
         inputs.limelightTimestamp[limelight.getId()] = getTimeStampSeconds(limelight);
         inputs.limelightLastSeenPose = getLastSeenPose();
+        inputs.limelightAprilTagDistance[limelight.getId()] =
+            getLimelightAprilTagDistance(limelight);
 
         latestInputs.set(inputs);
         limelightThreads.get(limelight).set(latestInputs.get());

diff --git a/src/main/java/frc/robot/subsystems/vision/SimulatedVision.java b/src/main/java/frc/robot/subsystems/vision/SimulatedVision.java
@@ -46,8 +46,8 @@ public SimulatedVision(Supplier<Pose2d> robotSimulationPose) {
     // Create simulated camera properties. These can be set to mimic your actual
     // camera.
     var cameraProperties = new SimCameraProperties();
-    // cameraProperties.setCalibration(kResWidth, kResHeight, Rotation2d.fromDegrees(97.7));
-    // cameraProperties.setCalibError(0.35, 0.10);
+    cameraProperties.setCalibration(kResWidth, kResHeight, Rotation2d.fromDegrees(97.7));
+    cameraProperties.setCalibError(0.35, 0.10);
     cameraProperties.setFPS(15);
     cameraProperties.setAvgLatencyMs(20);
     cameraProperties.setLatencyStdDevMs(5);
@@ -63,23 +63,23 @@ public SimulatedVision(Supplier<Pose2d> robotSimulationPose) {
     frontRightCameraSim =
         new PhotonCameraSim(getSimulationCamera(Limelight.FRONT_RIGHT), cameraProperties);
 
-    visionSim.addCamera(shooterCameraSim, VisionConstants.SHOOTER_TRANSFORM);
-    visionSim.addCamera(frontLeftCameraSim, VisionConstants.FRONT_LEFT_TRANSFORM);
-    visionSim.addCamera(frontRightCameraSim, VisionConstants.FRONT_RIGHT_TRANSFORM);
+    visionSim.addCamera(shooterCameraSim, VisionConstants.SHOOTER_TRANSFORM.inverse());
+    visionSim.addCamera(frontLeftCameraSim, VisionConstants.FRONT_LEFT_TRANSFORM.inverse());
+    visionSim.addCamera(frontRightCameraSim, VisionConstants.FRONT_RIGHT_TRANSFORM.inverse());
 
     // Enable the raw and processed streams. (http://localhost:1181 / 1182)
-    shooterCameraSim.enableRawStream(true);
-    shooterCameraSim.enableProcessedStream(true);
-    frontLeftCameraSim.enableRawStream(true);
-    frontLeftCameraSim.enableProcessedStream(true);
-    frontRightCameraSim.enableRawStream(true);
-    frontRightCameraSim.enableProcessedStream(true);
-
-    // Enable drawing a wireframe visualization of the field to the camera streams.
-    // This is extremely resource-intensive and is disabled by default.
-    shooterCameraSim.enableDrawWireframe(true);
-    frontLeftCameraSim.enableDrawWireframe(true);
-    frontRightCameraSim.enableDrawWireframe(true);
+    // shooterCameraSim.enableRawStream(true);
+    // shooterCameraSim.enableProcessedStream(true);
+    // frontLeftCameraSim.enableRawStream(true);
+    // frontLeftCameraSim.enableProcessedStream(true);
+    // frontRightCameraSim.enableRawStream(true);
+    // frontRightCameraSim.enableProcessedStream(true);
+
+    // // Enable drawing a wireframe visualization of the field to the camera streams.
+    // // This is extremely resource-intensive and is disabled by default.
+    // shooterCameraSim.enableDrawWireframe(true);
+    // frontLeftCameraSim.enableDrawWireframe(true);
+    // frontRightCameraSim.enableDrawWireframe(true);
   }
 
   @Override
@@ -97,8 +97,9 @@ public void updateInputs(VisionInputs inputs) {
           getSimulationCamera(limelight).getAllUnreadResults(),
           getLimelightTable(limelight),
           limelight);
-      inputs.limelightTargets[limelight.getId()] = getNumberOfAprilTags(limelight);
-      inputs.limelightAprilTagDistance[limelight.getId()] = getLimelightAprilTagDistance(limelight);
+      // inputs.limelightTargets[limelight.getId()] = getNumberOfAprilTags(limelight);
+      // inputs.limelightAprilTagDistance[limelight.getId()] =
+      // getLimelightAprilTagDistance(limelight);
     }
     super.updateInputs(inputs);
   }
@@ -145,7 +146,13 @@ private void writeToTable(
             .setDoubleArray(pose_data.stream().mapToDouble(Double::doubleValue).toArray());
         tagCount[limelight.getId()] = result.getMultiTagResult().get().fiducialIDsUsed.size();
         apriltagDist[limelight.getId()] =
-            result.getMultiTagResult().get().estimatedPose.best.getX();
+            result
+                .getMultiTagResult()
+                .get()
+                .estimatedPose
+                .best
+                .getTranslation()
+                .getDistance(result.getBestTarget().bestCameraToTarget.getTranslation());
       }
 
       table.getEntry("tv").setInteger(result.hasTargets() ? 1 : 0);