added some code for testing, and switched back to old gyro

Team334 · Mar 14, 2024 · 7fa007d · 7fa007d
1 parent f8547b9
commit 7fa007d
Show file tree

Hide file tree

Showing 2 changed files with 11 additions and 23 deletions.
diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
@@ -10,6 +10,8 @@
 import edu.wpi.first.math.MathUtil;
 import edu.wpi.first.math.filter.SlewRateLimiter;
 import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.wpilibj.DriverStation.Alliance;
 import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
@@ -145,31 +147,17 @@ private void configureBindings() {
     _operatorController.triangle().whileTrue(new FeedActuate(_intakeSubsystem, ActuatorState.STOWED, FeedMode.OUTTAKE));
     _operatorController.cross().whileTrue(new FeedActuate(_intakeSubsystem, FeedMode.INTAKE));
 
-    // _operatorController.R2().whileTrue(
-    //   Commands.runOnce(_intakeSubsystem::disableReverseSoftLimit, _intakeSubsystem).andThen(
-    //     Commands.run(() -> _intakeSubsystem.actuate(-0.3), _intakeSubsystem).handleInterrupt(() -> _intakeSubsystem.actuate(0))
-    // ));
-
-    // _operatorController.R2().onTrue(
-    //   Commands.runOnce(_intakeSubsystem::toggleReverseSoftLimit, _intakeSubsystem)
-    // );
-
-    // _operatorController.R1().onTrue(
-    //   Commands.run(() -> _intakeSubsystem.actuate(-0.1), _intakeSubsystem).handleInterrupt(
-    //     _intakeSubsystem::resetActuatorEncoder
-    //   )
-    // );
-
-    // _operatorController.R1().onTrue(
-    //   Commands.runOnce(_intakeSubsystem::resetReverseSoftLimit, _intakeSubsystem)
-    // );
-
     // driver bindings
     _driveController.L1().onTrue(Commands.runOnce(_swerveSubsystem::toggleSpeed, _swerveSubsystem));
     _driveController.R1().onTrue(Commands.runOnce(() -> _swerveSubsystem.fieldOriented = !_swerveSubsystem.fieldOriented, _swerveSubsystem));
     _driveController.triangle().onTrue(Commands.runOnce(_swerveSubsystem::resetGyro, _swerveSubsystem));
     _driveController.cross().whileTrue(new BrakeSwerve(_swerveSubsystem, _ledSubsystem));
 
+    // TESTING ONLY!!!
+    _driveController.circle().onTrue(Commands.runOnce(() -> _swerveSubsystem.resetPose(
+      new Pose2d(new Translation2d(1.31, 5.51), Rotation2d.fromDegrees(180))
+    ), _swerveSubsystem));
+
     _driveController.L2().whileTrue(
       new AutoAim(
         _shooterSubsystem, 

diff --git a/src/main/java/frc/robot/subsystems/SwerveDriveSubsystem.java b/src/main/java/frc/robot/subsystems/SwerveDriveSubsystem.java
@@ -64,11 +64,10 @@ public class SwerveDriveSubsystem extends SubsystemBase {
       new SwerveModuleState(_backRight.getDriveVelocity(), Rotation2d.fromDegrees(_frontLeft.getAngle())),
       new SwerveModuleState(_backLeft.getDriveVelocity(), Rotation2d.fromDegrees(_frontLeft.getAngle()))};
 
-  // private final BNO055 _gyro = BNO055.getInstance(BNO055.opmode_t.OPERATION_MODE_IMUPLUS,
-  //     BNO055.vector_type_t.VECTOR_EULER);
+  private final BNO055 _gyro = BNO055.getInstance(BNO055.opmode_t.OPERATION_MODE_IMUPLUS,
+      BNO055.vector_type_t.VECTOR_EULER);
 
   // private final AHRS _gyro = new AHRS();
-  private final AHRS _gyro = new AHRS();
 
   private VisionSubsystem _visionSubsystem;
 
@@ -301,7 +300,8 @@ public Rotation2d getHeading() {
 
   /** Get heading DIRECTLY from the BNO055 gyro as a Rotation2d. */
   public Rotation2d getHeadingRaw() {
-    return Rotation2d.fromDegrees(-Math.IEEEremainder(_gyro.getAngle(), 360));
+    return Rotation2d.fromDegrees(-Math.IEEEremainder(_gyro.getHeading(), 360));
+    // return Rotation2d.fromDegrees(-Math.IEEEremainder(_gyro.getAngle(), 360));
   }
 
   /**