Added 12 speed transmission, no clutch (for now...)

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

@@ -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

@@ -310,7 +310,6 @@ public class RobotContainer {
                                     true);
             }, m_robotSwerveDrive)
             .withName("SwerveDrive DefaultCommand"));
-           m_robotSwerveDrive.setToSlow();
 
         // ! Swerve Drive Default Command (Orientation Rotation)
         // m_robotSwerveDrive.setDefaultCommand(new RunCommand(() -> {
@@ -505,40 +504,6 @@ public class RobotContainer {
         new JoystickButton(getVirtualDriverController(), XboxController.START_BUTTON)
             .onTrue(new InstantCommand(() -> m_robotSwerveDrive.resetGyroRightAmp()))
             .onFalse(new InstantCommand(() -> m_robotSwerveDrive.add180()));
-
-
-       // *  /* D-Pad Stuff */
-    //    new Trigger(() -> getVirtualDriverController().getRawAxis(XboxController.TOP_BOTTOM_DPAD_AXIS) > 0.9)
-    //         .onTrue(new InstantCommand(() -> m_robotSwerveDrive.driveWithInput(new Translation2d(0, 1),
-    //                                                                            new Translation2d(0, 0),
-    //                                                                            true)))
-    //         .onFalse(new InstantCommand(() -> m_robotSwerveDrive.driveWithInput(new Translation2d(0, 0), 
-    //                                                                             new Translation2d(0, 0), 
-    //                                                                             true)));
-
-    //    new Trigger(() -> getVirtualDriverController().getRawAxis(XboxController.TOP_BOTTOM_DPAD_AXIS) > -0.9)
-    //         .onTrue(new InstantCommand(() -> m_robotSwerveDrive.driveWithInput(new Translation2d(0, -1),
-    //                                                                            new Translation2d(0, 0),
-    //                                                                            true)))
-    //         .onFalse(new InstantCommand(() -> m_robotSwerveDrive.driveWithInput(new Translation2d(0, 0), 
-    //                                                                             new Translation2d(0, 0), 
-    //                                                                             true)));
-
-    //    new Trigger(() -> getVirtualDriverController().getRawAxis(XboxController.LEFT_RIGHT_DPAD_AXIS) > 0.9)
-    //         .onTrue(new InstantCommand(() -> m_robotSwerveDrive.driveWithInput(new Translation2d(1, 0),
-    //                                                                            new Translation2d(0, 0),
-    //                                                                            true)))
-    //         .onFalse(new InstantCommand(() -> m_robotSwerveDrive.driveWithInput(new Translation2d(0, 0), 
-    //                                                                             new Translation2d(0, 0), 
-    //                                                                             true)));
-
-    //    new Trigger(() -> getVirtualDriverController().getRawAxis(XboxController.LEFT_RIGHT_DPAD_AXIS) > -0.9)
-    //         .onTrue(new InstantCommand(() -> m_robotSwerveDrive.driveWithInput(new Translation2d(-1, 0),
-    //                                                                            new Translation2d(0, 0),
-    //                                                                            true)))
-    //         .onFalse(new InstantCommand(() -> m_robotSwerveDrive.driveWithInput(new Translation2d(0, 0), 
-    //                                                                             new Translation2d(0, 0), 
-    //                                                                             true)));
         
         // ! /* Auto Recording */
         new JoystickButton(m_autoRecorderXbox, XboxController.LEFT_BUMPER_BUTTON)

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;
     }
   }