Controls update

src/main/java/frc/robot/Controls.java

@@ -5,15 +5,16 @@
 package frc.robot;
 
 import static edu.wpi.first.units.Units.MetersPerSecond;
-import static edu.wpi.first.units.Units.RadiansPerSecond;
-import static edu.wpi.first.units.Units.RotationsPerSecond;
 
 import com.ctre.phoenix6.swerve.SwerveModule.DriveRequestType;
 import com.ctre.phoenix6.swerve.SwerveRequest;
 import edu.wpi.first.math.MathUtil;
+import edu.wpi.first.wpilibj2.command.Commands;
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
 import edu.wpi.first.wpilibj2.command.button.Trigger;
 import frc.robot.generated.AlphaTunerConstants;
+import frc.robot.util.GetTargetFromPose;
+import frc.robot.util.tuning.WheelRadiusCharacterization;
 
 /**
  * This class is where the bulk of the robot should be declared. Since Command-based is a
@@ -24,37 +25,26 @@
 public class Controls {
 
   private static final int DRIVER_CONTROLLER_PORT = 0;
+  private static final int TEST_CONTROLLER_PORT = 1;
   private static final double JOYSTICK_DEADBAND = 0.1;
   private final Subsystems s;
   private static final double SWERVE_DEADBAND = 0.001;
+  private final SwerveRequest.SwerveDriveBrake brake = new SwerveRequest.SwerveDriveBrake();
 
   private final CommandXboxController driverController =
       new CommandXboxController(DRIVER_CONTROLLER_PORT);
 
-  public static final double MaxSpeed = AlphaTunerConstants.kSpeedAt12Volts.in(MetersPerSecond);
+  private final CommandXboxController testController =
+      new CommandXboxController(TEST_CONTROLLER_PORT);
 
-  // kSpeedAt12Volts desired top speed
-  public static double MaxAngularRate =
-      RotationsPerSecond.of(0.75)
-          .in(RadiansPerSecond); // 3/4 of a rotation per second max angular velocity
+  public static final double MaxSpeed = AlphaTunerConstants.kSpeedAt12Volts.in(MetersPerSecond);
 
   public Controls(Subsystems subsystems) {
     s = subsystems;
     // Configure the trigger bindings
-    configureBindings();
+    configureDrivebaseBindings();
   }
 
-  /**
-   * Use this method to define your trigger->command mappings. Triggers can be created via the
-   * {@link Trigger#Trigger(java.util.function.BooleanSupplier)} constructor with an arbitrary
-   * predicate, or via the named factories in {@link
-   * edu.wpi.first.wpilibj2.command.button.CommandGenericHID}'s subclasses for {@link
-   * CommandXboxController Xbox}/{@link edu.wpi.first.wpilibj2.command.button.CommandPS4Controller
-   * PS4} controllers or {@link edu.wpi.first.wpilibj2.command.button.CommandJoystick Flight
-   * joysticks}.
-   */
-  private void configureBindings() {}
-
   // takes the X value from the joystick, and applies a deadband and input scaling
   private double getDriveX() {
     // Joystick +Y is back
@@ -63,6 +53,10 @@ private double getDriveX() {
     return input * MaxSpeed;
   }
 
+  private Trigger connected(CommandXboxController controller) {
+    return new Trigger(() -> controller.isConnected());
+  }
+
   // takes the Y value from the joystick, and applies a deadband and input scaling
   private double getDriveY() {
     // Joystick +X is right
@@ -86,4 +80,46 @@ private double getDriveRotate() {
           .withDeadband(SWERVE_DEADBAND)
           .withRotationalDeadband(SWERVE_DEADBAND)
           .withDriveRequestType(DriveRequestType.Velocity);
+
+  private void configureDrivebaseBindings() {
+    if (s.drivebaseSubsystem == null) {
+      // Stop running this method
+      return;
+    }
+
+    // readyToShoot = GetTargetFromPose.autoShoot(s.drivebaseSubsystem);
+
+    connected(testController)
+        .and(testController.y())
+        .whileTrue(
+            WheelRadiusCharacterization.wheelRadiusCharacterizationCommand(s.drivebaseSubsystem));
+    // Note that X is defined as forward according to WPILib convention,
+    // and Y is defined as to the left according to WPILib convention.
+
+    // the driving command for just driving around
+    s.drivebaseSubsystem.setDefaultCommand(
+        // s.drivebaseSubsystem will execute this command periodically
+
+        // applying the request to drive with the inputs
+        s.drivebaseSubsystem
+            .applyRequest(
+                () ->
+                    drive
+                        .withVelocityX(getDriveX())
+                        .withVelocityY(getDriveY())
+                        .withRotationalRate(getDriveRotate()))
+            .withName("Drive"));
+
+    driverController
+        .a()
+        .whileTrue(Commands.run(() -> s.drivebaseSubsystem.setControl(brake)).withName("Brake"));
+
+    // reset pose incase vision is bugging
+    driverController
+        .back()
+        .onTrue(
+            s.drivebaseSubsystem
+                .runOnce(() -> s.drivebaseSubsystem.resetPose(GetTargetFromPose.getRestPose()))
+                .withName("Reset to Hub"));
+  }
 }