From 66b5383150aeb1b81a6d077c4c424c644a992034 Mon Sep 17 00:00:00 2001
From: Prog694 <prog692@gmail.com>
Date: Wed, 6 Sep 2023 13:44:50 -0400
Subject: [PATCH] Add robotrelative binding

---
 src/main/java/com/stuypulse/robot/RobotContainer.java      | 7 +------
 .../stuypulse/robot/commands/swerve/SwerveDriveDrive.java  | 6 ++++++
 2 files changed, 7 insertions(+), 6 deletions(-)

diff --git a/src/main/java/com/stuypulse/robot/RobotContainer.java b/src/main/java/com/stuypulse/robot/RobotContainer.java
index c0f7c405..03c6d65b 100644
--- a/src/main/java/com/stuypulse/robot/RobotContainer.java
+++ b/src/main/java/com/stuypulse/robot/RobotContainer.java
@@ -121,17 +121,11 @@ private void configureButtonBindings() {
     }
 
     private void configureDriverBindings() {
-        // wing
-        // driver.getSelectButton().onTrue(new WingToggle());
-
         // arm
         driver.getLeftTriggerButton()
             .whileTrue(new RobotScore());
         driver.getBottomButton()
             .whileTrue(new RobotScore());
-        driver.getLeftBumper()
-            .whileTrue(new RobotRelease())
-            .onFalse(new WaitCommand(0.5).andThen(new IntakeStop()));
         driver.getRightTriggerButton()
             .whileTrue(new RobotRelease())
             .onFalse(new WaitCommand(0.5).andThen(new IntakeStop()));
@@ -143,6 +137,7 @@ private void configureDriverBindings() {
 
         // swerve
         driver.getLeftButton().whileTrue(new SwerveDriveAlignThenBalance());
+        // left bumper -> robot relative
 
         // odometry
         driver.getDPadUp().onTrue(new OdometryRealign(Rotation2d.fromDegrees(180)));
diff --git a/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveDrive.java b/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveDrive.java
index e7e155a3..b5362211 100644
--- a/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveDrive.java
+++ b/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveDrive.java
@@ -25,6 +25,7 @@
 import com.stuypulse.robot.subsystems.swerve.SwerveDrive;
 
 import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.kinematics.ChassisSpeeds;
 import edu.wpi.first.wpilibj.Timer;
 import edu.wpi.first.wpilibj2.command.CommandBase;
 
@@ -122,6 +123,11 @@ public void execute() {
         if (plant.isEngaged() || driver.getRawStartButton() || driver.getRawSelectButton()) {
             swerve.setXMode();
         }
+
+        // if in robot relative mode, set raw chassis speed
+        else if (driver.getRawLeftBumper()) {
+            swerve.setChassisSpeeds(new ChassisSpeeds(speed.get().y, -speed.get().x, -angularVel));
+        }
         
         // use the angularVelocity for drive otherwise
         else {