From 045a77d08a9d8204c9cfdc6f0a8ec57013368a56 Mon Sep 17 00:00:00 2001 From: Aarav Shah Date: Sat, 15 Jan 2022 11:31:15 -0700 Subject: [PATCH] Revert "Switch to WPI's Xbox controller" This reverts commit 0605a1d7a43ce0223d5cd50a8654d035229eea1c. --- .../java/frc4388/robot/RobotContainer.java | 59 +++++++++---------- .../frc4388/robot/subsystems/SwerveDrive.java | 43 +++++++++----- ...ntroller.java.old => IHandController.java} | 0 ...ontroller.java.old => XboxController.java} | 0 ...Button.java.old => XboxTriggerButton.java} | 0 5 files changed, 57 insertions(+), 45 deletions(-) rename src/main/java/frc4388/utility/controller/{IHandController.java.old => IHandController.java} (100%) rename src/main/java/frc4388/utility/controller/{XboxController.java.old => XboxController.java} (100%) rename src/main/java/frc4388/utility/controller/{XboxTriggerButton.java.old => XboxTriggerButton.java} (100%) diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index db00580..2f4cd2b 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -5,11 +5,7 @@ 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; @@ -18,6 +14,8 @@ 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 @@ -49,8 +47,6 @@ 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. */ @@ -58,30 +54,19 @@ public class RobotContainer { configureButtonBindings(); /* Default Commands */ // drives the swerve drive with a two-axis input from the driver controller - 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 - )); + m_robotSwerveDrive.setDefaultCommand( + new RunCommand(() -> m_robotSwerveDrive.driveWithInput( + XboxController.ClampJoystickAxis( + getDriverController().getLeftXAxis(), + getDriverController().getLeftYAxis()), + -getDriverController().getRightXAxis(), + 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 @@ -94,11 +79,11 @@ public class RobotContainer { .whenPressed(() -> m_robotSwerveDrive.resetGyro());*/ /* Operator Buttons */ // activates "Lit Mode" - new JoystickButton(getOperatorController(), XboxController.Button.kA.value) + new JoystickButton(getOperatorJoystick(), XboxController.A_BUTTON) .whenPressed(() -> m_robotLED.setPattern(LEDPatterns.LAVA_RAINBOW)) .whenReleased(() -> m_robotLED.setPattern(LEDConstants.DEFAULT_PATTERN)); - new JoystickButton(getDriverController(), XboxController.Button.kA.value) + new JoystickButton(getDriverJoystick(), XboxController.A_BUTTON) .whenPressed(() -> { m_robotSwerveDrive.m_gyro.setYaw(0); }); @@ -117,14 +102,28 @@ public class RobotContainer { /** * Add your docs here. */ - public XboxController getDriverController() { + public IHandController getDriverController() { return m_driverXbox; } /** * Add your docs here. */ - public XboxController getOperatorController() { + public IHandController 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 658d1ca..ee9d1a7 100644 --- a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java +++ b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java @@ -22,6 +22,7 @@ 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; @@ -91,26 +92,38 @@ public class SwerveDrive extends SubsystemBase { */ public void driveWithInput(double[] speeds, double rot, boolean fieldRelative) { - // 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); + // 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); } } @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.old b/src/main/java/frc4388/utility/controller/IHandController.java similarity index 100% rename from src/main/java/frc4388/utility/controller/IHandController.java.old rename to src/main/java/frc4388/utility/controller/IHandController.java diff --git a/src/main/java/frc4388/utility/controller/XboxController.java.old b/src/main/java/frc4388/utility/controller/XboxController.java similarity index 100% rename from src/main/java/frc4388/utility/controller/XboxController.java.old rename to src/main/java/frc4388/utility/controller/XboxController.java diff --git a/src/main/java/frc4388/utility/controller/XboxTriggerButton.java.old b/src/main/java/frc4388/utility/controller/XboxTriggerButton.java similarity index 100% rename from src/main/java/frc4388/utility/controller/XboxTriggerButton.java.old rename to src/main/java/frc4388/utility/controller/XboxTriggerButton.java