fix: don't do auto intake in teleop

FRC3636 · Jun 13, 2024 · 16a3373 · 16a3373
1 parent f11b5e2
commit 16a3373
Showing 1 changed file with 27 additions and 2 deletions.
diff --git a/src/main/java/com/frcteam3636/frc2024/Robot.kt b/src/main/java/com/frcteam3636/frc2024/Robot.kt
@@ -112,7 +112,7 @@ object Robot : LoggedRobot() {
             Commands.sequence(
                 Commands.parallel(
                     Shooter.Pivot.pivotAndStop(Shooter.Pivot.Target.STOWED.profile.position()),
-                    doIntakeSequence()
+                    doIntakeAutoSequence()
                 ),
                 Commands.parallel(
                     Shooter.Pivot.followMotionProfile(Shooter.Pivot.Target.AIM),
@@ -323,7 +323,7 @@ fun autoIntake(): Command =
         )
     ).withTimeout(1.5)
 
-private fun doIntakeSequence(): Command =
+private fun doIntakeAutoSequence(): Command =
     Commands.sequence(
         Commands.waitUntil(Shooter.Pivot.isStowed),
         autoIntake(),
@@ -332,6 +332,31 @@ private fun doIntakeSequence(): Command =
         }
     )
 
+private fun doIntakeSequence(): Command =
+    Commands.sequence(
+        Intake.intake(),
+        Commands.runOnce({ Note.state = Note.State.HANDOFF }),
+        Commands.waitUntil(Shooter.Pivot.isStowed),
+        Commands.race(
+            Commands.parallel(
+                Intake.index(),
+                Shooter.Feeder.intake(),
+                Shooter.Flywheels.intake(),
+            ),
+            Commands.sequence(
+                //spinning up
+                Commands.waitUntil { Shooter.Flywheels.aboveIntakeThreshold },
+                //reached velocity setpoint
+                Commands.waitUntil { !Shooter.Flywheels.aboveIntakeThreshold },
+                //contacted note
+                Commands.waitUntil { Shooter.Flywheels.aboveIntakeThreshold },
+                //note stowed
+                Commands.waitUntil(Trigger { !Shooter.Flywheels.aboveIntakeThreshold }),
+                Commands.runOnce({ Note.state = Note.State.SHOOTER })
+            )
+        )
+    )
+
 object Note {
     enum class State(val index: Long) {
         NONE(0),