correct ir logic, stop cmd for 4 piece

blair-robot-project · Feb 9, 2024 · d8fd3d0 · d8fd3d0
1 parent ef83a49
commit d8fd3d0
Show file tree

Hide file tree

Showing 6 changed files with 73 additions and 38 deletions.
diff --git a/src/main/kotlin/frc/team449/robot2024/auto/AutoUtil.kt b/src/main/kotlin/frc/team449/robot2024/auto/AutoUtil.kt
@@ -3,11 +3,44 @@ package frc.team449.robot2024.auto
 import edu.wpi.first.math.MatBuilder
 import edu.wpi.first.math.MathUtil
 import edu.wpi.first.math.Nat
+import edu.wpi.first.wpilibj2.command.*
 import frc.team449.control.auto.ChoreoTrajectory
+import frc.team449.robot2024.Robot
+import frc.team449.robot2024.constants.auto.AutoConstants
 import frc.team449.robot2024.constants.field.FieldConstants
 import kotlin.math.PI
 
 object AutoUtil {
+
+  fun autoIntake(robot: Robot): Command {
+    return ParallelCommandGroup(
+      robot.undertaker.intake(),
+      robot.feeder.intake(),
+      SequentialCommandGroup(
+        WaitUntilCommand { !robot.infrared.get() },
+        robot.undertaker.stop(),
+        robot.feeder.outtake(),
+        robot.shooter.duringIntake(),
+        WaitCommand(AutoConstants.FEEDER_REVERSE_TIME),
+        robot.feeder.stop(),
+        robot.shooter.stop()
+      )
+    )
+  }
+
+  fun autoShoot(robot: Robot): Command {
+    return ParallelCommandGroup(
+      robot.shooter.shootSubwoofer(),
+      SequentialCommandGroup(
+        WaitUntilCommand { robot.shooter.atSetpoint() },
+        robot.feeder.intake(),
+        robot.undertaker.intake(),
+        WaitCommand(AutoConstants.SHOOT_INTAKE_TIME),
+        robot.feeder.stop(),
+        robot.undertaker.stop()
+      )
+    )
+  }
   fun transformForPos2(pathGroup: MutableList<ChoreoTrajectory>): MutableList<ChoreoTrajectory> {
     for (index in 0 until pathGroup.size) {
       for (time in pathGroup[index].objectiveTimestamps) {

diff --git a/src/main/kotlin/frc/team449/robot2024/auto/routines/Experimental4Piece.kt b/src/main/kotlin/frc/team449/robot2024/auto/routines/Experimental4Piece.kt
@@ -1,6 +1,5 @@
 package frc.team449.robot2024.auto.routines
 
-import edu.wpi.first.wpilibj2.command.WaitCommand
 import frc.team449.control.auto.ChoreoRoutine
 import frc.team449.control.auto.ChoreoRoutineStructure
 import frc.team449.control.auto.ChoreoTrajectory
@@ -12,14 +11,18 @@ class Experimental4Piece(
   isRed: Boolean
 ) : ChoreoRoutineStructure {
 
+  /** TODO STILL AUTO SUBSYSTEM CMD PARALLEL */
   override val routine =
     ChoreoRoutine(
       drive = robot.drive,
-      parallelEventMap = hashMapOf(),
+      parallelEventMap = hashMapOf(
+        0 to AutoUtil.autoIntake(robot)
+      ),
       stopEventMap = hashMapOf(
-        0 to WaitCommand(1.5),
-        1 to WaitCommand(1.5),
-        2 to WaitCommand(1.5)
+        0 to AutoUtil.autoShoot(robot),
+        1 to AutoUtil.autoShoot(robot),
+        2 to AutoUtil.autoShoot(robot),
+        3 to AutoUtil.autoShoot(robot)
       ),
       debug = true
     )

diff --git a/src/main/kotlin/frc/team449/robot2024/constants/auto/AutoConstants.kt b/src/main/kotlin/frc/team449/robot2024/constants/auto/AutoConstants.kt
@@ -9,4 +9,7 @@ object AutoConstants {
   const val DEFAULT_ROTATION_KP = 2.0
 
   const val ORBIT_KP = 2 * PI
+
+  const val SHOOT_INTAKE_TIME = 0.5
+  const val FEEDER_REVERSE_TIME = 0.5
 }
diff --git a/src/main/kotlin/frc/team449/robot2024/constants/subsystem/UndertakerConstants.kt b/src/main/kotlin/frc/team449/robot2024/constants/subsystem/UndertakerConstants.kt
@@ -11,4 +11,6 @@ object UndertakerConstants {
 
   const val INTAKE_VOLTAGE = 12.0
   const val REVERSE_VOLTAGE = -12.0
+
+  const val BRAKE_MODE = false
 }
diff --git a/src/main/kotlin/frc/team449/robot2024/subsystems/ControllerBindings.kt b/src/main/kotlin/frc/team449/robot2024/subsystems/ControllerBindings.kt
@@ -6,6 +6,7 @@ import edu.wpi.first.wpilibj2.command.*
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController
 import frc.team449.control.holonomic.SwerveSim
 import frc.team449.robot2024.Robot
+import frc.team449.robot2024.auto.AutoUtil
 import frc.team449.robot2024.commands.driveAlign.OrbitAlign
 import frc.team449.robot2024.constants.RobotConstants
 import frc.team449.robot2024.constants.field.FieldConstants
@@ -23,33 +24,31 @@ class ControllerBindings(
     FieldConstants.SUBWOOFER_POSE
   )
 
+  private fun stopAll(): Command {
+    return ParallelCommandGroup(
+      robot.undertaker.stop(),
+      robot.shooter.stop(),
+      robot.feeder.stop()
+    )
+  }
+
   private fun robotBindings() {
-    driveController.rightBumper().whileTrue(
-      ParallelCommandGroup(
-        robot.undertaker.intake(),
-        robot.feeder.intake(),
-        robot.shooter.duringIntake(),
-        /** IR Stuff */
-//        SequentialCommandGroup(
-//          WaitUntilCommand { !robot.infrared.get() },
-//          robot.undertaker.stop(),
-//          robot.feeder.outtake(),
-//          robot.shooter.duringIntake(),
-//          WaitCommand(0.50),
-//          robot.feeder.stop(),
-//          robot.shooter.stop()
-//        )
+    driveController.povUp().onTrue(
+      robot.pivot.moveAmp()
+    )
+
+    driveController.povRight().onTrue(
+      robot.shooter.scoreAmp().alongWith(
+        robot.feeder.stop()
       )
     ).onFalse(
-      SequentialCommandGroup(
-        robot.undertaker.stop(),
-        robot.shooter.stop(),
-        robot.feeder.outtake(),
-        robot.shooter.duringIntake(),
-        WaitCommand(0.50),
-        robot.feeder.stop(),
-        robot.shooter.stop()
-      )
+      stopAll()
+    )
+
+    driveController.rightBumper().onTrue(
+      AutoUtil.autoIntake(robot)
+    ).onFalse(
+      stopAll()
     )
 
 //    driveController.leftBumper().onTrue(
@@ -76,11 +75,7 @@ class ControllerBindings(
         )
       )
     ).onFalse(
-      robot.feeder.stop().alongWith(
-        robot.shooter.stop()
-      ).alongWith(
-        robot.undertaker.stop()
-      )
+      stopAll()
     )
 
     /** Shooting from anywhere */
@@ -123,9 +118,7 @@ class ControllerBindings(
       )
     ).onFalse(
       ParallelCommandGroup(
-        robot.feeder.stop(),
-        robot.shooter.stop(),
-        robot.undertaker.stop()
+        stopAll()
       )
     )
   }

diff --git a/src/main/kotlin/frc/team449/robot2024/subsystems/Undertaker.kt b/src/main/kotlin/frc/team449/robot2024/subsystems/Undertaker.kt
@@ -46,7 +46,8 @@ class Undertaker(
         ),
         inverted = UndertakerConstants.INVERTED,
         currentLimit = UndertakerConstants.CURRENT_LIM,
-        slaveSparks = mapOf(Pair(UndertakerConstants.FOLLOLWER_ID, UndertakerConstants.FOLLOWER_INV))
+        slaveSparks = mapOf(Pair(UndertakerConstants.FOLLOLWER_ID, UndertakerConstants.FOLLOWER_INV)),
+        enableBrakeMode = UndertakerConstants.BRAKE_MODE
       )
 
       return Undertaker(motor)