reset field oriented w/out changing gyro

applepi-2067 · Apr 1, 2024 · 7590000 · 7590000
1 parent 6c22dcc
commit 7590000
Show file tree

Hide file tree

Showing 2 changed files with 12 additions and 11 deletions.
diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
@@ -136,8 +136,8 @@ private void configDriverBindings() {
 
     m_driverController.povUp().onTrue(new InstantCommand(() -> m_drivetrain.drive(0.0, 0.0, 0.0), m_drivetrain));
 
-    m_driverController.rightBumper().onTrue(new InstantCommand(() -> m_drivetrain.resetGyro(true)));
-    m_driverController.leftBumper().onTrue(new InstantCommand(() -> m_drivetrain.resetGyro(false)));
+    // m_driverController.rightBumper().onTrue(new InstantCommand(() -> m_drivetrain.resetFieldOriented(true)));
+    m_driverController.leftBumper().onTrue(new InstantCommand(() -> m_drivetrain.resetFieldOriented(false)));
 
     m_driverController.rightTrigger().onTrue(new InstantCommand(() -> m_drivetrain.setTargetAprilTag(Optional.of(AprilTag.SPEAKER))));
     m_driverController.leftTrigger().onTrue(new InstantCommand(() -> m_drivetrain.setTargetAprilTag(Optional.empty())));

diff --git a/src/main/java/frc/robot/subsystems/Drivetrain.java b/src/main/java/frc/robot/subsystems/Drivetrain.java
@@ -63,6 +63,8 @@ public class Drivetrain extends SubsystemBase implements Loggable {
   private final PigeonIMU m_gyro;
   private final Field2d m_field;
 
+  private double m_fieldOrientedHeadingOffsetDegrees = 0.0;
+
   // Pose facing.
   private Optional<AprilTag> m_targetAprilTag = Optional.empty();
   private final PIDController m_poseFacingPIDController = new PIDController(0.015, 0.05, 0.0);
@@ -154,7 +156,7 @@ public void drive(double leftStickX, double leftStickY, double rightStickX) {
       xVelocityMetersPerSecond,
       yVelocityMetersPerSecond,
       rotationVelocityRadiansPerSecond,
-      Rotation2d.fromDegrees(getHeadingDegrees())
+      Rotation2d.fromDegrees(getHeadingDegrees() - m_fieldOrientedHeadingOffsetDegrees)
     );
 
     driveRobotRelative(chassisSpeeds);
@@ -206,16 +208,15 @@ public void setSwerveModuleStates(SwerveModuleState[] states) {
     }
   }
 
-  public void resetGyro(boolean fieldOriented) {
-    Rotation2d newHeading = new Rotation2d();
+  public void resetFieldOriented(boolean fieldOriented) {
     if (fieldOriented) {
-      newHeading = getRobotPose2d().getRotation();
-      if (!isBlue()) {
-        newHeading = newHeading.plus(Rotation2d.fromDegrees(180.0));
-      }
+      Rotation2d currRotation = getRobotPose2d().getRotation();  // FIXME: why this not work???
+      if (!isBlue()) {currRotation = currRotation.plus(Rotation2d.fromDegrees(180.0));}
+      m_fieldOrientedHeadingOffsetDegrees = currRotation.getDegrees() + getHeadingDegrees();
+    }
+    else {
+      m_fieldOrientedHeadingOffsetDegrees = getHeadingDegrees();
     }
-
-    m_gyro.setYaw(newHeading.getDegrees());
   }
 
   @Log (name="Heading (deg)")