From 3061563db329404fa63c5ef549fef61d61d80a4b Mon Sep 17 00:00:00 2001 From: Ryan Manley Date: Sat, 15 Jan 2022 15:52:38 -0700 Subject: [PATCH] integrated xboxraw --- src/main/java/frc4388/robot/Constants.java | 2 +- src/main/java/frc4388/robot/Robot.java | 2 + .../java/frc4388/robot/RobotContainer.java | 44 +++++++------ .../frc4388/robot/subsystems/SwerveDrive.java | 64 ++++++++++--------- .../utility/controller/IHandController.java | 2 + .../utility/controller/XboxController.java | 2 + .../utility/controller/XboxControllerRaw.java | 13 +++- .../controller/XboxControllerRawButton.java | 18 ++++++ 8 files changed, 93 insertions(+), 54 deletions(-) create mode 100644 src/main/java/frc4388/utility/controller/XboxControllerRawButton.java diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index 592df37..01b3b37 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -22,7 +22,7 @@ import frc4388.utility.LEDPatterns; */ public final class Constants { public static final class SwerveDriveConstants { - public static final double ROTATION_SPEED = 3; + public static final double ROTATION_SPEED = 4; public static final double WHEEL_SPEED = 0.1; public static final double WIDTH = 22; public static final double HEIGHT = 22; diff --git a/src/main/java/frc4388/robot/Robot.java b/src/main/java/frc4388/robot/Robot.java index dd717d6..03eda20 100644 --- a/src/main/java/frc4388/robot/Robot.java +++ b/src/main/java/frc4388/robot/Robot.java @@ -114,6 +114,8 @@ public class Robot extends TimedRobot { */ @Override public void teleopPeriodic() { + m_robotContainer.getDriverController().updateInput(); + m_robotContainer.getOperatorController().updateInput(); } /** diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index f91a605..76c2e5e 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -16,6 +16,8 @@ import frc4388.robot.subsystems.SwerveDrive; import frc4388.utility.LEDPatterns; import frc4388.utility.controller.IHandController; import frc4388.utility.controller.XboxController; +import frc4388.utility.controller.XboxControllerRaw; +import frc4388.utility.controller.XboxControllerRawButton; /** * This class is where the bulk of the robot should be declared. Since @@ -44,8 +46,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 XboxControllerRaw m_driverXbox = new XboxControllerRaw(OIConstants.XBOX_DRIVER_ID); + private final XboxControllerRaw m_operatorXbox = new XboxControllerRaw(OIConstants.XBOX_OPERATOR_ID); /** * The container for the robot. Contains subsystems, OI devices, and commands. @@ -75,21 +77,25 @@ public class RobotContainer { */ private void configureButtonBindings() { /* Driver Buttons */ - /*new JoystickButton(getDriverJoystick(), XboxController.Y_BUTTON) - .whenPressed(() -> m_robotSwerveDrive.resetGyro());*/ + // new JoystickButton(getDriverJoystick(), XboxController.Y_BUTTON) + new XboxControllerRawButton(m_driverXbox, XboxControllerRaw.Y_BUTTON) + .whenPressed(() -> m_robotSwerveDrive.m_gyro.reset()); - new JoystickButton(getDriverJoystick(), XboxController.LEFT_BUMPER_BUTTON) + // new JoystickButton(getDriverJoystick(), XboxController.LEFT_BUMPER_BUTTON) + new XboxControllerRawButton(m_driverXbox, XboxControllerRaw.LEFT_BUMPER_BUTTON) .whenPressed(() -> m_robotSwerveDrive.highSpeed(false)); - new JoystickButton(getDriverJoystick(), XboxController.RIGHT_BUMPER_BUTTON) + // new JoystickButton(getDriverJoystick(), XboxController.RIGHT_BUMPER_BUTTON) + new XboxControllerRawButton(m_driverXbox, XboxControllerRaw.RIGHT_BUMPER_BUTTON) .whenPressed(() -> m_robotSwerveDrive.highSpeed(true)); /* Operator Buttons */ // activates "Lit Mode" - new JoystickButton(getOperatorJoystick(), XboxController.A_BUTTON) + // new JoystickButton(getOperatorJoystick(), XboxController.A_BUTTON) + new XboxControllerRawButton(m_driverXbox, XboxControllerRaw.A_BUTTON) .whenPressed(() -> m_robotLED.setPattern(LEDPatterns.LAVA_RAINBOW)) .whenReleased(() -> m_robotLED.setPattern(LEDConstants.DEFAULT_PATTERN)); } @@ -118,17 +124,17 @@ public class RobotContainer { return m_operatorXbox; } - /** - * Add your docs here. - */ - public Joystick getOperatorJoystick() { - return m_operatorXbox.getJoyStick(); - } + // /** + // * Add your docs here. + // */ + // public Joystick getOperatorJoystick() { + // return m_operatorXbox.getJoyStick(); + // } - /** - * Add your docs here. - */ - public Joystick getDriverJoystick() { - return m_driverXbox.getJoyStick(); - } + // /** + // * Add your docs here. + // */ + // public Joystick getDriverJoystick() { + // return m_driverXbox.getJoyStick(); + // } } diff --git a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java index dabc3df..0c5df2c 100644 --- a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java +++ b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java @@ -97,37 +97,39 @@ public class SwerveDrive extends SubsystemBase { */ public void driveWithInput(double[] speeds, double rot, boolean fieldRelative) { - if (speeds[0] == 0 && speeds[1] == 0) ignoreAngles = true; + if (speeds[0] == 0 && speeds[1] == 0 && rot == 0) ignoreAngles = true; else ignoreAngles = false; - // 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]; - 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; + speeds[0] *= speeds[0] * speeds[0]; + speeds[1] *= speeds[1] * speeds[1]; + // // 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]; + // 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; // System.out.println("Inputx: " + speeds[0] + " Inputy: " + speeds[1]); @@ -144,7 +146,7 @@ public class SwerveDrive extends SubsystemBase { for (int i = 0; i < states.length; i++) { SwerveModule module = modules[i]; SwerveModuleState state = states[i]; - module.setDesiredState(state, ignoreAngle); + module.setDesiredState(state, ignoreAngles); } } diff --git a/src/main/java/frc4388/utility/controller/IHandController.java b/src/main/java/frc4388/utility/controller/IHandController.java index 319aebc..8db3d8f 100644 --- a/src/main/java/frc4388/utility/controller/IHandController.java +++ b/src/main/java/frc4388/utility/controller/IHandController.java @@ -18,4 +18,6 @@ public interface IHandController { public double getRightTriggerAxis(); public int getDpadAngle(); + + public void updateInput(); } diff --git a/src/main/java/frc4388/utility/controller/XboxController.java b/src/main/java/frc4388/utility/controller/XboxController.java index cc8a0b7..70b599b 100644 --- a/src/main/java/frc4388/utility/controller/XboxController.java +++ b/src/main/java/frc4388/utility/controller/XboxController.java @@ -53,6 +53,8 @@ public class XboxController implements IHandController 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}; diff --git a/src/main/java/frc4388/utility/controller/XboxControllerRaw.java b/src/main/java/frc4388/utility/controller/XboxControllerRaw.java index 20e6cfd..e31e293 100644 --- a/src/main/java/frc4388/utility/controller/XboxControllerRaw.java +++ b/src/main/java/frc4388/utility/controller/XboxControllerRaw.java @@ -25,15 +25,22 @@ public class XboxControllerRaw implements IHandController { public static final int LEFT_JOYSTICK_BUTTON = 8; public static final int RIGHT_JOYSTICK_BUTTON = 9; - private static final double DEADZONE = 0.1; + 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) 0, m_axes); - m_buttons = HAL.getJoystickButtons((byte) 0, m_buttonCountBuffer); + HAL.getJoystickAxes((byte) m_ID, m_axes); + m_buttons = HAL.getJoystickButtons((byte) m_ID, m_buttonCountBuffer); m_numButtons = m_buttonCountBuffer.get(0); } diff --git a/src/main/java/frc4388/utility/controller/XboxControllerRawButton.java b/src/main/java/frc4388/utility/controller/XboxControllerRawButton.java new file mode 100644 index 0000000..7de064e --- /dev/null +++ b/src/main/java/frc4388/utility/controller/XboxControllerRawButton.java @@ -0,0 +1,18 @@ +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); + } +}