From b0557ec3230cef768187d51a51620eacd89cf8ce Mon Sep 17 00:00:00 2001
From: David Tian <dtian.github@outlook.com>
Date: Sun, 10 Nov 2024 11:53:36 -0800
Subject: [PATCH] Before q77

---
 .../com/team841/calliope/RobotContainer.java  |  6 ++++--
 .../com/team841/calliope/drive/BioDrive.java  | 19 +++++++++++++++++--
 2 files changed, 21 insertions(+), 4 deletions(-)

diff --git a/src/main/java/com/team841/calliope/RobotContainer.java b/src/main/java/com/team841/calliope/RobotContainer.java
index a961337..7be5a75 100644
--- a/src/main/java/com/team841/calliope/RobotContainer.java
+++ b/src/main/java/com/team841/calliope/RobotContainer.java
@@ -138,7 +138,9 @@ public RobotContainer() {
                 () -> -sticksPS5[0].getLeftY() * Swerve.MaxSpeed,
                 () -> -sticksPS5[0].getLeftX() * Swerve.MaxSpeed,
                 () -> -sticksPS5[0].getRightX() * Swerve.MaxAngularRate,
-                () -> sticksPS5[0].L2().getAsBoolean());
+                () -> sticksPS5[0].L2().getAsBoolean(),
+                () -> sticksXbox[0].rightBumper().getAsBoolean(),
+                () -> -sticksXbox[0].getRightX() * 2 * Math.PI);
 
 
 
@@ -285,7 +287,7 @@ private void configureDuoStick() {
                                 () -> shooter.isShooting()))
                 .onFalse(new InstantCommand(indexer::stopIndexer));
         sticksXbox[0]
-                .rightBumper()
+                .start()
                 .onTrue(
                         new SequentialCommandGroup(
                                 new InstantCommand(indexer::stopIndexer),
diff --git a/src/main/java/com/team841/calliope/drive/BioDrive.java b/src/main/java/com/team841/calliope/drive/BioDrive.java
index 95d5660..f993c72 100644
--- a/src/main/java/com/team841/calliope/drive/BioDrive.java
+++ b/src/main/java/com/team841/calliope/drive/BioDrive.java
@@ -24,6 +24,15 @@ public class BioDrive extends Command {
 
      */
 
+    public BioDrive(Drivetrain drivetrain, DoubleSupplier velocityXGetter, DoubleSupplier velocityYGetter, DoubleSupplier velocityOmegaGetter, BooleanSupplier faceSpeakerGetter, BooleanSupplier overRideTurn, DoubleSupplier overrideVelocity) {
+
+        this(drivetrain, velocityXGetter, velocityYGetter, velocityOmegaGetter, faceSpeakerGetter);
+
+        this.Override = overRideTurn;
+        this.vOverride = overrideVelocity;
+
+    }
+
 
     public BioDrive(Drivetrain drivetrain, DoubleSupplier velocityXGetter, DoubleSupplier velocityYGetter, DoubleSupplier velocityOmegaGetter, BooleanSupplier faceSpeakerGetter) {
 
@@ -46,14 +55,16 @@ public BioDrive(Drivetrain drivetrain, DoubleSupplier velocityXGetter, DoubleSup
 
     private Command shootCommand;
 
-    private DoubleSupplier mVelocityX, mVelocityY, mVelocityOmega;
-    private BooleanSupplier mFaceSpeaker, mAutoShoot;
+    private DoubleSupplier mVelocityX, mVelocityY, mVelocityOmega, vOverride;
+    private BooleanSupplier mFaceSpeaker, mAutoShoot, Override;
 
     private boolean faceSpeaker = false;
     private double velocity_y = 0.0;
     private double velocity_x = 0.0;
     private double velocity_omega = 0.0;
     private boolean autoShoot = false;
+    private boolean overRide = false;
+    private double overrideVelocity = 0.0;
 
     private Drivetrain drivetrain;
 
@@ -81,6 +92,8 @@ public void execute() {
         this.velocity_y = mVelocityY.getAsDouble();
         this.velocity_omega = mVelocityOmega.getAsDouble();
         //this.autoShoot = mAutoShoot.getAsBoolean();
+        this.overRide = Override.getAsBoolean();
+        this.overrideVelocity = vOverride.getAsDouble();
 
         Logger.recordOutput("BioDrive/FaceSpeaker", this.faceSpeaker);
         Logger.recordOutput("BioDrive/velocityX", this.velocity_x);
@@ -97,6 +110,8 @@ public void execute() {
             this.drivetrain.setControl(fieldCentricFacingAngle.withVelocityX(-this.velocity_x).withVelocityY(-this.velocity_y).withTargetDirection(angle));
 
              */
+        } else if (overRide) {
+            this.drivetrain.setControl(fieldCentricDrive.withVelocityX(-this.velocity_x).withVelocityY(-this.velocity_y).withRotationalRate(this.overrideVelocity));
         } else {
             this.drivetrain.setControl(fieldCentricDrive.withVelocityX(-this.velocity_x).withVelocityY(-this.velocity_y).withRotationalRate(this.velocity_omega));
         }