Auto shifting

src/main/java/frc/robot/OI.java

@@ -20,6 +20,7 @@ private static class PORT {
     private static class DRIVER_MAP {
         private static final int SHIFT_UP_BUTTON = 3;
         private static final int SHIFT_DOWN_BUTTON = 2;
+        private static final int LOW_SHIFT_BUTTON = 1;
     }
 
     /** Operator Button Map */
@@ -34,6 +35,8 @@ private static class OPERATOR_MAP {
     public static final JoystickButton shiftUpButton = new JoystickButton(rightJoystick, DRIVER_MAP.SHIFT_UP_BUTTON);
     public static final JoystickButton shiftDownButton = new JoystickButton(rightJoystick, DRIVER_MAP.SHIFT_DOWN_BUTTON);
 
+    public static final JoystickButton lowShiftButton = new JoystickButton(leftJoystick, DRIVER_MAP.LOW_SHIFT_BUTTON);
+
 
     public static DoubleSupplier leftDriveSupplier = () -> {
         double raw = leftJoystick.getY();

src/main/java/frc/robot/RobotContainer.java

@@ -10,6 +10,7 @@
 import frc.robot.commands.instant.ShiftDownCommand;
 import frc.robot.commands.instant.ShiftUpCommand;
 import frc.robot.subsystems.Drivetrain;
+import frc.robot.subsystems.Drivetrain.Gear;
 
 public class RobotContainer {
 
@@ -22,8 +23,16 @@ public RobotContainer() {
   }
 
   private void configureBindings() {
-    OI.shiftUpButton.onTrue(new ShiftUpCommand(drivetrain));
-    OI.shiftDownButton.onTrue(new ShiftDownCommand(drivetrain));
+    //OI.shiftUpButton.onTrue(new ShiftUpCommand(drivetrain));
+    //OI.shiftDownButton.onTrue(new ShiftDownCommand(drivetrain));
+
+    // Not requiring drivetrain because we dont want to interupt DriveCommand
+    OI.lowShiftButton.whileTrue(Commands.runOnce(() -> {
+      drivetrain.disableAutoShifting();
+      drivetrain.setGear(Gear.LOW);
+    })).onFalse(Commands.runOnce(() -> {
+      drivetrain.enableAutoShifting();
+    }));
   }
 
   public Command getAutonomousCommand() {

src/main/java/frc/robot/commands/DriveCommand.java

@@ -27,6 +27,7 @@ public void initialize() {
     @Override
     public void execute() {
         drivetrain.setOutput(leftSupplier.getAsDouble(), rightSupplier.getAsDouble());
+        drivetrain.shiftGears();
     }
 
     @Override

src/main/java/frc/robot/subsystems/Drivetrain.java

@@ -4,6 +4,7 @@
 import com.revrobotics.RelativeEncoder;
 
 import edu.wpi.first.wpilibj.DoubleSolenoid;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 
 public class Drivetrain extends SubsystemBase {
@@ -15,6 +16,8 @@ public class Drivetrain extends SubsystemBase {
 
     Gear currentGear;
 
+    boolean autoShifting;
+
     /**
      * Encoder to inches conversion factor
      * <pre>
@@ -42,6 +45,8 @@ public Drivetrain(CANSparkMax leftLeadController, CANSparkMax rightLeadControlle
         rightDriveEncoder.setPositionConversionFactor(CONVERSION_FACTOR);
         rightDriveEncoder.setPosition(0);
         rightDriveEncoder.setInverted(false);   
+
+        autoShifting = true;
     }
 
     public enum Gear {
@@ -78,5 +83,40 @@ public void setGear(Gear gear) {
     public Gear getGear() {
         return currentGear;
     }
+
+    public void disableAutoShifting() {
+        autoShifting = false;
+    }
+
+    public void enableAutoShifting() {
+        autoShifting = true;
+    }
+
+    /**
+     * Pulls speed to figure out when to shift gear
+     * MUST BE RUN IN COMMAND excute()
+     */
+    public void shiftGears() {
+
+        if(!autoShifting) return;
+
+        double UPSHIFT_SPEED = 1200;
+        double DOWNSHIFT_SPEED = 800;
+
+        double currentSpeed = (leftDriveEncoder.getVelocity() + rightDriveEncoder.getVelocity()) / 2;
+
+        SmartDashboard.putNumber("Avg Speed", currentSpeed);
 
-}
+        if(currentSpeed >= UPSHIFT_SPEED) {
+            currentGear = Gear.HIGH;
+            shiftSolenoid.set(Gear.HIGH.value);
+        }
+
+        if(currentSpeed <= DOWNSHIFT_SPEED) {
+            currentGear = Gear.LOW;
+            shiftSolenoid.set(Gear.LOW.value);
+        }
+
+
+    }
+}