Refactor indexer commands

FRC3636 · Nov 14, 2024 · d8201f3 · d8201f3
1 parent 4d2e6e0
commit d8201f3
Show file tree

Hide file tree

Showing 2 changed files with 27 additions and 22 deletions.
diff --git a/src/main/java/com/frcteam3636/frc2024/Robot.kt b/src/main/java/com/frcteam3636/frc2024/Robot.kt
@@ -3,6 +3,7 @@ package com.frcteam3636.frc2024
 import BuildConstants
 import com.ctre.phoenix6.StatusSignal
 import com.frcteam3636.frc2024.subsystems.drivetrain.Drivetrain
+import com.frcteam3636.frc2024.subsystems.indexer.Indexer
 import com.frcteam3636.frc2024.subsystems.intake.Intake
 import edu.wpi.first.hal.FRCNetComm.tInstances
 import edu.wpi.first.hal.FRCNetComm.tResourceType
@@ -107,6 +108,7 @@ object Robot : LoggedRobot() {
     /** Start robot subsystems so that their periodic tasks are run */
     private fun configureSubsystems() {
         Drivetrain.register()
+        Indexer.register()
     }
 
     /** Expose commands for autonomous routines to use and display an auto picker in Shuffleboard. */
@@ -123,6 +125,7 @@ object Robot : LoggedRobot() {
     /** Configure which commands each joystick button triggers. */
     private fun configureBindings() {
         Drivetrain.defaultCommand = Drivetrain.driveWithJoysticks(joystickLeft, joystickRight)
+        Indexer.defaultCommand = Indexer.autoRun()
 
         // (The button with the yellow tape on it)
         JoystickButton(joystickLeft, 8).onTrue(Commands.runOnce({

diff --git a/src/main/java/com/frcteam3636/frc2024/subsystems/indexer/Indexer.kt b/src/main/java/com/frcteam3636/frc2024/subsystems/indexer/Indexer.kt
@@ -13,29 +13,31 @@ object Indexer: Subsystem {
     override fun periodic() {
         io.updateInputs(inputs)
         Logger.processInputs("Indexer", inputs)
-
-        DriverStation.getAlliance().map {
-            if (
-                (inputs.balloonState == BalloonState.Blue && it == DriverStation.Alliance.Blue)
-                || (inputs.balloonState == BalloonState.Red && it == DriverStation.Alliance.Red)
-            ) {
-                progressBalloon()
-            } else {
-                outtakeBalloon()
-            }
-        }
-
     }
 
-    fun progressBalloon(): Command =
-        startEnd(
-            {io.setSpinSpeed(0.5)},
-            {io.setSpinSpeed(0.0)}
-        )
-
-    fun outtakeBalloon(): Command =
-        startEnd(
-            {io.setSpinSpeed(-0.5)},
-            {io.setSpinSpeed(0.0)}
+    /**
+     * Runs the indexer forward if balloon matches the current alliance.
+     * Does not run if no balloon.
+     * Reverses if balloon is wrong alliance.
+     */
+    fun autoRun(): Command =
+        runEnd(
+            {
+                DriverStation.getAlliance().map {
+                    if (
+                        (inputs.balloonState == BalloonState.Blue && it == DriverStation.Alliance.Blue)
+                        || (inputs.balloonState == BalloonState.Red && it == DriverStation.Alliance.Red)
+                    ) {
+                        io.setSpinSpeed(0.5)
+                    } else if (inputs.balloonState == BalloonState.None) {
+                        io.setSpinSpeed(0.0)
+                    } else {
+                        io.setSpinSpeed(-0.5)
+                    }
+                }
+            },
+            {
+                io.setSpinSpeed(0.0)
+            }
         )
 }