drive still weird

simgui-ds.json

@@ -32,10 +32,10 @@
       "axisCount": 5,
       "buttonCount": 4,
       "buttonKeys": [
-        90,
-        88,
         67,
-        86
+        66,
+        88,
+        90
       ],
       "povConfig": [
         {
@@ -49,7 +49,7 @@
           "key90": 326
         }
       ],
-      "povCount": 1
+      "povCount": 0
     },
     {
       "axisConfig": [

src/main/java/frc/robot/Constants.java

@@ -7,6 +7,7 @@
 import static edu.wpi.first.units.Units.*;
 
 import edu.wpi.first.units.measure.AngularVelocity;
+import edu.wpi.first.units.measure.Distance;
 import edu.wpi.first.units.measure.Frequency;
 import edu.wpi.first.units.measure.LinearVelocity;
 import frc.robot.generated.TunerConstants;
@@ -27,7 +28,13 @@ public static class Ports {
   }
 
   public static class SwerveConstants {
-    public static final Frequency odometryFrequency = Hertz.of(100);
+    public static final Frequency odometryFrequency = Hertz.of(250);
+
+    public static final Distance driveRadius =
+        Meters.of(
+            Math.sqrt(
+                Math.pow(TunerConstants.FrontLeft.LocationX, 2)
+                    + Math.pow(TunerConstants.FrontLeft.LocationY, 2)));
 
     public static final LinearVelocity translationalDeadband = MetersPerSecond.of(0);
     public static final AngularVelocity rotationalDeadband = RadiansPerSecond.of(0);

src/main/java/frc/robot/Robot.java

@@ -54,6 +54,10 @@ public Robot() {
 
     DriverStation.silenceJoystickConnectionWarning(RobotBase.isSimulation());
 
+    configureBindings();
+  }
+
+  private void configureBindings() {
     _swerve.setDefaultCommand(
         _swerve.drive(
             InputStream.of(_driverController::getLeftY)
@@ -65,6 +69,9 @@ public Robot() {
             InputStream.of(_driverController::getRightX)
                 .negate()
                 .scale(SwerveConstants.maxAngularSpeed.in(RadiansPerSecond))));
+
+    _driverController.x().whileTrue(_swerve.brake());
+    _driverController.a().onTrue(_swerve.toggleFieldOriented());
   }
 
   /**
@@ -83,13 +90,6 @@ public void robotPeriodic() {
     CommandScheduler.getInstance().run();
   }
 
-  /** This function is called once each time the robot enters Disabled mode. */
-  @Override
-  public void disabledInit() {}
-
-  @Override
-  public void disabledPeriodic() {}
-
   /** This autonomous runs the autonomous command. */
   @Override
   public void autonomousInit() {
@@ -99,10 +99,6 @@ public void autonomousInit() {
     }
   }
 
-  /** This function is called periodically during autonomous. */
-  @Override
-  public void autonomousPeriodic() {}
-
   @Override
   public void teleopInit() {
     // This makes sure that the autonomous stops running when
@@ -114,25 +110,9 @@ public void teleopInit() {
     }
   }
 
-  /** This function is called periodically during operator control. */
-  @Override
-  public void teleopPeriodic() {}
-
   @Override
   public void testInit() {
     // Cancels all running commands at the start of test mode.
     CommandScheduler.getInstance().cancelAll();
   }
-
-  /** This function is called periodically during test mode. */
-  @Override
-  public void testPeriodic() {}
-
-  /** This function is called once when the robot is first started up. */
-  @Override
-  public void simulationInit() {}
-
-  /** This function is called periodically whilst in simulation. */
-  @Override
-  public void simulationPeriodic() {}
 }

src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java

@@ -95,12 +95,15 @@ public CommandSwerveDrivetrain(
 
   /** Toggles the field oriented boolean. */
   public Command toggleFieldOriented() {
-    return brake().withTimeout(2).alongWith(runOnce(() -> _isFieldOriented = !_isFieldOriented));
+    return brake()
+        .withTimeout(0.5)
+        .andThen(runOnce(() -> _isFieldOriented = !_isFieldOriented))
+        .withName("Toggle Field Oriented");
   }
 
   /** Brakes the swerve drive (module form an "X" formation). */
   public Command brake() {
-    return run(() -> setControl(_brakeRequest));
+    return run(() -> setControl(_brakeRequest)).withName("Brake");
   }
 
   /**
@@ -114,11 +117,6 @@ public Command drive(InputStream velX, InputStream velY, InputStream velOmega) {
     return run(() -> {
           drive(velX.get(), velY.get(), velOmega.get());
         })
-        .beforeStarting(
-            () -> {
-              _isFieldOriented = true;
-              _isOpenLoop = true;
-            })
         .withName("Drive");
   }