From 2a8b7b867ee4e90669e22b65f6a364886ab08d49 Mon Sep 17 00:00:00 2001
From: Evan Grinnell <155865196+S0L0GUY@users.noreply.github.com>
Date: Sun, 10 Nov 2024 09:35:28 -0800
Subject: [PATCH] Add Slow Mode and Fix Controller Inputs

---
 src/main/java/frc/robot/RobotContainer.java        | 3 +--
 src/main/java/frc/robot/commands/Drive.java        | 7 +++++--
 src/main/java/frc/robot/subsystems/Drivetrain.java | 9 +++++++--
 3 files changed, 13 insertions(+), 6 deletions(-)

diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
index 23a2276..6ba0085 100644
--- a/src/main/java/frc/robot/RobotContainer.java
+++ b/src/main/java/frc/robot/RobotContainer.java
@@ -28,8 +28,7 @@ public class RobotContainer {
   private final Hopper subHopper = new Hopper();
   private final Shooter subShooter = new Shooter();
   private final Stager subStager = new Stager();
-  private final Drive com_Drive = new Drive(subDrivetrain, m_driverController.axis_LeftY,
-      m_driverController.axis_RightX);
+  private final Drive com_Drive = new Drive(subDrivetrain, m_driverController.axis_RightX, m_driverController.axis_LeftY, m_driverController.btn_LeftBumper);
   private final IntakeGround com_IntakeGround = new IntakeGround(subIntake, subStager);
   private final PrepShooter com_PrepShooter = new PrepShooter(subShooter);
   private final HasGP com_StageGP = new HasGP(subStager);
diff --git a/src/main/java/frc/robot/commands/Drive.java b/src/main/java/frc/robot/commands/Drive.java
index 22ac3ea..cc9115c 100644
--- a/src/main/java/frc/robot/commands/Drive.java
+++ b/src/main/java/frc/robot/commands/Drive.java
@@ -7,6 +7,7 @@
 import java.util.function.DoubleSupplier;
 
 import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.button.Trigger;
 import frc.robot.subsystems.Drivetrain;
 
 public class Drive extends Command {
@@ -15,12 +16,14 @@ public class Drive extends Command {
   Drivetrain globalDrivetrain;
   DoubleSupplier globalForwardSpeed;
   DoubleSupplier globalRotationSpeed;
+  Trigger globalSlowMode;
 
-  public Drive(Drivetrain passedDrivetrain, DoubleSupplier passedForwardSpeed, DoubleSupplier passedRotationSpeed) {
+  public Drive(Drivetrain passedDrivetrain, DoubleSupplier passedForwardSpeed, DoubleSupplier passedRotationSpeed, Trigger btn_LeftBumper) {
     // Use addRequirements() here to declare subsystem dependencies.
     globalDrivetrain = passedDrivetrain;
     globalForwardSpeed = passedForwardSpeed;
     globalRotationSpeed = passedRotationSpeed;
+    globalSlowMode = btn_LeftBumper;
 
     addRequirements(globalDrivetrain);
   }
@@ -32,7 +35,7 @@ public void initialize() {}
   // Called every time the scheduler runs while the command is scheduled.
   @Override
   public void execute() {
-    globalDrivetrain.setDrivetrainSpeed(globalForwardSpeed.getAsDouble(), globalRotationSpeed.getAsDouble());
+    globalDrivetrain.setDrivetrainSpeed(globalForwardSpeed.getAsDouble(), globalRotationSpeed.getAsDouble(), globalSlowMode.getAsBoolean());
   }
 
   // Called once the command ends or is interrupted.
diff --git a/src/main/java/frc/robot/subsystems/Drivetrain.java b/src/main/java/frc/robot/subsystems/Drivetrain.java
index 9d7fd2f..75e122e 100644
--- a/src/main/java/frc/robot/subsystems/Drivetrain.java
+++ b/src/main/java/frc/robot/subsystems/Drivetrain.java
@@ -31,11 +31,16 @@ public Drivetrain() {
    *                        drivetrain.
    * @param rotationSpeed   The rotation speed to apply to the drivetrain.
    */
-  public void setDrivetrainSpeed(double forwardVelocity, double rotationSpeed) {
+  public void setDrivetrainSpeed(double forwardVelocity, double rotationSpeed, boolean leftBumperDown) {
+    // Divide speed in half if slow mode is activated
+    if (leftBumperDown) {
+      forwardVelocity = forwardVelocity * 0.5;
+      rotationSpeed = rotationSpeed * 0.5;
+    }
+
     // Set right velocity
     frontRightMotor.set(forwardVelocity - rotationSpeed);
     backRightMotor.set(forwardVelocity - rotationSpeed);
-
     // Set left velocity
     frontLeftMotor.set(forwardVelocity + rotationSpeed);
     backLeftMotor.set(forwardVelocity + rotationSpeed);