From 0605a1d7a43ce0223d5cd50a8654d035229eea1c Mon Sep 17 00:00:00 2001 From: nathanrsxtn Date: Sat, 15 Jan 2022 02:01:32 -0700 Subject: [PATCH] Switch to WPI's Xbox controller - Added toggle to bypass Driver Station controller inputs --- .../java/frc4388/robot/RobotContainer.java | 59 ++++++++++--------- .../frc4388/robot/subsystems/SwerveDrive.java | 43 +++++--------- ...ntroller.java => IHandController.java.old} | 0 ...ontroller.java => XboxController.java.old} | 0 ...Button.java => XboxTriggerButton.java.old} | 0 5 files changed, 45 insertions(+), 57 deletions(-) rename src/main/java/frc4388/utility/controller/{IHandController.java => IHandController.java.old} (100%) rename src/main/java/frc4388/utility/controller/{XboxController.java => XboxController.java.old} (100%) rename src/main/java/frc4388/utility/controller/{XboxTriggerButton.java => XboxTriggerButton.java.old} (100%) diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 2f4cd2b..db00580 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -5,7 +5,11 @@ package frc4388.robot; import edu.wpi.first.hal.HAL; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.wpilibj.Joystick; +import edu.wpi.first.wpilibj.XboxController; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.RunCommand; @@ -14,8 +18,6 @@ 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; /** * This class is where the bulk of the robot should be declared. Since @@ -47,6 +49,8 @@ public class RobotContainer { private final XboxController m_driverXbox = new XboxController(OIConstants.XBOX_DRIVER_ID); private final XboxController m_operatorXbox = new XboxController(OIConstants.XBOX_OPERATOR_ID); + private final boolean bypassDSController = true; + /** * The container for the robot. Contains subsystems, OI devices, and commands. */ @@ -54,19 +58,30 @@ public class RobotContainer { configureButtonBindings(); /* Default Commands */ // 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(), - true), - m_robotSwerveDrive)); + float[] joystickAxes = new float[HAL.kMaxJoystickAxes]; + HAL.getJoystickAxes((byte) getDriverController().getPort(), joystickAxes); + double leftXAxis = bypassDSController ? joystickAxes[XboxController.Axis.kLeftX.value] : getDriverController().getLeftX(); + double leftYAxis = bypassDSController ? joystickAxes[XboxController.Axis.kLeftY.value] : getDriverController().getLeftY(); + double rightXAxis = bypassDSController ? joystickAxes[XboxController.Axis.kRightX.value] : getDriverController().getRightX(); + m_robotSwerveDrive.setDefaultCommand(new RunCommand(() -> + m_robotSwerveDrive.driveWithInput(clampJoystickAxis(leftXAxis, leftYAxis), -rightXAxis, true), + m_robotSwerveDrive + )); // continually sends updates to the Blinkin LED controller to keep the lights on m_robotLED.setDefaultCommand(new RunCommand(m_robotLED::updateLED, m_robotLED)); } - + private static double[] clampJoystickAxis(double x, double y) { + double squareMag = x * x + y * y; + double[] ret = { x, y }; + if (squareMag > 1.d) { + double mag = Math.sqrt(squareMag); + ret[0] = x / mag; + ret[1] = y / mag; + } + SmartDashboard.putNumberArray("Input", new double[] {squareMag, x, y, ret[0], ret[1]}); + return ret; + } /** * Use this method to define your button->command mappings. Buttons can be * created by instantiating a {@link GenericHID} or one of its subclasses @@ -79,11 +94,11 @@ public class RobotContainer { .whenPressed(() -> m_robotSwerveDrive.resetGyro());*/ /* Operator Buttons */ // activates "Lit Mode" - new JoystickButton(getOperatorJoystick(), XboxController.A_BUTTON) + new JoystickButton(getOperatorController(), XboxController.Button.kA.value) .whenPressed(() -> m_robotLED.setPattern(LEDPatterns.LAVA_RAINBOW)) .whenReleased(() -> m_robotLED.setPattern(LEDConstants.DEFAULT_PATTERN)); - new JoystickButton(getDriverJoystick(), XboxController.A_BUTTON) + new JoystickButton(getDriverController(), XboxController.Button.kA.value) .whenPressed(() -> { m_robotSwerveDrive.m_gyro.setYaw(0); }); @@ -102,28 +117,14 @@ public class RobotContainer { /** * Add your docs here. */ - public IHandController getDriverController() { + public XboxController getDriverController() { return m_driverXbox; } /** * 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(); - } } diff --git a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java index ee9d1a7..658d1ca 100644 --- a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java +++ b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java @@ -22,7 +22,6 @@ import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc4388.robot.Constants.SwerveDriveConstants; import frc4388.utility.Gains; -import frc4388.utility.controller.XboxController; public class SwerveDrive extends SubsystemBase { SwerveDriveKinematics m_kinematics; @@ -92,38 +91,26 @@ public class SwerveDrive extends SubsystemBase { */ public void driveWithInput(double[] speeds, double rot, boolean fieldRelative) { - // Temporary janky raw joysticks - float[] driveController = new float[HAL.kMaxJoystickAxes]; - HAL.getJoystickAxes((byte) 0, driveController); - float leftXAxis = driveController[0]; - float leftYAxis = driveController[1]; - float rightXAxis = driveController[4]; - float rightYAxis = driveController[5]; - speeds = XboxController.ClampJoystickAxis(leftXAxis, leftYAxis); - rot = -rightXAxis; - - - // System.out.println("Inputx: " + speeds[0] + " Inputy: " + speeds[1]); - /*var speeds = new ChassisSpeeds(strafeX, strafeY, rotate * SwerveDriveConstants.ROTATION_SPEED //in rad/s ); - driveFromSpeeds(speeds);*/ - double xSpeedMetersPerSecond = -speeds[0] * SwerveDriveConstants.JOYSTICK_TO_METERS_PER_SECOND; - double ySpeedMetersPerSecond = speeds[1] * SwerveDriveConstants.JOYSTICK_TO_METERS_PER_SECOND; - SwerveModuleState[] states = - kinematics.toSwerveModuleStates( - fieldRelative - ? ChassisSpeeds.fromFieldRelativeSpeeds(xSpeedMetersPerSecond, ySpeedMetersPerSecond, rot*3, m_gyro.getRotation2d()) - : new ChassisSpeeds(xSpeedMetersPerSecond, ySpeedMetersPerSecond, rot*3)); - SwerveDriveKinematics.desaturateWheelSpeeds(states, Units.feetToMeters(SwerveDriveConstants.MAX_SPEED_FEET_PER_SEC)); - for (int i = 0; i < states.length; i++) { - SwerveModule module = modules[i]; - SwerveModuleState state = states[i]; - module.setDesiredState(state); + // System.out.println("Inputx: " + speeds[0] + " Inputy: " + speeds[1]); + /*var speeds = new ChassisSpeeds(strafeX, strafeY, rotate * SwerveDriveConstants.ROTATION_SPEED //in rad/s ); + driveFromSpeeds(speeds);*/ + double xSpeedMetersPerSecond = -speeds[0] * SwerveDriveConstants.JOYSTICK_TO_METERS_PER_SECOND; + double ySpeedMetersPerSecond = speeds[1] * SwerveDriveConstants.JOYSTICK_TO_METERS_PER_SECOND; + SwerveModuleState[] states = + kinematics.toSwerveModuleStates( + fieldRelative + ? ChassisSpeeds.fromFieldRelativeSpeeds(xSpeedMetersPerSecond, ySpeedMetersPerSecond, rot*3, m_gyro.getRotation2d()) + : new ChassisSpeeds(xSpeedMetersPerSecond, ySpeedMetersPerSecond, rot*3)); + SwerveDriveKinematics.desaturateWheelSpeeds(states, Units.feetToMeters(SwerveDriveConstants.MAX_SPEED_FEET_PER_SEC)); + for (int i = 0; i < states.length; i++) { + SwerveModule module = modules[i]; + SwerveModuleState state = states[i]; + module.setDesiredState(state); } } @Override public void periodic() { - System.err.println(m_gyro.getFusedHeading() +" aaa"); // m_gyro.setFusedHeadingToCompass(); // m_gyro.setYawToCompass(); // m_gyro.getRotation2d(); diff --git a/src/main/java/frc4388/utility/controller/IHandController.java b/src/main/java/frc4388/utility/controller/IHandController.java.old similarity index 100% rename from src/main/java/frc4388/utility/controller/IHandController.java rename to src/main/java/frc4388/utility/controller/IHandController.java.old diff --git a/src/main/java/frc4388/utility/controller/XboxController.java b/src/main/java/frc4388/utility/controller/XboxController.java.old similarity index 100% rename from src/main/java/frc4388/utility/controller/XboxController.java rename to src/main/java/frc4388/utility/controller/XboxController.java.old diff --git a/src/main/java/frc4388/utility/controller/XboxTriggerButton.java b/src/main/java/frc4388/utility/controller/XboxTriggerButton.java.old similarity index 100% rename from src/main/java/frc4388/utility/controller/XboxTriggerButton.java rename to src/main/java/frc4388/utility/controller/XboxTriggerButton.java.old