reset Gyro button

awtybots · Feb 24, 2024 · fcbe925 · fcbe925
1 parent 6a77022
commit fcbe925
Show file tree

Hide file tree

Showing 4 changed files with 25 additions and 15 deletions.
diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
@@ -16,7 +16,6 @@
 import com.pathplanner.lib.auto.AutoBuilder;
 import com.pathplanner.lib.auto.NamedCommands;
 import com.pathplanner.lib.commands.PathPlannerAuto;
-
 import edu.wpi.first.wpilibj.GenericHID;
 import edu.wpi.first.wpilibj.XboxController;
 import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
@@ -27,9 +26,9 @@
 import frc.robot.commands.ControlCommands.ArmElevatorCommands;
 import frc.robot.commands.ControlCommands.DriveCommands;
 import frc.robot.commands.ControlCommands.WristCommands;
+import frc.robot.commands.FeedForwardCharacterization;
 import frc.robot.commands.Positions.AmpShot;
 import frc.robot.commands.Positions.FloorPickup;
-import frc.robot.commands.FeedForwardCharacterization;
 import frc.robot.commands.Positions.StowPosition;
 import frc.robot.commands.Positions.Upwards;
 import frc.robot.subsystems.arm.Arm;
@@ -66,10 +65,8 @@
 import frc.robot.subsystems.wrist.WristIO;
 import frc.robot.subsystems.wrist.WristIOSim;
 import frc.robot.subsystems.wrist.WristIOSparkMax;
-
 import java.util.ArrayList;
 import java.util.List;
-
 import org.littletonrobotics.junction.networktables.LoggedDashboardChooser;
 import org.littletonrobotics.junction.networktables.LoggedDashboardNumber;
 
@@ -272,6 +269,9 @@ private void configureButtonBindings() {
         .b()
         .whileFalse(Commands.startEnd(() -> sIntake.runVelocity(0), sIntake::stop, sIntake));
 
+    driverController.start().whileTrue(Commands.startEnd(() -> sDrive.resetRotation(), sDrive::stop, sDrive));
+
+    // Operator controller configurations
     sArm.setDefaultCommand(ArmCommands.joystickDrive(sArm, () -> -operatorController.getRightY()));
 
     sWrist.setDefaultCommand(
@@ -283,17 +283,17 @@ private void configureButtonBindings() {
             () -> operatorController.getLeftTriggerAxis(),
             () -> operatorController.getRightTriggerAxis()));
 
-    // operatorController
-    //     .a()
-    //     .whileTrue(
-    //         Commands.startEnd(() -> sClimber.runTargetPosition(0), sClimber::stop, sClimber));
-    // operatorController
-    //     .b()
-    //     .whileTrue(
-    //         Commands.startEnd(
-    //             () -> sClimber.runTargetPosition(0.55),
-    //             sClimber::stop,
-    //             sClimber)); // !Testing numbers
+    operatorController
+        .a()
+        .whileTrue(
+            Commands.startEnd(() -> sClimber.runTargetPosition(0), sClimber::stop, sClimber));
+    operatorController
+        .b()
+        .whileTrue(
+            Commands.startEnd(
+                () -> sClimber.runTargetPosition(0.55), // !Testing numbers
+                sClimber::stop,
+                sClimber)); 
 
     // run straight up position when y is pressed on operator. Using command Upwards
     operatorController.y().whileTrue(Upwards.run(sArm, sArmElevator, sWrist));

diff --git a/src/main/java/frc/robot/subsystems/drive/Drive.java b/src/main/java/frc/robot/subsystems/drive/Drive.java
@@ -222,6 +222,10 @@ public Rotation2d getRotation() {
     return pose.getRotation();
   }
 
+  public void resetRotation() {
+    this.gyroIO.resetRotation();
+  }
+
   /** Resets the current odometry pose. */
   public void setPose(Pose2d pose) {
     this.pose = pose;

diff --git a/src/main/java/frc/robot/subsystems/drive/GyroIO.java b/src/main/java/frc/robot/subsystems/drive/GyroIO.java
@@ -26,4 +26,6 @@ public static class GyroIOInputs {
   }
 
   public default void updateInputs(GyroIOInputs inputs) {}
+
+  public default void resetRotation() {}
 }
diff --git a/src/main/java/frc/robot/subsystems/drive/GyroIONavX.java b/src/main/java/frc/robot/subsystems/drive/GyroIONavX.java
@@ -60,4 +60,8 @@ public void updateInputs(GyroIOInputs inputs) {
 
     yawPositionQueue.clear();
   }
+
+  public void resetRotation() {
+    ahrs.zeroYaw();
+  }
 }