if shooter doesnt see apriltag and wants to shoot

DeepBlueRobotics · Jul 20, 2024 · 0c349bf · 0c349bf
1 parent e6a15e1
commit 0c349bf
Show file tree

Hide file tree

Showing 2 changed files with 14 additions and 6 deletions.
diff --git 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
@@ -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.