Merge branch 'swerve' of https://github.com/Team4388/2022NoWayHome into swerve

.vscode/settings.json

@@ -26,5 +26,5 @@
     },
   ],
   "java.test.defaultConfig": "WPIlibUnitTests",
-  "java.dependency.packagePresentation": "flat"
+  "java.dependency.packagePresentation": "hierarchical"
 }

simgui-ds.json

@@ -88,5 +88,10 @@
       "buttonCount": 0,
       "povCount": 0
     }
+  ],
+  "robotJoysticks": [
+    {
+      "guid": "Keyboard0"
+    }
   ]
 }

simgui.json

@@ -1,11 +1,38 @@
 {
+  "HALProvider": {
+    "Other Devices": {
+      "Talon FX[2]": {
+        "header": {
+          "open": true
+        }
+      },
+      "Talon FX[3]": {
+        "header": {
+          "open": true
+        }
+      },
+      "Talon FX[4]": {
+        "header": {
+          "open": true
+        }
+      }
+    }
+  },
   "NTProvider": {
     "types": {
       "/FMSInfo": "FMSInfo",
       "/LiveWindow/LED": "Subsystem",
       "/LiveWindow/SwerveDrive": "Subsystem",
       "/LiveWindow/SwerveModule": "Subsystem",
-      "/LiveWindow/Ungrouped/Scheduler": "Scheduler"
+      "/LiveWindow/Ungrouped/Scheduler": "Scheduler",
+      "/SmartDashboard/Field": "Field2d"
+    },
+    "windows": {
+      "/SmartDashboard/Field": {
+        "window": {
+          "visible": true
+        }
+      }
     }
   },
   "NetworkTables": {

src/main/java/frc4388/robot/Constants.java

@@ -102,5 +102,6 @@ public final class Constants {
   public static final class OIConstants {
     public static final int XBOX_DRIVER_ID = 0;
     public static final int XBOX_OPERATOR_ID = 1;
+    public static final double AXIS_DEADBAND = 0.1;
   }
 }

src/main/java/frc4388/robot/Robot.java

@@ -4,6 +4,7 @@
 
 package frc4388.robot;
 
+import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj.TimedRobot;
 import edu.wpi.first.wpilibj.livewindow.LiveWindow;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
@@ -115,6 +116,7 @@ public class Robot extends TimedRobot {
       m_autonomousCommand.cancel();
     }
     m_robotTime.startMatchTime();
+    DriverStation.silenceJoystickConnectionWarning(true);
   }
 
   /**

src/main/java/frc4388/robot/RobotContainer.java

@@ -18,6 +18,7 @@ import edu.wpi.first.math.trajectory.Trajectory;
 import edu.wpi.first.math.trajectory.TrajectoryConfig;
 import edu.wpi.first.math.trajectory.TrapezoidProfile;
 import edu.wpi.first.wpilibj.Joystick;
+import edu.wpi.first.wpilibj.XboxController;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.InstantCommand;
 import edu.wpi.first.wpilibj2.command.RamseteCommand;
@@ -29,8 +30,7 @@ import frc4388.robot.Constants.*;
 import frc4388.robot.subsystems.LED;
 import frc4388.robot.subsystems.SwerveDrive;
 import frc4388.utility.LEDPatterns;
-import frc4388.utility.controller.IHandController;
-import frc4388.utility.controller.XboxController;
+import frc4388.utility.controller.DeadbandedXboxController;
 
 /**
  * This class is where the bulk of the robot should be declared. Since
@@ -50,8 +50,8 @@ public class RobotContainer {
   private final LED m_robotLED = new LED(m_robotMap.LEDController);
 
   /* Controllers */
-  private final XboxController m_driverXbox = new XboxController(OIConstants.XBOX_DRIVER_ID);
-  private final XboxController m_operatorXbox = new XboxController(OIConstants.XBOX_OPERATOR_ID);
+  private final XboxController m_driverXbox = new DeadbandedXboxController(OIConstants.XBOX_DRIVER_ID);
+  private final XboxController m_operatorXbox = new DeadbandedXboxController(OIConstants.XBOX_OPERATOR_ID);
 
   /**
    * The container for the robot. Contains subsystems, OI devices, and commands.
@@ -62,10 +62,10 @@ public class RobotContainer {
     // drives the swerve drive with a two-axis input from the driver controller
     m_robotSwerveDrive.setDefaultCommand(
         new RunCommand(() -> m_robotSwerveDrive.driveWithInput(
-          XboxController.ClampJoystickAxis(
-              getDriverController().getLeftXAxis(),
-              getDriverController().getLeftYAxis()), 
-          -getDriverController().getRightXAxis(), 
+          clampJoystickAxes(
+              getDriverController().getLeftX(),
+              getDriverController().getLeftY()), 
+          -getDriverController().getRightX(), 
           true),
            m_robotSwerveDrive));
 
@@ -81,26 +81,27 @@ public class RobotContainer {
    */
   private void configureButtonBindings() {
     /* Driver Buttons */
-    new JoystickButton(getDriverJoystick(), XboxController.Y_BUTTON)
+    new JoystickButton(getDriverController(), XboxController.Button.kY.value)
     // new XboxControllerRawButton(m_driverXbox, XboxControllerRaw.Y_BUTTON)
       .whenPressed(() -> m_robotSwerveDrive.m_gyro.reset());
 
-    new JoystickButton(getDriverJoystick(), XboxController.LEFT_BUMPER_BUTTON)
+    new JoystickButton(getDriverController(), XboxController.Button.kLeftBumper.value)
     // new XboxControllerRawButton(m_driverXbox, XboxControllerRaw.LEFT_BUMPER_BUTTON)
       .whenPressed(() -> m_robotSwerveDrive.highSpeed(false));
 
 
-    new JoystickButton(getDriverJoystick(), XboxController.RIGHT_BUMPER_BUTTON)
+    new JoystickButton(getDriverController(), XboxController.Button.kRightBumper.value)
     // new XboxControllerRawButton(m_driverXbox, XboxControllerRaw.RIGHT_BUMPER_BUTTON)
       .whenPressed(() -> m_robotSwerveDrive.highSpeed(true));
     
-    new JoystickButton(getDriverJoystick(), XboxController.A_BUTTON)
+    new JoystickButton(getDriverController(), XboxController.Button.kA.value)
 
       .whenPressed(() -> zeroOdometry(new Pose2d(0, 0, new Rotation2d(0))));
+      //.whenPressed(this::resetOdometry);
 
     /* Operator Buttons */
     // activates "Lit Mode"
-    new JoystickButton(getOperatorJoystick(), XboxController.A_BUTTON)
+    new JoystickButton(getOperatorController(), XboxController.Button.kA.value)
     // new XboxControllerRawButton(m_driverXbox, XboxControllerRaw.A_BUTTON)
         .whenPressed(() -> m_robotLED.setPattern(LEDPatterns.LAVA_RAINBOW))
         .whenReleased(() -> m_robotLED.setPattern(LEDConstants.DEFAULT_PATTERN));
@@ -152,7 +153,7 @@ public class RobotContainer {
   /**
    * Add your docs here.
    */
-  public IHandController getDriverController() {
+  public XboxController getDriverController() {
     return m_driverXbox;
   }
 
@@ -166,21 +167,16 @@ public class RobotContainer {
   /**
    * Add your docs here.
    */
-  public IHandController getOperatorController() {
+  public XboxController getOperatorController() {
     return m_operatorXbox;
   }
 
-  /**
-   * Add your docs here.
-   */
-  public Joystick getOperatorJoystick() {
-    return m_operatorXbox.getJoyStick();
-  }
-
-  /**
-   * Add your docs here.
-   */
-  public Joystick getDriverJoystick() {
-    return m_driverXbox.getJoyStick();
+  public static double[] clampJoystickAxes(double x, double y) {
+    double square_mag = x * x + y * y;
+    if (square_mag > 1.d) {
+      double mag = Math.sqrt(square_mag);
+      return new double[] { x / mag, y / mag };
+    }
+    return new double[] { x, y };
   }
 }

src/main/java/frc4388/robot/subsystems/SwerveDrive.java

@@ -18,6 +18,7 @@ import edu.wpi.first.math.kinematics.ChassisSpeeds;
 import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
 import edu.wpi.first.math.kinematics.SwerveModuleState;
 import edu.wpi.first.math.util.Units;
+import edu.wpi.first.wpilibj.smartdashboard.Field2d;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc4388.robot.Constants.SwerveDriveConstants;
@@ -53,6 +54,8 @@ public class SwerveDrive extends SubsystemBase {
   public double speedAdjust = SwerveDriveConstants.JOYSTICK_TO_METERS_PER_SECOND_SLOW;
   public boolean ignoreAngles;
 
+  private final Field2d m_field = new Field2d();
+
   public SwerveDrive(SwerveModule leftFront, SwerveModule leftBack, SwerveModule rightFront, SwerveModule rightBack, WPI_PigeonIMU gyro) {
       // m_leftFrontSteerMotor = leftFrontSteerMotor;
       // m_leftFrontWheelMotor = leftFrontWheelMotor;
@@ -89,7 +92,8 @@ public class SwerveDrive extends SubsystemBase {
           VecBuilder.fill(0.05, 0.05, Units.degreesToRadians(5)),
           VecBuilder.fill(Units.degreesToRadians(0.01)),
           VecBuilder.fill(0.5, 0.5, Units.degreesToRadians(30)));
-      m_gyro.reset(); 
+    m_gyro.reset(); 
+    SmartDashboard.putData("Field", m_field);
   }
 //https://github.com/ZachOrr/MK3-Swerve-Example
  /**
@@ -140,6 +144,7 @@ public class SwerveDrive extends SubsystemBase {
     // m_gyro.setStatusFramePeriod(PigeonIMU_StatusFrame.CondStatus_6_SensorFusion, 1, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
     // m_gyro.setStatusFramePeriod(PigeonIMU_StatusFrame.CondStatus_1_General, 1, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
 
+    m_field.setRobotPose(m_poseEstimator.getEstimatedPosition());
     super.periodic();
   }
 

src/main/java/frc4388/utility/controller/DeadbandedRawXboxController.java

@@ -0,0 +1,13 @@
+package frc4388.utility.controller;
+
+import edu.wpi.first.math.MathUtil;
+import frc4388.robot.Constants;
+
+public class DeadbandedRawXboxController extends RawXboxController {
+  public DeadbandedRawXboxController(int port) { super(port); }
+  @Override public double getLeftX() { return applyDeadband(super.getLeftX()); }
+  @Override public double getLeftY() { return applyDeadband(super.getLeftY()); }
+  @Override public double getRightX() { return applyDeadband(super.getRightX()); }
+  @Override public double getRightY() { return applyDeadband(super.getRightY()); }
+  private static double applyDeadband(double value) { return MathUtil.applyDeadband(value, Constants.OIConstants.AXIS_DEADBAND); }
+}

src/main/java/frc4388/utility/controller/DeadbandedXboxController.java

@@ -0,0 +1,14 @@
+package frc4388.utility.controller;
+
+import edu.wpi.first.math.MathUtil;
+import edu.wpi.first.wpilibj.XboxController;
+import frc4388.robot.Constants;
+
+public class DeadbandedXboxController extends XboxController {
+  public DeadbandedXboxController(int port) { super(port); }
+  @Override public double getLeftX() { return applyDeadband(super.getLeftX()); }
+  @Override public double getLeftY() { return applyDeadband(super.getLeftY()); }
+  @Override public double getRightX() { return applyDeadband(super.getRightX()); }
+  @Override public double getRightY() { return applyDeadband(super.getRightY()); }
+  private static double applyDeadband(double value) { return MathUtil.applyDeadband(value, Constants.OIConstants.AXIS_DEADBAND); }
+}

src/main/java/frc4388/utility/controller/IHandController.java

@@ -1,23 +0,0 @@
-package frc4388.utility.controller;
-
-/**
- * Add your docs here.
- */
-public interface IHandController {
-
-  public double getLeftXAxis();
-
-  public double getLeftYAxis();
-
-  public double getRightXAxis();
-
-  public double getRightYAxis();
-
-  public double getLeftTriggerAxis();
-
-  public double getRightTriggerAxis();
-
-  public int getDpadAngle();
-
-  public void updateInput();
-}

src/main/java/frc4388/utility/controller/RawXboxController.java

@@ -0,0 +1,241 @@
+package frc4388.utility.controller;
+
+import java.nio.ByteBuffer;
+
+import edu.wpi.first.hal.HAL;
+import edu.wpi.first.wpilibj.DriverStation;
+import edu.wpi.first.wpilibj.Timer;
+import edu.wpi.first.wpilibj.XboxController;
+
+public class RawXboxController extends XboxController {
+  private final int m_port;
+
+  public RawXboxController(int port) {
+    super(port);
+    if (port < 0 || port >= DriverStation.kJoystickPorts)
+      throw new IllegalArgumentException("Joystick index is out of range, should be 0-5");
+    m_port = port;
+  }
+
+  private static class HALJoystickButtons {
+    public int m_buttons;
+    public byte m_count;
+  }
+
+  private static class HALJoystickAxes {
+    public float[] m_axes;
+    public short m_count;
+
+    HALJoystickAxes(int count) {
+      m_axes = new float[count];
+    }
+  }
+
+  private static class HALJoystickPOVs {
+    public short[] m_povs;
+    public short m_count;
+
+    HALJoystickPOVs(int count) {
+      m_povs = new short[count];
+      for (int i = 0; i < count; i++) {
+        m_povs[i] = -1;
+      }
+    }
+  }
+
+  private static final double JOYSTICK_UNPLUGGED_MESSAGE_INTERVAL = 1.0;
+  private static double m_nextMessageTime;
+
+  private HALJoystickAxes m_joystickAxes = new HALJoystickAxes(HAL.kMaxJoystickAxes);
+  private HALJoystickPOVs m_joystickPOVs = new HALJoystickPOVs(HAL.kMaxJoystickAxes);
+  private HALJoystickButtons m_joystickButtons = new HALJoystickButtons();
+
+  // Joystick button rising/falling edge flags
+  private int m_joystickButtonsPressed = 0;
+  private int m_joystickButtonsReleased = 0;
+
+  private static final ByteBuffer m_buttonCountBuffer = ByteBuffer.allocateDirect(1);
+
+  @Override
+  public double getRawAxis(int axis) {
+    return getStickAxis(axis);
+  }
+
+  @Override
+  public boolean getRawButton(int button) {
+    return getStickButton((byte) button);
+  }
+
+  @Override
+  public boolean getRawButtonPressed(int button) {
+    return getStickButtonPressed((byte) button);
+  }
+
+  @Override
+  public boolean getRawButtonReleased(int button) {
+    return getStickButtonReleased(button);
+  }
+
+  @Override
+  public int getPOV(int pov) {
+    return getStickPOV(pov);
+  }
+
+  /**
+   * The state of one joystick button. Button indexes begin at 1.
+   *
+   * @param button The button index, beginning at 1.
+   * @return The state of the joystick button.
+   */
+  public boolean getStickButton(final int button) {
+    if (button <= 0) {
+      reportJoystickUnpluggedError();
+      return false;
+    }
+    if (button <= m_joystickButtons.m_count) {
+      return (m_joystickButtons.m_buttons & 1 << (button - 1)) != 0;
+    }
+    reportJoystickUnpluggedWarning(button, m_port);
+    return false;
+  }
+
+  /**
+   * Whether one joystick button was pressed since the last check. Button indexes begin at 1.
+   *
+   * @param button The button index, beginning at 1.
+   * @return Whether the joystick button was pressed since the last check.
+   */
+  public boolean getStickButtonPressed(final int button) {
+    getData();
+    if (button <= 0) {
+      reportJoystickUnpluggedError();
+      return false;
+    }
+    if (button <= m_joystickButtons.m_count) {
+      // If button was pressed, clear flag and return true
+      if ((m_joystickButtonsPressed & 1 << (button - 1)) != 0) {
+        m_joystickButtonsPressed &= ~(1 << (button - 1));
+        return true;
+      } else {
+        return false;
+      }
+    }
+    reportJoystickUnpluggedWarning(button, m_port);
+    return false;
+  }
+
+  /**
+   * Whether one joystick button was released since the last check. Button indexes begin at 1.
+   *
+   * @param button The button index, beginning at 1.
+   * @return Whether the joystick button was released since the last check.
+   */
+  public boolean getStickButtonReleased(final int button) {
+    getData();
+    if (button <= 0) {
+      reportJoystickUnpluggedError();
+      return false;
+    }
+    if (button <= m_joystickButtons.m_count) {
+      // If button was released, clear flag and return true
+      if ((m_joystickButtonsReleased & 1 << (button - 1)) != 0) {
+        m_joystickButtonsReleased &= ~(1 << (button - 1));
+        return true;
+      } else {
+        return false;
+      }
+    }
+    reportJoystickUnpluggedWarning(button, m_port);
+    return false;
+  }
+
+  /**
+   * Get the value of the axis on a joystick. This depends on the mapping of the joystick connected to
+   * the specified port.
+   *
+   * @param axis The analog axis value to read from the joystick.
+   * @return The value of the axis on the joystick.
+   */
+  public double getStickAxis(int axis) {
+    getData();
+    if (axis < 0 || axis >= HAL.kMaxJoystickAxes) {
+      throw new IllegalArgumentException("Joystick axis is out of range");
+    }
+    if (axis < m_joystickAxes.m_count) {
+      return m_joystickAxes.m_axes[axis];
+    }
+    reportJoystickUnpluggedWarning(axis, m_port);
+    return 0.0;
+  }
+
+  /**
+   * Get the state of a POV on the joystick.
+   *
+   * @param pov The POV to read.
+   * @return the angle of the POV in degrees, or -1 if the POV is not pressed.
+   */
+  public int getStickPOV(int pov) {
+    getData();
+    if (pov < 0 || pov >= HAL.kMaxJoystickPOVs) {
+      throw new IllegalArgumentException("Joystick POV is out of range");
+    }
+    if (pov < m_joystickPOVs.m_count) {
+      return m_joystickPOVs.m_povs[pov];
+    }
+    reportJoystickUnpluggedWarning(pov, m_port);
+    return -1;
+  }
+
+  /**
+   * The state of the buttons on the joystick.
+   *
+   * @return The state of the buttons on the joystick.
+   */
+  public int getStickButtons() {
+    getData();
+    return m_joystickButtons.m_buttons;
+  }
+
+  protected void getData() {
+    // Get the status of all of the joysticks
+    byte stick = (byte) m_port;
+    m_joystickAxes.m_count = HAL.getJoystickAxes(stick, m_joystickAxes.m_axes);
+    m_joystickPOVs.m_count = HAL.getJoystickPOVs(stick, m_joystickPOVs.m_povs);
+    m_joystickButtons.m_buttons = HAL.getJoystickButtons(stick, m_buttonCountBuffer);
+    m_joystickButtons.m_count = m_buttonCountBuffer.get(0);
+  }
+
+  /**
+   * Reports errors related to unplugged joysticks Throttles the errors so that they don't overwhelm
+   * the DS.
+   */
+  private static void reportJoystickUnpluggedError(String message) {
+    double currentTime = Timer.getFPGATimestamp();
+    if (currentTime > m_nextMessageTime) {
+      DriverStation.reportError(message, false);
+      m_nextMessageTime = currentTime + JOYSTICK_UNPLUGGED_MESSAGE_INTERVAL;
+    }
+  }
+
+  /**
+   * Reports errors related to unplugged joysticks Throttles the errors so that they don't overwhelm
+   * the DS.
+   */
+  private static void reportJoystickUnpluggedWarning(String message) {
+    if (DriverStation.isFMSAttached() || !DriverStation.isJoystickConnectionWarningSilenced()) {
+      double currentTime = Timer.getFPGATimestamp();
+      if (currentTime > m_nextMessageTime) {
+        DriverStation.reportWarning(message, false);
+        m_nextMessageTime = currentTime + JOYSTICK_UNPLUGGED_MESSAGE_INTERVAL;
+      }
+    }
+  }
+
+  private static void reportJoystickUnpluggedWarning(final int pov, final int stick) {
+    reportJoystickUnpluggedWarning("Joystick POV " + pov + " on port " + stick + " not available, check if controller is plugged in");
+  }
+
+  private static void reportJoystickUnpluggedError() {
+    reportJoystickUnpluggedError("Button indexes begin at 1 in WPILib for C++ and Java\n");
+  }
+}

src/main/java/frc4388/utility/controller/XboxController.java

@@ -1,233 +0,0 @@
-package frc4388.utility.controller;
-
-import edu.wpi.first.wpilibj.Joystick;
-
-/**
- * This is a wrapper for the WPILib Joystick class that represents an XBox 
- * controller.
- * @author frc1675
- */
-public class XboxController implements IHandController
-{
-  public static final int LEFT_X_AXIS = 0;
-  public static final int LEFT_Y_AXIS = 1;
-  public static final int LEFT_TRIGGER_AXIS = 2;
-  public static final int RIGHT_TRIGGER_AXIS = 3;
-  public static final int RIGHT_X_AXIS = 4;
-  public static final int RIGHT_Y_AXIS = 5;
-  public static final int LEFT_RIGHT_DPAD_AXIS = 6;
-  public static final int TOP_BOTTOM_DPAD_AXIS = 6;
-
-  public static final int A_BUTTON = 1;
-  public static final int B_BUTTON = 2;
-  public static final int X_BUTTON = 3;
-  public static final int Y_BUTTON = 4;
-  public static final int LEFT_BUMPER_BUTTON = 5;
-  public static final int RIGHT_BUMPER_BUTTON = 6;
-  public static final int BACK_BUTTON = 7;
-  public static final int START_BUTTON = 8;
-
-  public static final int LEFT_JOYSTICK_BUTTON = 9;
-  public static final int RIGHT_JOYSTICK_BUTTON = 10;
-
-  private static final double LEFT_DPAD_TOLERANCE = -0.9;
-  private static final double RIGHT_DPAD_TOLERANCE = 0.9;
-  private static final double BOTTOM_DPAD_TOLERANCE = -0.9;
-  private static final double TOP_DPAD_TOLERANCE = 0.9;
-
-  private static final double LEFT_TRIGGER_TOLERANCE = 0.5;
-  private static final double RIGHT_TRIGGER_TOLERANCE = 0.5;
-
-  private static final double RIGHT_AXIS_UP_TOLERANCE = -0.9;
-  private static final double RIGHT_AXIS_DOWN_TOLERANCE = 0.9;
-  private static final double RIGHT_AXIS_RIGHT_TOLERANCE = 0.9;
-  private static final double RIGHT_AXIS_LEFT_TOLERANCE = -0.9;
-
-  private static final double LEFT_AXIS_UP_TOLERANCE = -0.9;
-  private static final double LEFT_AXIS_DOWN_TOLERANCE = 0.9;
-  private static final double LEFT_AXIS_RIGHT_TOLERANCE = 0.9;
-  private static final double LEFT_AXIS_LEFT_TOLERANCE = -0.9;
-
-  private static final double DEADZONE = 0.2;
-
-  private Joystick m_stick;
-
-  public void updateInput() {}
-
-  public static double[] ClampJoystickAxis(double x, double y) {
-    double square_mag = x * x + y * y;
-    double[] ret = {x,y};
-    if (square_mag > 1.d) {
-      double mag = Math.sqrt(square_mag);
-      ret[0] = x / mag;
-      ret[1] = y / mag;
-    }
-    //double [] to_smart_dashboard = {square_mag, x, y, ret[0], ret[1]};
-    //SmartDashboard.putNumberArray("Input", to_smart_dashboard);
-    return ret;
-  }
-
-  /**
-     * Add your docs here.
-     */
-  public XboxController(int portNumber){
-    m_stick = new Joystick(portNumber);
-  }
-
-  /**
-     * Add your docs here.
-     */
-  public Joystick getJoyStick() {
-    return m_stick;
-  }
-  
-  /**
-   * Checks if the input falls within the deadzone.
-   * @param input from an axis on the controller
-   * @return true if input falls in deadzone, false if input falls outside deadzone
-   */
-  public static boolean inDeadZone(double input){
-    return (Math.abs(input) < DEADZONE);
-  }
-
-  /**
-   * Updates an input to have a deadzone around the 0 position
-   * @param input from an axis on the controller
-   * @return updated input
-   */
-  private double getAxisWithDeadZoneCheck(double input){
-    if(inDeadZone(input)){
-      return 0.0;       
-    } else {
-      return input;
-    }
-  }
-
-  public boolean getAButton(){
-    return m_stick.getRawButton(A_BUTTON);
-  }
-
-  public boolean getXButton(){
-    return m_stick.getRawButton(X_BUTTON);    
-  }
-
-  public boolean getBButton(){
-    return m_stick.getRawButton(B_BUTTON);
-  }
-
-  public boolean getYButton(){
-    return m_stick.getRawButton(Y_BUTTON);
-  } 
-
-  public boolean getBackButton(){
-    return m_stick.getRawButton(BACK_BUTTON);
-  }
-
-  public boolean getStartButton(){
-    return m_stick.getRawButton(START_BUTTON);
-  } 
-
-  public boolean getLeftBumperButton(){
-    return m_stick.getRawButton(LEFT_BUMPER_BUTTON);
-  }
-
-  public boolean getRightBumperButton(){
-    return m_stick.getRawButton(RIGHT_BUMPER_BUTTON);
-  }
-
-  public boolean getLeftJoystickButton(){
-    return m_stick.getRawButton(LEFT_JOYSTICK_BUTTON);
-  }
-
-  public boolean getRightJoystickButton(){
-    return m_stick.getRawButton(RIGHT_JOYSTICK_BUTTON);
-  }
-
-  public double getLeftXAxis(){         
-    return getAxisWithDeadZoneCheck(m_stick.getRawAxis(LEFT_X_AXIS)); 
-  }
-
-  public double getLeftYAxis(){
-    return getAxisWithDeadZoneCheck(m_stick.getRawAxis(LEFT_Y_AXIS));
-  }
-
-  public double getRightXAxis(){
-    return getAxisWithDeadZoneCheck(m_stick.getRawAxis(RIGHT_X_AXIS));
-  }
-
-  public double getRightYAxis(){
-    return getAxisWithDeadZoneCheck(m_stick.getRawAxis(RIGHT_Y_AXIS)); 
-  }
-
-  public double getLeftTriggerAxis(){
-    return getAxisWithDeadZoneCheck(m_stick.getRawAxis(LEFT_TRIGGER_AXIS)); 
-  }
-
-  public double getRightTriggerAxis(){
-    return getAxisWithDeadZoneCheck(m_stick.getRawAxis(RIGHT_TRIGGER_AXIS)); 
-  }
-
-  /**
-   * Get the angle input from the dpad.
-   * @return -1 if nothing is pressed, or the angle of the button pressed. 0 = up, 90 = right, etc.
-   */
-  public int getDpadAngle() {
-    return m_stick.getPOV(0);
-  }
-
-  public boolean getDPadLeft(){
-      return (m_stick.getRawAxis(LEFT_RIGHT_DPAD_AXIS) < LEFT_DPAD_TOLERANCE);
-    }
-
-    public boolean getDPadRight(){
-      return (m_stick.getRawAxis(LEFT_RIGHT_DPAD_AXIS) > RIGHT_DPAD_TOLERANCE);
-    }
-
-    public boolean getDPadTop(){
-      return (m_stick.getRawAxis(TOP_BOTTOM_DPAD_AXIS) < TOP_DPAD_TOLERANCE);
-    }
-
-    public boolean getDPadBottom(){
-      return (m_stick.getRawAxis(TOP_BOTTOM_DPAD_AXIS) > BOTTOM_DPAD_TOLERANCE);
-    }
-
-  public boolean getLeftTrigger(){
-    return (getLeftTriggerAxis() > LEFT_TRIGGER_TOLERANCE);
-  }
-
-  public boolean getRightTrigger(){
-    return (getRightTriggerAxis() > RIGHT_TRIGGER_TOLERANCE);
-  }   
-
-  public boolean getRightAxisUpTrigger(){
-    return (getRightYAxis() < RIGHT_AXIS_UP_TOLERANCE);
-  }   
-
-  public boolean getRightAxisDownTrigger(){
-    return (getRightYAxis() > RIGHT_AXIS_DOWN_TOLERANCE);
-  }   
-
-  public boolean getRightAxisLeftTrigger(){
-    return (getRightXAxis() > RIGHT_AXIS_LEFT_TOLERANCE);
-  }   
-
-  public boolean getRightAxisRightTrigger(){
-    return (getRightXAxis() > RIGHT_AXIS_RIGHT_TOLERANCE);
-  }   
-
-  public boolean getLeftAxisUpTrigger(){
-    return (getLeftYAxis() < LEFT_AXIS_UP_TOLERANCE);
-  }   
-
-  public boolean getLeftAxisDownTrigger(){
-    return (getLeftYAxis() > LEFT_AXIS_DOWN_TOLERANCE);
-  }   
-
-  public boolean getLeftAxisLeftTrigger(){
-    return (getLeftXAxis() > LEFT_AXIS_LEFT_TOLERANCE);
-  }   
-
-  public boolean getLeftAxisRightTrigger(){
-    return (getLeftXAxis() > LEFT_AXIS_RIGHT_TOLERANCE);
-  }
-}

src/main/java/frc4388/utility/controller/XboxControllerRaw.java

@@ -1,104 +0,0 @@
-package frc4388.utility.controller;
-import java.nio.ByteBuffer;
-import edu.wpi.first.hal.HAL;
-
-
-public class XboxControllerRaw implements IHandController {
-    public static final int LEFT_X_AXIS = 0;
-    public static final int LEFT_Y_AXIS = 1;
-    public static final int LEFT_TRIGGER_AXIS = 2;
-    public static final int RIGHT_TRIGGER_AXIS = 3;
-    public static final int RIGHT_X_AXIS = 4;
-    public static final int RIGHT_Y_AXIS = 5;
-    public static final int LEFT_RIGHT_DPAD_AXIS = 6;
-    public static final int TOP_BOTTOM_DPAD_AXIS = 6;
-  
-    public static final int A_BUTTON = 0;
-    public static final int B_BUTTON = 1;
-    public static final int X_BUTTON = 2;
-    public static final int Y_BUTTON = 3;
-    public static final int LEFT_BUMPER_BUTTON = 4;
-    public static final int RIGHT_BUMPER_BUTTON = 5;
-    public static final int BACK_BUTTON = 6;
-    public static final int START_BUTTON = 7;
-  
-    public static final int LEFT_JOYSTICK_BUTTON = 8;
-    public static final int RIGHT_JOYSTICK_BUTTON = 9;
-    
-    private static final double DEADZONE = 0.08;
-
-  private int m_ID;
-  private int m_buttons = 0;
-  private int m_numButtons;
-  private ByteBuffer m_buttonCountBuffer = ByteBuffer.allocateDirect(1);
-  float[] m_axes = new float[HAL.kMaxJoystickAxes];
-
-  public XboxControllerRaw(int id) {
-    m_ID = id;
-  }
-
-
-  public void updateInput() {
-    HAL.getJoystickAxes((byte) m_ID, m_axes);
-    m_buttons = HAL.getJoystickButtons((byte) m_ID, m_buttonCountBuffer);
-    m_numButtons = m_buttonCountBuffer.get(0);
-  }
-
-  public int getNumButtons() {
-    return m_numButtons;
-  }
-
-  public boolean getButton(int index) {
-      return (m_buttons & 1 << index) > 0;
-  }
-
-  /**
-   * Checks if the input falls within the deadzone.
-   * @param input from an axis on the controller
-   * @return true if input falls in deadzone, false if input falls outside deadzone
-   */
-  public static boolean inDeadZone(double input){
-    return (Math.abs(input) < DEADZONE);
-  }
-
-  /**
-   * Updates an input to have a deadzone around the 0 position
-   * @param input from an axis on the controller
-   * @return updated input
-   */
-  private double getAxisWithDeadZoneCheck(double input){
-    if(inDeadZone(input)){
-      return 0.0;       
-    } else {
-      return input;
-    }
-  }
-
-  public double getLeftXAxis() {
-      return getAxisWithDeadZoneCheck(m_axes[LEFT_X_AXIS]);
-  }
-
-  public double getLeftYAxis() {
-    return getAxisWithDeadZoneCheck(m_axes[LEFT_Y_AXIS]);
-  }
-
-  public double getRightXAxis() {
-    return getAxisWithDeadZoneCheck(m_axes[RIGHT_X_AXIS]);
-  }
-
-  public double getRightYAxis() {
-    return getAxisWithDeadZoneCheck(m_axes[RIGHT_Y_AXIS]);
-  }
-
-  public double getLeftTriggerAxis() {
-    return getAxisWithDeadZoneCheck(m_axes[LEFT_TRIGGER_AXIS]);
-  }
-
-  public double getRightTriggerAxis() {
-    return getAxisWithDeadZoneCheck(m_axes[RIGHT_TRIGGER_AXIS]);
-  }
-
-  public int getDpadAngle() {
-      return -1;
-  }
-}

src/main/java/frc4388/utility/controller/XboxControllerRawButton.java

@@ -1,18 +0,0 @@
-package frc4388.utility.controller;
-
-import edu.wpi.first.wpilibj2.command.button.Button;
-
-public class XboxControllerRawButton extends Button {
-    private final XboxControllerRaw m_controller;
-    private final int m_buttonNumber;
-
-    public XboxControllerRawButton(XboxControllerRaw controller, int buttonNumber) {
-        m_controller = controller;
-        m_buttonNumber = buttonNumber;
-    }
-
-    @Override
-    public boolean get() {
-        return m_controller.getButton(m_buttonNumber);
-    }
-}

src/main/java/frc4388/utility/controller/XboxTriggerButton.java

@@ -1,50 +0,0 @@
-package frc4388.utility.controller;
-
-import edu.wpi.first.wpilibj2.command.button.Button;
-
-/**
- * Mapping for the Xbox controller triggers to allow triggers to be defined as
- * buttons in {@link frc4388.robot.OI}. Checks to see if the given trigger 
- * exceeds a tolerance defined in {@link XboxController}.
- */
-public class XboxTriggerButton extends Button {
-  public static final int RIGHT_TRIGGER = 0;
-  public static final int LEFT_TRIGGER = 1;
-  public static final int RIGHT_AXIS_UP_TRIGGER = 2;
-  public static final int RIGHT_AXIS_DOWN_TRIGGER = 3;
-  public static final int RIGHT_AXIS_RIGHT_TRIGGER = 4;
-  public static final int RIGHT_AXIS_LEFT_TRIGGER = 5;
-  public static final int LEFT_AXIS_UP_TRIGGER = 6;
-  public static final int LEFT_AXIS_DOWN_TRIGGER = 7;
-  public static final int LEFT_AXIS_RIGHT_TRIGGER = 8;
-  public static final int LEFT_AXIS_LEFT_TRIGGER = 9;
-
-  private XboxController m_controller;
-  private int m_trigger;
-
-  /**
-   * Creates a Trigger-Button mapped to a specific Xbox controller and trigger
-   */
-  public XboxTriggerButton(XboxController controller, int trigger) {
-    m_controller = controller;
-    m_trigger = trigger;
-  }
-
-  /** {@inheritDoc} */
-  @Override
-  public boolean get() {
-    switch (m_trigger) {
-      case RIGHT_TRIGGER: return m_controller.getRightTrigger();
-      case LEFT_TRIGGER: return m_controller.getLeftTrigger();
-      case RIGHT_AXIS_UP_TRIGGER: return m_controller.getRightAxisUpTrigger();
-      case RIGHT_AXIS_DOWN_TRIGGER: return m_controller.getRightAxisDownTrigger();
-      case RIGHT_AXIS_RIGHT_TRIGGER: return m_controller.getRightAxisRightTrigger();
-      case RIGHT_AXIS_LEFT_TRIGGER: return m_controller.getRightAxisLeftTrigger();
-      case LEFT_AXIS_UP_TRIGGER: return m_controller.getLeftAxisUpTrigger();
-      case LEFT_AXIS_DOWN_TRIGGER: return m_controller.getLeftAxisDownTrigger();
-      case LEFT_AXIS_RIGHT_TRIGGER: return m_controller.getLeftAxisRightTrigger();
-      case LEFT_AXIS_LEFT_TRIGGER: return m_controller.getLeftAxisLeftTrigger();
-      default: return false;
-    }
-  }
-}