Bumpers and drift protection

src/main/java/frc4388/robot/Constants.java

@@ -22,11 +22,12 @@ import frc4388.utility.LEDPatterns;
  */
 public final class Constants {
   public static final class SwerveDriveConstants {
-    public static final double ROTATION_SPEED = 0.1;
+    public static final double ROTATION_SPEED = 3;
     public static final double WHEEL_SPEED = 0.1;
     public static final double WIDTH = 22;
     public static final double HEIGHT = 22;
-    public static final double JOYSTICK_TO_METERS_PER_SECOND = 11;
+    public static final double JOYSTICK_TO_METERS_PER_SECOND_FAST = 11;
+    public static final double JOYSTICK_TO_METERS_PER_SECOND_SLOW = 2;
     public static final double MAX_SPEED_FEET_PER_SEC = 20;
     public static final double SPEED_FEET_PER_SECOND_AT_FULL_POWER = 20;
       //IDs

src/main/java/frc4388/robot/RobotContainer.java

@@ -77,16 +77,21 @@ public class RobotContainer {
     /* Driver Buttons */
     /*new JoystickButton(getDriverJoystick(), XboxController.Y_BUTTON)
       .whenPressed(() -> m_robotSwerveDrive.resetGyro());*/
+
+    new JoystickButton(getDriverJoystick(), XboxController.LEFT_BUMPER_BUTTON)
+      .whenPressed(() -> m_robotSwerveDrive.highSpeed(false));
+
+
+    new JoystickButton(getDriverJoystick(), XboxController.RIGHT_BUMPER_BUTTON)
+      .whenPressed(() -> m_robotSwerveDrive.highSpeed(true));
+
+
+
     /* Operator Buttons */
     // activates "Lit Mode"
     new JoystickButton(getOperatorJoystick(), XboxController.A_BUTTON)
         .whenPressed(() -> m_robotLED.setPattern(LEDPatterns.LAVA_RAINBOW))
         .whenReleased(() -> m_robotLED.setPattern(LEDConstants.DEFAULT_PATTERN));
-
-    // new JoystickButton(getDriverJoystick(), XboxController.A_BUTTON)
-    // .whenPressed(() -> {
-    //   m_robotSwerveDrive.m_gyro.setYaw(0);
-    // });
   }
 
   /**

src/main/java/frc4388/robot/subsystems/SwerveDrive.java

@@ -16,6 +16,7 @@ import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.math.kinematics.ChassisSpeeds;
 import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
+import edu.wpi.first.math.kinematics.SwerveDriveOdometry;
 import edu.wpi.first.math.kinematics.SwerveModuleState;
 import edu.wpi.first.math.util.Units;
 import edu.wpi.first.wpilibj.interfaces.Gyro;
@@ -54,6 +55,9 @@ public class SwerveDrive extends SubsystemBase {
   public SwerveModule[] modules;
   public WPI_PigeonIMU m_gyro; //TODO Add Gyro Lol
 
+  public double speedAdjust = SwerveDriveConstants.JOYSTICK_TO_METERS_PER_SECOND_SLOW;
+  public boolean ignoreAngles;
+
   public SwerveDrive(WPI_TalonFX leftFrontSteerMotor,WPI_TalonFX leftFrontWheelMotor,WPI_TalonFX rightFrontSteerMotor,WPI_TalonFX rightFrontWheelMotor,
   WPI_TalonFX leftBackSteerMotor,WPI_TalonFX leftBackWheelMotor,WPI_TalonFX rightBackSteerMotor,WPI_TalonFX rightBackWheelMotor, CANCoder leftFrontEncoder,
   CANCoder rightFrontEncoder,
@@ -93,6 +97,8 @@ public class SwerveDrive extends SubsystemBase {
  */
   public void driveWithInput(double[] speeds, double rot, boolean fieldRelative)
   {
+      if (speeds[0] == 0 && speeds[1] == 0) ignoreAngles = true;
+      else ignoreAngles = false;
       // Temporary janky raw joysticks
       float[] driveController = new float[HAL.kMaxJoystickAxes];
       HAL.getJoystickAxes((byte) 0, driveController);
@@ -127,18 +133,18 @@ public class SwerveDrive extends SubsystemBase {
       // 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 xSpeedMetersPerSecond = -speeds[0] * speedAdjust;
-      double ySpeedMetersPerSecond = speeds[1] * SwerveDriveConstants.JOYSTICK_TO_METERS_PER_SECOND;
+      double ySpeedMetersPerSecond = speeds[1] * speedAdjust;
       SwerveModuleState[] states =
           kinematics.toSwerveModuleStates(
               fieldRelative
-                ? ChassisSpeeds.fromFieldRelativeSpeeds(xSpeedMetersPerSecond, ySpeedMetersPerSecond, rot*3, m_gyro.getRotation2d())
+                ? ChassisSpeeds.fromFieldRelativeSpeeds(xSpeedMetersPerSecond, ySpeedMetersPerSecond, rot * SwerveDriveConstants.ROTATION_SPEED, m_gyro.getRotation2d())
-                : new ChassisSpeeds(xSpeedMetersPerSecond, ySpeedMetersPerSecond, rot*3));
+                : new ChassisSpeeds(xSpeedMetersPerSecond, ySpeedMetersPerSecond, rot * SwerveDriveConstants.ROTATION_SPEED));
        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);
+          module.setDesiredState(state, ignoreAngle);
     }
   }
   
@@ -151,4 +157,12 @@ public class SwerveDrive extends SubsystemBase {
     super.periodic();
   }
 
+  public void highSpeed(boolean shift){
+    if (shift){
+      speedAdjust = SwerveDriveConstants.JOYSTICK_TO_METERS_PER_SECOND_FAST;
+    }
+    else{
+      speedAdjust = SwerveDriveConstants.JOYSTICK_TO_METERS_PER_SECOND_SLOW;
+    }
+  }
 }

src/main/java/frc4388/robot/subsystems/SwerveModule.java

@@ -71,7 +71,7 @@ public class SwerveModule extends SubsystemBase {
    * Set the speed + rotation of the swerve module from a SwerveModuleState object
    * @param desiredState - A SwerveModuleState representing the desired new state of the module
    */
-  public void setDesiredState(SwerveModuleState desiredState) {
+  public void setDesiredState(SwerveModuleState desiredState, boolean ignoreAngle) {
     Rotation2d currentRotation = getAngle();
     
     SmartDashboard.putNumber("Motor " + angleMotor.getDeviceID(), currentRotation.getDegrees());
@@ -85,7 +85,9 @@ public class SwerveModule extends SubsystemBase {
     // Convert the CANCoder from it's position reading back to ticks
     double currentTicks = canCoder.getPosition() / canCoder.configGetFeedbackCoefficient();
     double desiredTicks = currentTicks + deltaTicks;
+    if (!ignoreAngle){
       angleMotor.set(TalonFXControlMode.Position, desiredTicks);
+    }
 
 
     double feetPerSecond = Units.metersToFeet(state.speedMetersPerSecond);