add operator input, fix drive commands

src/main/java/frc/robot/OperatorInput.java

@@ -0,0 +1,74 @@
+package frc.robot;
+
+import edu.wpi.first.math.MathUtil;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.wpilibj.XboxController;
+import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
+import edu.wpi.first.wpilibj2.command.button.Trigger;
+
+public class OperatorInput {
+  final CommandXboxController operatorControl = new CommandXboxController(1);
+  public final CommandXboxController driver = new CommandXboxController(0);
+
+  public boolean actuallyIsTeleop = false;
+  final Trigger isTeleop = new Trigger(() -> actuallyIsTeleop); // TODO fill this out
+
+  // DRIVER'S CONTROLS
+  public final Trigger slowModeHold = driver.leftTrigger();
+  public final Trigger turboModeHold = driver.rightTrigger();
+
+  final Trigger resetGyroPress = driver.start();
+
+  final Trigger xNotDesired =
+      driver
+          .axisGreaterThan(XboxController.Axis.kLeftX.value, 0.1)
+          .or(driver.axisLessThan(XboxController.Axis.kLeftX.value, -0.1))
+          .negate();
+  final Trigger yNotDesired =
+      driver
+          .axisGreaterThan(XboxController.Axis.kLeftY.value, 0.1)
+          .or(driver.axisLessThan(XboxController.Axis.kLeftY.value, -0.1))
+          .negate();
+  final Trigger thetaNotDesired =
+      driver
+          .axisGreaterThan(XboxController.Axis.kRightX.value, 0.1)
+          .or(driver.axisLessThan(XboxController.Axis.kRightX.value, -0.1))
+          .negate();
+  public final Trigger movementNotDesired = xNotDesired.and(yNotDesired).and(thetaNotDesired);
+  public final Trigger movementDesired = movementNotDesired.negate();
+
+  /**
+   * @param input a value
+   * @return that value, deadbanded
+   */
+  static double deadband(double input) {
+    double value = input;
+
+    value = MathUtil.applyDeadband(value, 0.1);
+    value = Math.copySign(value * value, value);
+
+    return value;
+  }
+
+  public double getDriverRightComponentRaw() {
+    return -driver.getLeftX(); // reference frame stuff
+  }
+
+  public double getRobotForwardComponentRaw() {
+    return -driver.getLeftY(); // reference frame stuff
+  }
+
+  public double getRobotRotationRaw() {
+    return driver.getRightX();
+  }
+
+  public Rotation2d getDriverRightAsAngle() {
+    double rotZeroToOne = (driver.getRightX() + 1) % 1;
+
+    return Rotation2d.fromRotations(rotZeroToOne);
+  }
+
+  public double getDriverAngularComponentRaw() {
+    return deadband(-driver.getRightX());
+  }
+}

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

@@ -66,8 +66,6 @@ public void robotInit() {
     }
 
     Logger.start();
-
-    robotContainer = new RobotContainer();
   }
 
   @Override

src/main/java/frc/robot/RobotContainer.java

@@ -14,12 +14,9 @@
 package frc.robot;
 
 import com.pathplanner.lib.auto.AutoBuilder;
-import edu.wpi.first.math.geometry.Pose2d;
-import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.wpilibj.GenericHID;
 import edu.wpi.first.wpilibj.XboxController;
 import edu.wpi.first.wpilibj2.command.Command;
-import edu.wpi.first.wpilibj2.command.Commands;
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
 import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine;
 import frc.robot.commands.DriveCommands;
@@ -36,6 +33,7 @@ public class RobotContainer {
   // Subsystems
   private final Drive drive;
   // private final Flywheel flywheel;
+  public final OperatorInput operatorInput;
 
   // Controller
   private final CommandXboxController controller = new CommandXboxController(0);
@@ -47,6 +45,9 @@ public class RobotContainer {
 
   /** The container for the robot. Contains subsystems, OI devices, and commands. */
   public RobotContainer() {
+
+    this.operatorInput = new OperatorInput();
+
     switch (Constants.currentMode) {
       case REAL:
         // Real robot, instantiate hardware IO implementations
@@ -112,7 +113,7 @@ public RobotContainer() {
         "Drive SysId (Dynamic Reverse)", drive.sysIdDynamic(SysIdRoutine.Direction.kReverse));
 
     // Configure the button bindings
-    configureButtonBindings();
+    loadCommands();
   }
 
   /**
@@ -121,24 +122,14 @@ public RobotContainer() {
    * edu.wpi.first.wpilibj.Joystick} or {@link XboxController}), and then passing it to a {@link
    * edu.wpi.first.wpilibj2.command.button.JoystickButton}.
    */
-  private void configureButtonBindings() {
-    drive.setDefaultCommand(
-        DriveCommands.joystickDrive(
+  void loadCommands() {
+    operatorInput.movementDesired.whileTrue(
+        DriveCommands.BaseDriveCommand(
             drive,
             () -> -controller.getLeftY(),
             () -> -controller.getLeftX(),
             () -> -controller.getRightX()));
-    controller.x().onTrue(Commands.runOnce(drive::stopWithX, drive));
-    controller
-        .b()
-        .onTrue(
-            Commands.runOnce(
-                    () ->
-                        drive.setPose(
-                            new Pose2d(drive.getPose().getTranslation(), new Rotation2d())),
-                    drive)
-                .ignoringDisable(true));
-  }
+  } // TODO FIX COMMAND THIS WILL BREAK DO NOT RUN IT
 
   /**
    * Use this to pass the autonomous command to the main {@link Robot} class.

src/main/java/frc/robot/commands/DriveCommands.java

@@ -34,7 +34,7 @@ private DriveCommands() {}
   /**
    * Field relative drive command using two joysticks (controlling linear and angular velocities).
    */
-  public static Command joystickDrive(
+  public static Command BaseDriveCommand(
       Drive drive,
       DoubleSupplier xSupplier,
       DoubleSupplier ySupplier,