Fix field oriented drive (#19)

* "back/select" button resets odometry * Increased joystick deadband to prevent drift
DevilBotz2876 · Jan 27, 2024 · 3f5779e · 3f5779e
1 parent 6b2773e
commit 3f5779e
Show file tree

Hide file tree

Showing 2 changed files with 13 additions and 5 deletions.
diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
@@ -62,15 +62,17 @@ private void configureBindings() {
     drive.setDefaultCommand(
         new DriveCommand(
             drive,
-            () -> MathUtil.applyDeadband(-controller.getLeftY(), 0.01),
-            () -> MathUtil.applyDeadband(-controller.getLeftX(), 0.01),
-            () -> MathUtil.applyDeadband(-controller.getRightX(), 0.01)));
+            () -> MathUtil.applyDeadband(-controller.getLeftY(), 0.05),
+            () -> MathUtil.applyDeadband(-controller.getLeftX(), 0.05),
+            () -> MathUtil.applyDeadband(-controller.getRightX(), 0.05)));
     // TODO: Move deadband to constants file
 
     controller
         .start()
         .onTrue(
             new InstantCommand(() -> drive.setFieldOrientedDrive(!drive.isFieldOrientedDrive())));
+
+    controller.back().onTrue(new InstantCommand(() -> drive.resetOdometry()));
   }
 
   public Command getAutonomousCommand() {

diff --git a/src/main/java/frc/robot/subsystems/drive/DriveSwerveYAGSL.java b/src/main/java/frc/robot/subsystems/drive/DriveSwerveYAGSL.java
@@ -1,19 +1,21 @@
 package frc.robot.subsystems.drive;
 
+import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.kinematics.ChassisSpeeds;
 import edu.wpi.first.math.util.Units;
 import edu.wpi.first.wpilibj.Filesystem;
 import java.io.File;
+import org.littletonrobotics.junction.AutoLogOutput;
 import swervelib.SwerveDrive;
 import swervelib.parser.SwerveParser;
 
 public class DriveSwerveYAGSL extends DriveBase {
   private final double maximumSpeed =
-      Units.feetToMeters(2); // * TODO: Calculate actual max speed */
+      Units.feetToMeters(4.5); // * TODO: Calculate actual max speed */
   private final File swerveJsonDirectory =
       new File(Filesystem.getDeployDirectory(), "swervePracticeBot");
   private SwerveDrive swerveDrive;
-  private boolean fieldOrientedDrive = false;
+  @AutoLogOutput private boolean fieldOrientedDrive = false;
 
   public DriveSwerveYAGSL() {
     try {
@@ -49,4 +51,8 @@ public void setFieldOrientedDrive(boolean enable) {
   public boolean isFieldOrientedDrive() {
     return fieldOrientedDrive;
   }
+
+  public void resetOdometry() {
+    swerveDrive.resetOdometry(new Pose2d());
+  }
 }