From 0c349bf21653e6c2a9068e799cb78e3641c3bede Mon Sep 17 00:00:00 2001
From: stwiggy <144397102+stwiggy@users.noreply.github.com>
Date: Sat, 20 Jul 2024 15:48:35 -0700
Subject: [PATCH] if shooter doesnt see apriltag and wants to shoot

---
 .../java/org/carlmontrobotics/RobotContainer.java     | 11 +++++++++--
 .../org/carlmontrobotics/commands/AimArmSpeaker.java  |  9 +++++----
 2 files changed, 14 insertions(+), 6 deletions(-)

diff --git a/src/main/java/org/carlmontrobotics/RobotContainer.java b/src/main/java/org/carlmontrobotics/RobotContainer.java
index 0018970..579b5e8 100644
--- a/src/main/java/org/carlmontrobotics/RobotContainer.java
+++ b/src/main/java/org/carlmontrobotics/RobotContainer.java
@@ -12,6 +12,7 @@
 import static org.carlmontrobotics.Constants.Armc.*;
 import static org.carlmontrobotics.Constants.OI.Manipulator.*;
 import static org.carlmontrobotics.Constants.Effectorc.*;
+import static org.carlmontrobotics.Constants.Limelightc.*;
 
 // non static constants
 import org.carlmontrobotics.Constants.OI;
@@ -52,6 +53,7 @@
 
 // commands
 import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.ConditionalCommand;
 import edu.wpi.first.wpilibj2.command.InstantCommand;
 import edu.wpi.first.wpilibj2.command.ParallelCommandGroup;
 import edu.wpi.first.wpilibj2.command.ParallelDeadlineGroup;
@@ -202,9 +204,14 @@ private void setBindingsManipulator() {
 
         // new JoystickButton(manipulatorController, A_BUTTON)
         // .onTrue(new RampMaxRPMDriving(intakeShooter));
+        // axisTrigger(manipulatorController, Manipulator.SHOOTER_BUTTON).whileTrue(
+        // new SequentialCommandGroup(new AimArmSpeaker(arm, limelight),
+        // new PassToOuttake(intakeShooter)));
+
         axisTrigger(manipulatorController, Manipulator.SHOOTER_BUTTON).whileTrue(
-                new SequentialCommandGroup(new AimArmSpeaker(arm, limelight),
-                        new PassToOuttake(intakeShooter)));
+                new ConditionalCommand(new SequentialCommandGroup(new AimArmSpeaker(arm, limelight),
+                        new PassToOuttake(intakeShooter)), new InstantCommand(() -> {
+                        }), () -> LimelightHelpers.getTV(SHOOTER_LL_NAME)));
 
         // axisTrigger(manipulatorController, Manipulator.SHOOTER_BUTTON)
         // .whileTrue(new PassToOuttake(intakeShooter));
diff --git a/src/main/java/org/carlmontrobotics/commands/AimArmSpeaker.java b/src/main/java/org/carlmontrobotics/commands/AimArmSpeaker.java
index 457d0e3..1f8b786 100644
--- a/src/main/java/org/carlmontrobotics/commands/AimArmSpeaker.java
+++ b/src/main/java/org/carlmontrobotics/commands/AimArmSpeaker.java
@@ -4,7 +4,8 @@
 
 package org.carlmontrobotics.commands;
 
-import static org.carlmontrobotics.Constants.Limelightc.SHOOTER_LL_NAME;
+import static org.carlmontrobotics.Constants.Armc.*;
+import static org.carlmontrobotics.Constants.Limelightc.*;
 
 import org.carlmontrobotics.Constants.Limelightc;
 import org.carlmontrobotics.subsystems.Arm;
@@ -30,10 +31,10 @@ public void initialize() {
         if (LimelightHelpers.getTV(Limelightc.SHOOTER_LL_NAME)) {
             double goal = ll.getOptimizedArmAngleRadsMT2();
             arm.setArmTarget(goal);
-        } else {
-            arm.setArmTarget(.23);
         }
-
+        else {
+            arm.setArmTarget(SPEAKER_ANGLE_RAD);
+        }
     }
 
     // Called every time the scheduler runs while the command is scheduled.