diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index a40798e..d5374b6 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -23,7 +23,7 @@ import frc4388.utility.LEDPatterns; public final class Constants { public static final class SwerveDriveConstants { - public static final double MAX_ROT_SPEED = 3.5; + public static final double MAX_ROT_SPEED = 2.5; public static final double AUTO_MAX_ROT_SPEED = 1.5; public static final double MIN_ROT_SPEED = 1.0; public static double ROTATION_SPEED = MAX_ROT_SPEED; @@ -33,9 +33,12 @@ 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.5; - public static final double FAST_SPEED = 1.0; - public static final double TURBO_SPEED = 4.0; + public static final double[] SPEEDS = {0.25, 0.5, 0.75, 1.0, 1.25, 1.5, 1.75, 2, 2.5, 3, 3.5, 4}; + // public static final double[] SPEEDS = {0.5, 1.0, 4.0}; + + // public static final double SLOW_SPEED = 0.5; + // public static final double FAST_SPEED = 1.0; + // public static final double TURBO_SPEED = 4.0; public static final class DefaultSwerveRotOffsets { public static final double FRONT_LEFT_ROT_OFFSET = 220; diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 9ad9a37..8d9f0ba 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -154,7 +154,6 @@ public class RobotContainer { new VirtualController[]{getVirtualDriverController(), getVirtualOperatorController()}, true, false); - // private PlaybackChooser playbackChooser = new PlaybackChooser(m_robotSwerveDrive) // .addOption("Taxi Auto", taxi.asProxy()) // .addOption("One Note Auto Starting in Front of Speaker", oneNoteStartingSpeaker.asProxy()) @@ -163,9 +162,7 @@ public class RobotContainer { // // .addOption("One Note Auto Starting from Right Position", oneNoteStartingFromRight.asProxy()) // .addOption("Two Note Starting in Front of Speaker", twoNoteStartingFromSpeaker.asProxy()) // .addOption("Stay Two Note Starting in Front of Speaker", stayTwoNoteStartingFromSpeaker.asProxy()) - // .buildDisplay(); - - + // .buildDisplay(); /** * The container for the robot. Contains subsystems, OI devices, and commands. @@ -189,7 +186,6 @@ public class RobotContainer { false); }, m_robotSwerveDrive) .withName("SwerveDrive DefaultCommand")); - m_robotSwerveDrive.setToSlow(); // ! Swerve Drive Default Command (Orientation Rotation) // m_robotSwerveDrive.setDefaultCommand(new RunCommand(() -> { @@ -204,12 +200,6 @@ public class RobotContainer { } - // private void changeAuto() { - // autoPlayback.unloadAuto(); - // autoPlayback.loadAuto(); - // lastAutoName = autoplaybackName.get(); - // System.out.println("AUTO: Changed auto to; `" + lastAutoName + "`"); - // } /** * Use this method to define your button->command mappings. Buttons can be * created by instantiating a {@link GenericHID} or one of its subclasses @@ -278,18 +268,6 @@ public class RobotContainer { true, false)) .onFalse(new InstantCommand()); - // new JoystickButton(getDeadbandedDriverController(), XboxController.RIGHT_BUMPER_BUTTON) - // .whileTrue(new JoystickRecorder(m_robotSwerveDrive, - // () -> getDeadbandedDriverController().getLeftX(), - // () -> getDeadbandedDriverController().getLeftY(), - // () -> getDeadbandedDriverController().getRightX(), - // () -> getDeadbandedDriverController().getRightY(), - // "Taxi.txt")) - // .onFalse(new InstantCommand()); - - // new JoystickButton(getDeadbandedDriverController(), XboxController.LEFT_BUMPER_BUTTON) - // .onTrue(new JoystickPlayback(m_robotSwerveDrive, "Taxi.txt")) - // .onFalse(new InstantCommand()); // ! /* Speed */ new JoystickButton(getDeadbandedDriverController(), XboxController.RIGHT_BUMPER_BUTTON) // final .onTrue(new InstantCommand(() -> m_robotSwerveDrive.shiftUp())); @@ -304,12 +282,7 @@ public class RobotContainer { new Trigger(() -> getDeadbandedDriverController().getPOV() == 90) .onTrue(new InstantCommand(() -> m_robotSwerveDrive.shiftUpRot())); - // new JoystickButton(getDeadbandedDriverController(), XboxController.Y_BUTTON) - // .whileTrue(new InstantCommand(() -> - // m_robotSwerveDrive.driveWithInput(new Translation2d(0, 1), - // new Translation2d(0, 0), - // true), m_robotSwerveDrive)); - + //? /* Operator Buttons */ @@ -349,10 +322,6 @@ public class RobotContainer { //spins up shooter, no wind down DualJoystickButton(getDeadbandedOperatorController(), getVirtualOperatorController(), XboxController.LEFT_JOYSTICK_BUTTON) .onTrue(new InstantCommand(() -> m_robotShooter.spin(), m_robotShooter)); - - // DualJoystickButton(getDeadbandedOperatorController(), getVirtualOperatorController(), XboxController.RIGHT_JOYSTICK_BUTTON) - // .onTrue(new InstantCommand(() -> m_robotIntake.talonSpinIntakeMotor(), m_robotIntake)) - // .onFalse(new InstantCommand(() -> m_robotIntake.talonStopIntakeMotors(), m_robotIntake)); DualJoystickButton(getDeadbandedOperatorController(), getVirtualOperatorController(), XboxController.RIGHT_JOYSTICK_BUTTON) .onTrue(emergencyRetract); diff --git a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java index 8e00166..98feae6 100644 --- a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java +++ b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java @@ -92,13 +92,10 @@ public class SwerveDrive extends SubsystemBase { public void playbackDriveWithInput(Translation2d leftStick, Translation2d rightStick, boolean fieldRelative) { if (fieldRelative) { - double rot = 0; - // ! drift correction if (rightStick.getNorm() > 0.05) { rotTarget = gyro.getAngle(); - rot = rightStick.getX() * SwerveDriveConstants.ROTATION_SPEED; - // SmartDashboard.putBoolean("drift correction", false); + // SmartDashboard.putBoolean("drift correction", false); stopped = false; } else if(leftStick.getNorm() > 0.05) { if (!stopped) { @@ -107,8 +104,7 @@ public class SwerveDrive extends SubsystemBase { } // SmartDashboard.putBoolean("drift correction", true); - - rot = ((rotTarget - gyro.getAngle()) / 360) * SwerveDriveConstants.ROT_CORRECTION_SPEED; + } @@ -129,7 +125,6 @@ public class SwerveDrive extends SubsystemBase { Translation2d rightStick = new Translation2d(-rightX, rightY); if(fieldRelative) { - double rot = 0; if(rightStick.getNorm() > 0.5) { orientRotTarget = new Rotation2d(rightX, -rightY).minus(new Rotation2d(0,1)); @@ -139,7 +134,6 @@ public class SwerveDrive extends SubsystemBase { if(tmp.getDegrees() < 0) min*=-1; tmp = new Rotation2d(min * Math.PI / 180); - rot = tmp.getRadians(); // x x - y/x } Translation2d speed = leftStick.times(leftStick.getNorm() * speedAdjust); @@ -228,61 +222,18 @@ public class SwerveDrive extends SubsystemBase { // SmartDashboard.putNumber("Gyro", getGyroAngle()); } + private int GEAR = SwerveDriveConstants.SPEEDS.length /2; 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; - + if(GEAR > 0){ + GEAR--; + this.speedAdjust = SwerveDriveConstants.SPEEDS[GEAR]; } } - 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 (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_MIN; - } else { - this.speedAdjust = SwerveDriveConstants.Conversions.JOYSTICK_TO_METERS_PER_SECOND_SLOW; - SwerveDriveConstants.ROT_CORRECTION_SPEED = SwerveDriveConstants.CORRECTION_MIN; + if(GEAR < SwerveDriveConstants.SPEEDS.length){ + GEAR++; + this.speedAdjust = SwerveDriveConstants.SPEEDS[GEAR]; } }