From 20218af427fc99a188c100f537f863a59b7478c9 Mon Sep 17 00:00:00 2001
From: Kaya <95276965+kytpbs@users.noreply.github.com>
Date: Sun, 13 Oct 2024 11:27:36 +0300
Subject: [PATCH] Improve boost system

Now you can use both shoulder analog buttons to speedup or slow down the robot instead of just being able to speed it up
---
 .../robot/commands/DefaultDriveCommand.java   | 24 ++++++++++++++++++-
 1 file changed, 23 insertions(+), 1 deletion(-)

diff --git a/src/main/java/frc/robot/commands/DefaultDriveCommand.java b/src/main/java/frc/robot/commands/DefaultDriveCommand.java
index 92b4756..eb220fa 100644
--- a/src/main/java/frc/robot/commands/DefaultDriveCommand.java
+++ b/src/main/java/frc/robot/commands/DefaultDriveCommand.java
@@ -1,10 +1,15 @@
 package frc.robot.commands;
 
+import edu.wpi.first.math.MathUtil;
 import edu.wpi.first.wpilibj.XboxController;
 import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
 import frc.robot.subsystems.DriveSubsystem;
 
 public class DefaultDriveCommand extends SequentialCommandGroup {
+  // This is the minimum value the robot will slow down to when the left trigger is pushed.
+  static final double MIN_BOOST = -0.7; // Will constantly boost if higher than 0
+  static final double MAX_BOOST = 1.0;
+
   public DefaultDriveCommand(DriveSubsystem driveSubsystem, XboxController controller) {
     super(
         driveSubsystem.run(
@@ -13,6 +18,23 @@ public DefaultDriveCommand(DriveSubsystem driveSubsystem, XboxController control
                     controller.getLeftY(),
                     -controller.getLeftX(), // ! THIS IS INVERTED BECAUSE +y IS LEFT of robot
                     -controller.getRightX(), // ! THIS IS INVERTED BECAUSE + IS COUNTERCLOCKWISE
-                    controller.getRightTriggerAxis())));
+                    getBoostByValue(controller))));
+  }
+
+  /**
+   * Get the boost value for the drive command Boosts According to the shoulder analog inputs
+   * doesn't go less than 0.2 for the convince of the driver
+   *
+   * @param controller
+   * @return a double value between -0.8 and 1.0
+   */
+  private static double getBoostByValue(XboxController controller) {
+    var boostBy = controller.getRightTriggerAxis();
+    var reduceAmount = controller.getLeftTriggerAxis(); // between 0 and 1
+
+    double totalBoost = boostBy - reduceAmount;
+
+    totalBoost = MathUtil.clamp(totalBoost, MIN_BOOST, MAX_BOOST);
+    return totalBoost;
   }
 }