moving raw input to XboxControllerRaw [WIP]

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

@@ -26,7 +26,7 @@ public final class Constants {
     public static final double WHEEL_SPEED = 0.1;
     public static final double WIDTH = 22;
     public static final double HEIGHT = 22;
-    public static final double JOYSTICK_TO_METERS_PER_SECOND = 1;
+    public static final double JOYSTICK_TO_METERS_PER_SECOND = 11;
     public static final double MAX_SPEED_FEET_PER_SEC = 20;
     public static final double SPEED_FEET_PER_SECOND_AT_FULL_POWER = 20;
       //IDs

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

@@ -83,10 +83,10 @@ public class RobotContainer {
         .whenPressed(() -> m_robotLED.setPattern(LEDPatterns.LAVA_RAINBOW))
         .whenReleased(() -> m_robotLED.setPattern(LEDConstants.DEFAULT_PATTERN));
 
-    new JoystickButton(getDriverJoystick(), XboxController.A_BUTTON)
-    .whenPressed(() -> {
-      m_robotSwerveDrive.m_gyro.setYaw(0);
-    });
+    // new JoystickButton(getDriverJoystick(), XboxController.A_BUTTON)
+    // .whenPressed(() -> {
+    //   m_robotSwerveDrive.m_gyro.setYaw(0);
+    // });
   }
 
   /**

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

@@ -4,6 +4,7 @@
 
 package frc4388.robot.subsystems;
 
+import java.nio.ByteBuffer;
 import java.util.Arrays;
 
 import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX;
@@ -95,10 +96,30 @@ public class SwerveDrive extends SubsystemBase {
       // Temporary janky raw joysticks
       float[] driveController = new float[HAL.kMaxJoystickAxes];
       HAL.getJoystickAxes((byte) 0, driveController);
+      // ByteBuffer buttons = new 
+      ByteBuffer num_buttons_buffer = ByteBuffer.allocateDirect(1);
+      int buttons_int = HAL.getJoystickButtons((byte) 0, num_buttons_buffer);
+      int num_buttons = num_buttons_buffer.get(0);
+      boolean[] buttons = new boolean[num_buttons];
+      for (int i = 0; i < num_buttons; i++) {
+        buttons[i] = (buttons_int & 1 << i) > 0;
+      }
+      if (buttons[0]) 
+        m_gyro.reset();
       float leftXAxis = driveController[0];
       float leftYAxis = driveController[1];
       float rightXAxis = driveController[4];
-      float rightYAxis = driveController[5];
+      leftXAxis = XboxController.inDeadZone(leftYAxis) ? 0.f : leftXAxis;
+      leftYAxis = XboxController.inDeadZone(leftYAxis) ? 0.f : leftYAxis;
+      rightXAxis = XboxController.inDeadZone(rightXAxis) ? 0.f : rightXAxis;
+      leftXAxis *= leftXAxis * leftXAxis;
+      leftYAxis *= leftYAxis * leftYAxis;
+      rightXAxis *= rightXAxis * rightXAxis;
+      double[] dashboardNums = new double[HAL.kMaxJoystickAxes];
+      for (int i = 0; i < HAL.kMaxJoystickAxes; i++) {
+        dashboardNums[i] = (double)((int)(driveController[i] * 100.f)) / 100.d;
+      }
+      SmartDashboard.putNumberArray("axes", dashboardNums);
       speeds = XboxController.ClampJoystickAxis(leftXAxis, leftYAxis);
       rot = -rightXAxis;
 

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

@@ -85,7 +85,7 @@ public class XboxController implements IHandController
    * @param input from an axis on the controller
    * @return true if input falls in deadzone, false if input falls outside deadzone
    */
-  private boolean inDeadZone(double input){
+  public static boolean inDeadZone(double input){
     return (Math.abs(input) < DEADZONE);
   }
 

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

@@ -0,0 +1,20 @@
+// package frc4388.utility.controller;
+
+// public class XboxControllerRaw implements IHandController {
+//   int buttons;
+//   public void updateInput() {
+//   }
+//   public double getLeftXAxis() {}
+
+//   public double getLeftYAxis();
+
+//   public double getRightXAxis();
+
+//   public double getRightYAxis();
+
+//   public double getLeftTriggerAxis();
+
+//   public double getRightTriggerAxis();
+
+//   public int getDpadAngle();
+// }