Merge branch 'Denver-Comp' into Cleanup

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[] 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 FAST_SPEED = 1.0;
+        // public static final double[] SPEEDS = {0.5, 1.0, 4.0};
-        public static final double TURBO_SPEED = 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;

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 {
                                     true);
             }, m_robotSwerveDrive)
             .withName("SwerveDrive DefaultCommand"));
-           m_robotSwerveDrive.setToSlow();
 
         // ! Swerve Drive Default Command (Orientation Rotation)
         // m_robotSwerveDrive.setDefaultCommand(new RunCommand(() -> {

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) {
+    if(GEAR > 0){
-      
+      GEAR--;
-    } else if (Math.abs(this.speedAdjust - SwerveDriveConstants.FAST_SPEED) < .01) {
+      this.speedAdjust = SwerveDriveConstants.SPEEDS[GEAR];
-      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) {
+    if(GEAR < SwerveDriveConstants.SPEEDS.length){
-      this.speedAdjust = SwerveDriveConstants.FAST_SPEED;
+      GEAR++;
-    } else if (Math.abs(this.speedAdjust - SwerveDriveConstants.FAST_SPEED) < .01) {
+      this.speedAdjust = SwerveDriveConstants.SPEEDS[GEAR];
-      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;
     }
   }