for fall fest

NEED TO TEST
team178 · Oct 19, 2023 · 021830d · 021830d
1 parent 4a7ab37
commit 021830d
Show file tree

Hide file tree

Showing 3 changed files with 35 additions and 6 deletions.
diff --git 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
@@ -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
@@ -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