diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index 83ac014..80ad347 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -24,9 +24,17 @@ 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; + 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; + public static double ROT_CORRECTION_SPEED = 10; // MIN_ROT_SPEED; + + 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; diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 1b6fb4c..eac33f3 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -118,25 +118,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_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 fcaadb9..f65dd00 100644 --- a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java +++ b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java @@ -16,6 +16,7 @@ import edu.wpi.first.wpilibj2.command.SubsystemBase; import edu.wpi.first.math.kinematics.SwerveModulePosition; import frc4388.robot.Constants.SwerveDriveConstants; import frc4388.utility.RobotGyro; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; public class SwerveDrive extends SubsystemBase { @@ -37,7 +38,7 @@ public class SwerveDrive extends SubsystemBase { public double speedAdjust = SwerveDriveConstants.Conversions.JOYSTICK_TO_METERS_PER_SECOND_SLOW; // * slow by default - public Rotation2d rotTarget = new Rotation2d(); + public double rotTarget = 0.0; public ChassisSpeeds chassisSpeeds = new ChassisSpeeds(); /** Creates a new SwerveDrive. */ @@ -52,15 +53,26 @@ 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) { double rot = 0; - if (rightStick.getNorm() > 0.1) { - rotTarget = gyro.getRotation2d(); - rot = rightStick.getX(); - } else { - rot = rotTarget.minus(gyro.getRotation2d()).getRadians(); + + if (rightStick.getNorm() > 0.05) { + rotTarget = gyro.getAngle(); + rot = rightStick.getX() * SwerveDriveConstants.ROTATION_SPEED; + SmartDashboard.putBoolean("drift correction", false); + 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. @@ -95,7 +107,7 @@ public class SwerveDrive extends SubsystemBase { public void resetGyro() { gyro.reset(); - rotTarget = new Rotation2d(0); + rotTarget = 0.0; } public void stopModules() { @@ -113,11 +125,60 @@ public class SwerveDrive extends SubsystemBase { // This method will be called once per scheduler run } - public void toggleGear() { - if (this.speedAdjust == SwerveDriveConstants.Conversions.JOYSTICK_TO_METERS_PER_SECOND_SLOW) { + 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 (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; } } 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