From 6ab8b13056a56eb3fa57b0fcd8ca897dd9f2fc7f Mon Sep 17 00:00:00 2001 From: Aarav Date: Thu, 16 Mar 2023 12:58:55 -0600 Subject: [PATCH] fixes for driving --- src/main/java/frc4388/robot/Constants.java | 5 +- .../java/frc4388/robot/RobotContainer.java | 34 ++++++---- .../frc4388/robot/subsystems/SwerveDrive.java | 62 +++++++++++++++++-- .../controller/DeadbandedXboxController.java | 8 ++- 4 files changed, 90 insertions(+), 19 deletions(-) diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index f82c168..29a76af 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -23,7 +23,6 @@ import frc4388.utility.LEDPatterns; */ public final class Constants { public static final class SwerveDriveConstants { - public static final double MAX_ROT_SPEED = 1.5; public static final double MIN_ROT_SPEED = 0.8; public static double ROTATION_SPEED = MAX_ROT_SPEED; @@ -32,6 +31,10 @@ public final class Constants { public static final double CORRECTION_MIN = 10; public static final double CORRECTION_MAX = 50; + public static final double SLOW_SPEED = 0.8; + public static final double FAST_SPEED = 1.0; + public static final double TURBO_SPEED = 4.0; + public static final class IDs { public static final int LEFT_FRONT_WHEEL_ID = 2; public static final int LEFT_FRONT_STEER_ID = 3; diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 1e721ef..45ca574 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -122,25 +122,33 @@ public class RobotContainer { new JoystickButton(getDeadbandedDriverController(), XboxController.A_BUTTON) .onTrue(new InstantCommand(() -> m_robotSwerveDrive.resetGyro(), m_robotSwerveDrive)); - new JoystickButton(getDeadbandedDriverController(), XboxController.X_BUTTON) - .onTrue(new InstantCommand(() -> m_robotSwerveDrive.toggleGear(m_robotArm.getArmRotation()-135), m_robotSwerveDrive)); + new JoystickButton(getDeadbandedDriverController(), XboxController.RIGHT_BUMPER_BUTTON) + .onTrue(new InstantCommand(() -> m_robotSwerveDrive.setToTurbo())) + .onFalse(new InstantCommand(() -> m_robotSwerveDrive.setToFast())); + + // new JoystickButton(getDeadbandedDriverController(), XboxController.RIGHT_BUMPER_BUTTON) + // .whileTrue(new InstantCommand(() -> m_robotSwerveDrive.setToTurbo())); + + new JoystickButton(getDeadbandedDriverController(), XboxController.LEFT_BUMPER_BUTTON) + .onTrue(new InstantCommand(() -> m_robotSwerveDrive.setToSlow())); + // // .onFalse() new JoystickButton(getDeadbandedDriverController(), XboxController.Y_BUTTON) .onTrue(new AutoBalance(m_robotMap.gyro, m_robotSwerveDrive)); - new JoystickButton(getDeadbandedDriverController(), XboxController.RIGHT_BUMPER_BUTTON) - .whileTrue(new JoystickRecorder(m_robotSwerveDrive, - () -> getDeadbandedDriverController().getLeftX(), - () -> getDeadbandedDriverController().getLeftY(), - () -> getDeadbandedDriverController().getRightX(), - () -> getDeadbandedDriverController().getRightY(), - "Blue1Path.txt")) - .onFalse(new InstantCommand()); + // new JoystickButton(getDeadbandedDriverController(), XboxController.RIGHT_BUMPER_BUTTON) + // .whileTrue(new JoystickRecorder(m_robotSwerveDrive, + // () -> getDeadbandedDriverController().getLeftX(), + // () -> getDeadbandedDriverController().getLeftY(), + // () -> getDeadbandedDriverController().getRightX(), + // () -> getDeadbandedDriverController().getRightY(), + // "Blue1Path.txt")) + // .onFalse(new InstantCommand()); - new JoystickButton(getDeadbandedDriverController(), XboxController.LEFT_BUMPER_BUTTON) - .onTrue(new JoystickPlayback(m_robotSwerveDrive, "Blue1Path.txt")) - .onFalse(new InstantCommand()); + // new JoystickButton(getDeadbandedDriverController(), XboxController.LEFT_BUMPER_BUTTON) + // .onTrue(new JoystickPlayback(m_robotSwerveDrive, "Blue1Path.txt")) + // .onFalse(new InstantCommand()); // * Operator Buttons // new JoystickButton(getDeadbandedOperatorController(), XboxController.X_BUTTON) diff --git a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java index 2edb77d..556c63e 100644 --- a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java +++ b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java @@ -53,6 +53,7 @@ public class SwerveDrive extends SubsystemBase { this.modules = new SwerveModule[] {this.leftFront, this.rightFront, this.leftBack, this.rightBack}; } + boolean stopped = false; public void driveWithInput(Translation2d leftStick, Translation2d rightStick, boolean fieldRelative) { if (fieldRelative) { @@ -61,13 +62,20 @@ public class SwerveDrive extends SubsystemBase { rotTarget = gyro.getAngle(); rot = rightStick.getX() * SwerveDriveConstants.ROTATION_SPEED; SmartDashboard.putBoolean("drift correction", false); - } else { + stopped = false; + } else if(leftStick.getNorm() > 0.05) { + if (!stopped) { + stopModules(); + stopped = true; + } + SmartDashboard.putBoolean("drift correction", true); rot = ((rotTarget - gyro.getAngle()) / 360) * SwerveDriveConstants.ROT_CORRECTION_SPEED; } // Use the left joystick to set speed. Apply a cubic curve and the set max speed. Translation2d speed = leftStick.times(leftStick.getNorm() * speedAdjust); + // Translation2d speed = leftStick.times(speedAdjust / leftStick.getNorm()); Translation2d cubedSpeed = new Translation2d(Math.pow(speed.getX(), 3.00), Math.pow(speed.getY(), 3.00)); // Convert field-relative speeds to robot-relative speeds. @@ -116,11 +124,57 @@ public class SwerveDrive extends SubsystemBase { // This method will be called once per scheduler run } + public void shiftDown() { + if (Math.abs(this.speedAdjust - SwerveDriveConstants.SLOW_SPEED) < .01) { + + } else if (Math.abs(this.speedAdjust - SwerveDriveConstants.FAST_SPEED) < .01) { + this.speedAdjust = SwerveDriveConstants.SLOW_SPEED; + } else { + this.speedAdjust = SwerveDriveConstants.FAST_SPEED; + } + } + + public void setToSlow() { + this.speedAdjust = SwerveDriveConstants.SLOW_SPEED; + System.out.println("SLOW"); + System.out.println("SLOW"); + System.out.println("SLOW"); + System.out.println("SLOW"); + System.out.println("SLOW"); + } + + public void setToFast() { + this.speedAdjust = SwerveDriveConstants.FAST_SPEED; + System.out.println("FAST"); + System.out.println("FAST"); + System.out.println("FAST"); + System.out.println("FAST"); + System.out.println("FAST"); + } + + public void setToTurbo() { + this.speedAdjust = SwerveDriveConstants.TURBO_SPEED; + System.out.println("TURBO"); + System.out.println("TURBO"); + System.out.println("TURBO"); + System.out.println("TURBO"); + System.out.println("TURBO"); + } + + public void shiftUp() { + if (Math.abs(this.speedAdjust - SwerveDriveConstants.SLOW_SPEED) < .01) { + this.speedAdjust = SwerveDriveConstants.FAST_SPEED; + } else if (Math.abs(this.speedAdjust - SwerveDriveConstants.FAST_SPEED) < .01) { + this.speedAdjust = SwerveDriveConstants.TURBO_SPEED; + } else { + + } + } + public void toggleGear(double angle) { - if (this.speedAdjust == SwerveDriveConstants.Conversions.JOYSTICK_TO_METERS_PER_SECOND_SLOW - && Math.abs(angle) < 2) { + if (Math.abs(this.speedAdjust - SwerveDriveConstants.Conversions.JOYSTICK_TO_METERS_PER_SECOND_SLOW) < .01 && Math.abs(angle) < 10) { this.speedAdjust = SwerveDriveConstants.Conversions.JOYSTICK_TO_METERS_PER_SECOND_FAST; - SwerveDriveConstants.ROT_CORRECTION_SPEED = SwerveDriveConstants.CORRECTION_MAX; + SwerveDriveConstants.ROT_CORRECTION_SPEED = SwerveDriveConstants.CORRECTION_MIN; } else { this.speedAdjust = SwerveDriveConstants.Conversions.JOYSTICK_TO_METERS_PER_SECOND_SLOW; SwerveDriveConstants.ROT_CORRECTION_SPEED = SwerveDriveConstants.CORRECTION_MIN; diff --git a/src/main/java/frc4388/utility/controller/DeadbandedXboxController.java b/src/main/java/frc4388/utility/controller/DeadbandedXboxController.java index bad1605..d339841 100644 --- a/src/main/java/frc4388/utility/controller/DeadbandedXboxController.java +++ b/src/main/java/frc4388/utility/controller/DeadbandedXboxController.java @@ -20,8 +20,14 @@ public class DeadbandedXboxController extends XboxController { public static Translation2d skewToDeadzonedCircle(double x, double y) { Translation2d translation2d = new Translation2d(x, y); double magnitude = translation2d.getNorm(); - if (OIConstants.SKEW_STICKS && magnitude >= 1) return translation2d.div(magnitude); + + // if (OIConstants.SKEW_STICKS && magnitude >= 1) { + // System.out.println("if statement running"); + // return translation2d.div(magnitude); + // } + if (magnitude < LEFT_AXIS_DEADBAND) return new Translation2d(0,0); + return translation2d; } } \ No newline at end of file