Update Feeding

Team3128 · Oct 12, 2024 · 7a5ab1e · 7a5ab1e
1 parent 4a9a191
commit 7a5ab1e
Showing 1 changed file with 22 additions and 5 deletions.
diff --git a/src/main/java/frc/team3128/commands/CmdManager.java b/src/main/java/frc/team3128/commands/CmdManager.java
@@ -174,13 +174,30 @@ public static Command ejectNote(){
         );
     }
 
-    public static Command feed(double rpm, double angle){
+    // public static Command feed(double rpm, double angle){
+    //     return sequence(
+    //         shooter.runShooter(rpm),
+    //         swerve.turnInPlace(()-> angle),
+    //         shooter.runKickMotor(KICK_POWER),
+    //         hopper.intake(),
+    //         waitSeconds(1),
+    //         shooter.stopMotors(),
+    //         hopper.runManipulator(0)
+    //     );
+    // }
+
+    //better feed logic UNTESTED
+    public Command feed(double shooterPower, double angle){
         return sequence(
-            shooter.runShooter(rpm),
-            swerve.turnInPlace(()-> angle),
+            parallel(
+                swerve.turnInPlace(()-> angle),
+                shooter.runShooter(shooterPower)
+            ),
             shooter.runKickMotor(KICK_POWER),
-            hopper.intake(),
-            waitSeconds(1),
+            waitSeconds(0.25),
+            hopper.runManipulator(HOPPER_INTAKE_POWER),
+            waitUntil(()-> !shooter.noteInRollers() || !hopper.hasObjectPresent()),
+            waitSeconds(0.5),
             shooter.stopMotors(),
             hopper.runManipulator(0)
         );