intake-state (#61)

FRC3636 · Feb 19, 2025 · 8aa7106 · 8aa7106
2 parents 4170612 + 5de5f19
commit 8aa7106
Showing 1 changed file with 14 additions and 2 deletions.
diff --git a/src/main/kotlin/com/frcteam3636/frc2025/subsystems/manipulator/Manipulator.kt b/src/main/kotlin/com/frcteam3636/frc2025/subsystems/manipulator/Manipulator.kt
@@ -1,6 +1,7 @@
 package com.frcteam3636.frc2025.subsystems.manipulator
 
 import com.frcteam3636.frc2025.Robot
+import com.frcteam3636.frc2025.utils.LimelightHelpers
 import com.frcteam3636.frc2025.utils.math.degreesPerSecond
 import edu.wpi.first.networktables.NetworkTableInstance
 import edu.wpi.first.units.Units.Amps
@@ -42,7 +43,8 @@ object Manipulator : Subsystem {
         }, emptySet()),
         Commands.runOnce({
             coralState = CoralState.HELD
-        })
+            blinkLimelight().schedule()
+        }),
     )
 
     init {
@@ -59,6 +61,14 @@ object Manipulator : Subsystem {
         Logger.recordOutput("/Manipulator/Mechanism", mechanism)
     }
 
+    private fun blinkLimelight(): Command = Commands.runOnce({
+        LimelightHelpers.setLEDMode_ForceBlink("limelight-rear")
+    })
+        .andThen(Commands.waitSeconds(0.3))
+        .finallyDo { ->
+            LimelightHelpers.setLEDMode_PipelineControl("limelight-rear")
+        }
+
 
     fun idle(): Command = startEnd({
         io.setSpeed(-0.02)
@@ -88,9 +98,11 @@ object Manipulator : Subsystem {
         }
     )
         .withInterruptBehavior(Command.InterruptionBehavior.kCancelIncoming)
+
+
 }
 
 enum class CoralState {
     NONE,
     HELD
-}
+}