From 021830d4cd595c10a9486a9f3f87fb0ac93b541a Mon Sep 17 00:00:00 2001
From: 178Laptop <programming@farmingtonrobotics.org>
Date: Thu, 19 Oct 2023 16:35:02 -0400
Subject: [PATCH] for fall fest

NEED TO TEST
---
 src/main/java/frc/robot/Constants.java        |  3 +++
 src/main/java/frc/robot/RobotContainer.java   | 18 ++++++++++++-----
 .../java/frc/robot/subsystems/Drivetrain.java | 20 ++++++++++++++++++-
 3 files changed, 35 insertions(+), 6 deletions(-)

diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java
index cd758b8..8b2993c 100644
--- a/src/main/java/frc/robot/Constants.java
+++ b/src/main/java/frc/robot/Constants.java
@@ -87,6 +87,9 @@ private static Matrix<N3, N1> m_createVisionTrustMatrix() {
       matrix.set(2, 0, 5); // Theta
       return matrix;
     }
+
+    public static final double slowModeMult = 0.2;
+    public static final double defaultSpeedMult = 0.6;
   }
 
   public static class ClawConstants {
diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
index 63d98ba..66fee7d 100644
--- a/src/main/java/frc/robot/RobotContainer.java
+++ b/src/main/java/frc/robot/RobotContainer.java
@@ -98,13 +98,16 @@ private void configureBindings() {
         // .whileFalse(
         //   Commands.run(() -> m_drivetrain.setSpeedMult(0.05))
         // );
-
         m_driverController.leftTrigger()
             .whileTrue(
-                Commands.run(() -> m_drivetrain.setSpeedMult(0.2)))
-            .whileFalse(Commands.run(() -> m_drivetrain.setSpeedMult(1)));
-        // m_driverController.rightTrigger()
-        //     .whileTrue(Commands.run(() -> m_drivetrain.setSpeedMult(1)));
+                Commands.run(() -> m_drivetrain.setSlowMode(true)))
+            .whileFalse(
+                Commands.run(() -> m_drivetrain.setSlowMode(false)));
+        m_driverController.rightTrigger()
+            .whileTrue(
+                Commands.run(() -> m_drivetrain.setFastMode(true)))
+            .whileFalse(
+                Commands.run(() -> m_drivetrain.setFastMode(false)));
 
         m_auxBox.b().onTrue(
             m_arm.setPosition(ArmPosition.HOME));
@@ -131,6 +134,11 @@ private void configureBindings() {
         m_auxBox.leftStick().onTrue(m_lights.runYellow());
         m_auxBox.rightStick().onTrue(m_lights.runPurple());
         m_auxBox.leftTrigger().onTrue(m_lights.runDefaultColor());
+        m_auxBox.rightTrigger().
+        whileTrue(
+            Commands.run(() -> m_drivetrain.setPauseMainControl(true)))
+        .whileFalse(
+            Commands.run(() -> m_drivetrain.setPauseMainControl(false)));
 
         // new Combo(m_auxBox.getHID())
         //     .quarterCircleKick()
diff --git a/src/main/java/frc/robot/subsystems/Drivetrain.java b/src/main/java/frc/robot/subsystems/Drivetrain.java
index 45dbe78..4871949 100644
--- a/src/main/java/frc/robot/subsystems/Drivetrain.java
+++ b/src/main/java/frc/robot/subsystems/Drivetrain.java
@@ -62,7 +62,11 @@ public class Drivetrain extends SubsystemBase {
   private final Field2d m_field = new Field2d();
   private final NetworkTable m_limelight = NetworkTableInstance.getDefault().getTable("limelight");
 
-  private double m_speedMult = 1;
+  private boolean slowMode = false;
+  private boolean fastMode = false;
+  private boolean pauseMainControl = false;
+
+  private double m_speedMult = DriveConstants.defaultSpeedMult;
 
   /* Creates a new Drivetrain. */
   public Drivetrain() {
@@ -230,6 +234,8 @@ public void setWheelSpeeds(double leftSpeed, double rightSpeed) {
 
   public Command arcadeDrive(DoubleSupplier forward, DoubleSupplier rot, double deadzone) {
     return this.run(() -> {
+      if(pauseMainControl) return;
+      m_speedMult = fastMode ? 1 : (slowMode ? DriveConstants.slowModeMult : DriveConstants.defaultSpeedMult);
       double x = MathUtil.applyDeadband(forward.getAsDouble(), deadzone)
           * (DriveConstants.kMaxSpeedMetersPerSecond * m_speedMult);
       double z = MathUtil.applyDeadband(rot.getAsDouble(), deadzone)
@@ -283,6 +289,18 @@ public Command cheesyDrive(DoubleSupplier forward, DoubleSupplier rot, Trigger q
     });
   }
 
+  public void setSlowMode(boolean slowMode) {
+      this.slowMode = slowMode;
+  }
+
+  public void setFastMode(boolean fastMode) {
+      this.fastMode = fastMode;
+  }
+
+  public void setPauseMainControl(boolean pauseMainControl) {
+      this.pauseMainControl = pauseMainControl;
+  }
+
   @Override
   public void periodic() {
     // This method will be called once per scheduler run