From 172c72c33f95947dae86b17446107dc87662e586 Mon Sep 17 00:00:00 2001
From: Anay Nagar <anay.nagar@outlook.com>
Date: Fri, 8 Mar 2024 23:45:20 -0800
Subject: [PATCH] Changed controls for Driver and Operator

---
 src/main/java/frc/robot/RobotContainer.java | 43 ++++++++++++++-------
 1 file changed, 29 insertions(+), 14 deletions(-)

diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
index 32b79ef..ef47d21 100644
--- a/src/main/java/frc/robot/RobotContainer.java
+++ b/src/main/java/frc/robot/RobotContainer.java
@@ -129,7 +129,7 @@ public RobotContainer() {
                         .getLeftTriggerAxis()
                         * IOConstants.kSlowModeScalar)
                     / 2,
-                !m_driverController.getRightBumper()),
+                !m_driverController.getLeftBumper()),
             m_robotDrive));
   }
 
@@ -151,15 +151,13 @@ private void configureBindings() {
             new NoteOuttakeCommand(m_intakeSubsystem)))
         .onFalse(new ShooterSetSpeedCommand(m_shooterSubsystem, ShootSpeed.Off));
 
-    new JoystickButton(m_operatorController, Button.kA.value)
-        .onTrue(new InstantCommand(() -> m_climberSubsystem.forward(),
-            m_climberSubsystem));
-    new JoystickButton(m_operatorController, Button.kB.value)
-        .onTrue(new InstantCommand(() -> m_climberSubsystem.reverse(),
-            m_climberSubsystem));
+    new JoystickButton(m_driverController, Button.kRightBumper.value)
+        .onTrue(new IntakeArmPositionCommand(m_intakeSubsystem, ArmPosition.Amp))
+        .onFalse(new IntakeArmPositionCommand(m_intakeSubsystem, ArmPosition.Retracted));
 
+    // Intake, Driver Controller Right Trigger
     new Trigger(() -> {
-      return m_driverController.getLeftTriggerAxis() > 0.5;
+      return m_driverController.getRightTriggerAxis() > 0.5;
     }).whileTrue(
         new SequentialCommandGroup(
             new IntakeArmPositionCommand(m_intakeSubsystem, ArmPosition.Extended),
@@ -167,13 +165,30 @@ private void configureBindings() {
             new IntakeArmPositionCommand(m_intakeSubsystem, ArmPosition.Retracted)))
         .onFalse(new IntakeArmPositionCommand(m_intakeSubsystem, ArmPosition.Retracted));
 
+    // Outtake, Operator Controller Right Trigger
     new Trigger(() -> {
-      return m_driverController.getRightTriggerAxis() > 0.5;
-    }).whileTrue(
-        new SequentialCommandGroup(
-            new IntakeArmPositionCommand(m_intakeSubsystem, ArmPosition.Amp),
-            new NoteOuttakeCommand(m_intakeSubsystem)))
-        .onFalse(new IntakeArmPositionCommand(m_intakeSubsystem, ArmPosition.Retracted));
+      return m_operatorController.getRightTriggerAxis() > 0.5;
+    }).whileTrue(new NoteOuttakeCommand(m_intakeSubsystem));
+
+    new Trigger(() -> {
+      return m_operatorController.getLeftTriggerAxis() > 0.5;
+    }).onTrue(new ShooterSetSpeedCommand(m_shooterSubsystem, ShootSpeed.Shooting))
+        .onFalse(new ShooterSetSpeedCommand(m_shooterSubsystem, ShootSpeed.Off));
+
+    // Climber Up, Operator Controller Right Bumper + A Button
+    new Trigger(() -> {
+      return m_operatorController.getAButton() && m_operatorController.getRightBumper();
+    }).whileTrue(new InstantCommand(() -> m_climberSubsystem.forward()));
+
+    // Climber Down, Operator Controller Right Bumper + B Button
+    new Trigger(() -> {
+      return m_operatorController.getBButton() && m_operatorController.getRightBumper();
+    }).whileTrue(new InstantCommand(() -> m_climberSubsystem.reverse()));
+
+    // Toggle Distance Sensor, Operatoe Controller Left Bumper + Start Button
+    new Trigger(() -> {
+      return m_operatorController.getLeftBumper() && m_operatorController.getStartButton();
+    }).onTrue(new InstantCommand(() -> m_intakeSubsystem.toggleDistanceSensor()));
   }
 
   /**