Merge pull request #34 from Gongoliers/3_19_2024_drive_testing

3 19 2024 drive testing

Author: haydenheroux

Diff for: src/main/java/frc/robot/RobotConstants.java

@@ -53,5 +53,5 @@ public enum Subsystem {
           Subsystem.SHOOTER,
           Subsystem.SWERVE);
 
-  public static final Set<Subsystem> REAL_SUBSYSTEMS = ALL_SUBSYSTEMS;
+  public static final Set<Subsystem> REAL_SUBSYSTEMS = EnumSet.of(Subsystem.ODOMETRY, Subsystem.SWERVE);
 }

Diff for: src/main/java/frc/robot/odometry/Odometry.java

@@ -47,7 +47,7 @@ public class Odometry extends Subsystem {
   private final Field2d field;
 
   /** List of functions to be called when pose is manually updated. */
-  private final List<Consumer<Pose2d>> poseUpdateConsumers;
+  private final List<Consumer<Rotation2d>> yawUpdateConsumers;
 
   /** Creates a new instance of the odometry subsystem. */
   private Odometry() {
@@ -73,7 +73,7 @@ private Odometry() {
 
     field = new Field2d();
 
-    poseUpdateConsumers = new ArrayList<Consumer<Pose2d>>();
+    yawUpdateConsumers= new ArrayList<Consumer<Rotation2d>>();
   }
 
   /**
@@ -102,11 +102,16 @@ public void periodic() {
 
   @Override
   public void addToShuffleboard(ShuffleboardTab tab) {
+    ShuffleboardLayout shouldFlip = Telemetry.addColumn(tab, "Should Flip?");
+
+    shouldFlip.addBoolean("Should Flip?", () -> AllianceFlipHelper.shouldFlip());
+
     ShuffleboardLayout position = Telemetry.addColumn(tab, "Position");
 
     position.addDouble("X (m)", () -> getPosition().getX());
     position.addDouble("Y (m)", () -> getPosition().getY());
-    position.addDouble("Rotation (deg)", () -> getPosition().getRotation().getDegrees());
+    position.addDouble("Field Rotation (deg)", () -> getFieldRelativeHeading().getDegrees());
+    position.addDouble("Driver Rotation (deg)", () -> getDriverRelativeHeading().getDegrees());
 
     ShuffleboardLayout velocity = Telemetry.addColumn(tab, "Velocity");
 
@@ -146,13 +151,9 @@ public Rotation2d getFieldRelativeHeading() {
    *     alliance.
    */
   public Rotation2d getDriverRelativeHeading() {
-    Rotation2d fieldRelativeHeading = getFieldRelativeHeading();
-
-    if (AllianceFlipHelper.shouldFlip()) {
-      return fieldRelativeHeading.plus(Rotation2d.fromDegrees(180));
-    }
+    gyroscope.update(gyroscopeValues);
 
-    return fieldRelativeHeading;
+    return Rotation2d.fromRotations(gyroscopeValues.yawRotations);
   }
 
   /**
@@ -183,25 +184,21 @@ public void setRotation(Rotation2d rotation) {
    *
    * @param consumer consumer for when pose is manually updated.
    */
-  public void onPoseUpdate(Consumer<Pose2d> consumer) {
-    poseUpdateConsumers.add(consumer);
+  public void onYawUpdate(Consumer<Rotation2d> consumer) {
+    yawUpdateConsumers.add(consumer);
   }
 
   /**
-   * Tares the rotation of the robot.
+   * Zeroes the driver-relative rotation of the robot.
    *
-   * @return a command that zeroes the rotation of the robot.
+   * @return a command that zeroes the driver-relative rotation of the robot.
    */
   public Command tare() {
     return Commands.runOnce(
         () -> {
-          if (AllianceFlipHelper.shouldFlip()) {
-            setRotation(Rotation2d.fromDegrees(180));
-          } else {
-            setRotation(Rotation2d.fromDegrees(0));
-          }
+          gyroscope.setYaw(0.0);
 
-          poseUpdateConsumers.forEach(consumer -> consumer.accept(getPosition()));
+          yawUpdateConsumers.forEach(consumer -> consumer.accept(Rotation2d.fromDegrees(0)));
         });
   }
 

Diff for: src/main/java/frc/robot/swerve/DriveCommand.java

@@ -56,7 +56,7 @@ public DriveCommand(CommandXboxController driverController) {
 
     headingSnapper = SnapRotation.to(Rotation2d.fromDegrees(90));
 
-    odometry.onPoseUpdate(newPose -> resetHeadingGoal());
+    odometry.onYawUpdate(newPose -> resetHeadingGoal());
   }
 
   @Override
@@ -73,22 +73,22 @@ public void execute() {
 
     Rotation2d driverRelativeHeading = odometry.getDriverRelativeHeading();
 
-    if (DriveRequest.startedDrifting(previousRequest, request)) {
-      resetHeadingGoal();
-    }
+    // if (DriveRequest.startedDrifting(previousRequest, request)) {
+    //   resetHeadingGoal();
+    // }
 
-    if (request.isSnapping()) {
-      setPositionHeadingGoal(headingSnapper.snap(request.fieldHeading()));
-    }
+    // if (request.isSnapping()) {
+    //   setPositionHeadingGoal(headingSnapper.snap(request.driverHeading()));
+    // }
 
-    Rotation2d omega;
+    Rotation2d omega = new Rotation2d();
 
     if (request.isSpinning()) {
       updateVelocity(request.omega());
 
       omega = request.omega();
     } else {
-      omega = calculateHeadingProfileOmega();
+      // omega = calculateHeadingProfileOmega();
     }
 
     ChassisSpeeds chassisSpeeds =
@@ -152,7 +152,7 @@ private ChassisSpeeds clampChassisSpeeds(ChassisSpeeds desiredChassisSpeeds) {
    * @return the reference heading to use with the heading motion profile.
    */
   private Rotation2d getReferenceHeading() {
-    return odometry.getFieldRelativeHeading();
+    return odometry.getDriverRelativeHeading();
   }
 
   /**

Diff for: src/main/java/frc/robot/swerve/DriveRequest.java

@@ -4,7 +4,6 @@
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
-import frc.lib.AllianceFlipHelper;
 
 public record DriveRequest(
     DriveRequest.TranslationMode translationMode,
@@ -103,14 +102,4 @@ public Rotation2d omega() {
   public Rotation2d driverHeading() {
     return rotation().getAngle();
   }
-
-  public Rotation2d fieldHeading() {
-    Rotation2d heading = driverHeading();
-
-    if (AllianceFlipHelper.shouldFlip()) {
-      heading = heading.plus(Rotation2d.fromDegrees(180));
-    }
-
-    return heading;
-  }
 }

Diff for: src/main/java/frc/robot/swerve/SwerveConstants.java

@@ -67,28 +67,28 @@ public static class MK4iConstants {
       new SwerveModuleConfig(
           new SwerveModuleCAN(11, 10, 12, SWERVE_BUS),
           new Translation2d(X_OFFSET, Y_OFFSET),
-          Rotation2d.fromRotations(-0.027100));
+          Rotation2d.fromRotations(-0.385742));
 
   /** Module configuration for the north east swerve module. */
   public static final SwerveModuleConfig NORTH_EAST_MODULE_CONFIG =
       new SwerveModuleConfig(
           new SwerveModuleCAN(2, 1, 3, SWERVE_BUS),
           new Translation2d(X_OFFSET, -Y_OFFSET),
-          Rotation2d.fromRotations(-0.429688));
+          Rotation2d.fromRotations(-0.229004));
 
   /** Module configuration for the south east swerve module. */
   public static final SwerveModuleConfig SOUTH_EAST_MODULE_CONFIG =
       new SwerveModuleConfig(
           new SwerveModuleCAN(5, 4, 6, SWERVE_BUS),
           new Translation2d(-X_OFFSET, -Y_OFFSET),
-          Rotation2d.fromRotations(-0.097168));
+          Rotation2d.fromRotations(-0.096436));
 
   /** Module configuration for the south west swerve module. */
   public static final SwerveModuleConfig SOUTH_WEST_MODULE_CONFIG =
       new SwerveModuleConfig(
           new SwerveModuleCAN(8, 7, 9, SWERVE_BUS),
           new Translation2d(-X_OFFSET, Y_OFFSET),
-          Rotation2d.fromRotations(-0.309814));
+          Rotation2d.fromRotations(-0.448242));
 
   /**
    * Calculates the maximum attainable open loop speed in meters per second.