comp day 2

src/main/java/cshcyberhawks/lib/commands/ForCommand.kt

@@ -35,6 +35,20 @@ import edu.wpi.first.wpilibj2.command.SequentialCommandGroup
 //    }
 //}
 
+/**
+ * A command that executes a sequence of commands based on a loop counter.
+ *
+ * @param max The maximum value for the loop counter (exclusive)
+ * @param callback A function that takes the loop counter as input and returns a Command to execute
+ *
+ * Example usage:
+ * ```
+ * ForCommand(5) { i ->
+ *   Commands.runOnce { println("Loop iteration $i") }
+ * }
+ * ```
+ * This would print the loop counter 5 times in sequence upon scheduling the ForCommand.
+ */
 class ForCommand(private val max: Int, private val callback: (i: Int) -> Command) : SequentialCommandGroup(
     *(0..<max).map { callback(it) }.toTypedArray()
 )

src/main/java/cshcyberhawks/lib/requests/Request.kt

@@ -1,5 +1,7 @@
 package cshcyberhawks.lib.requests
 
+import java.util.Optional
+
 /**
  * Abstract base class representing a generic request.
  * Requests system inspired by the one in Citrus Circuits' 2024 Robot Code
@@ -24,6 +26,7 @@ abstract class Request {
     open fun isFinished(): Boolean = true
 
     private var prerequisites: MutableList<Prerequisite> = mutableListOf()
+    private var deadline: Optional<() -> Boolean> = Optional.empty()
 
     /**
      * Adds a prerequisite to the current request.
@@ -51,7 +54,15 @@ abstract class Request {
         return this
     }
 
+    fun withDeadline(deadline: () -> Boolean): Request {
+        this.deadline = Optional.of(deadline)
+
+        return this
+    }
+
     fun allowed(): Boolean {
         return prerequisites.all { prerequisite -> prerequisite.met() }
     }
+
+    fun deadlineHit() = deadline.isPresent && deadline.get()()
 }

src/main/java/cshcyberhawks/lib/requests/SequentialRequest.kt

@@ -13,7 +13,7 @@ class SequentialRequest(vararg requests: Request) : Request() {
     private var startedCurrentRequest = false
 
     private fun startNextRequest() {
-        if (idleRequests.isNotEmpty() && currentRequest.isFinished()) {
+        if (idleRequests.isNotEmpty() && (currentRequest.isFinished() || currentRequest.deadlineHit()) && startedCurrentRequest) {
             currentRequest = idleRequests.removeAt(0)
             startedCurrentRequest = false
             startIfAllowed()

src/main/java/frc/robot/Robot.kt

@@ -55,6 +55,8 @@ object Robot : TimedRobot() {
 
         SmartDashboard.putBoolean("Start by unclimbing?", false)
 
+        SmartDashboard.putBoolean("Start with a disabled climb", false)
+
         CanBridge.runTCP()
     }
 
@@ -75,6 +77,8 @@ object Robot : TimedRobot() {
         SmartDashboard.putBoolean("Teleop pose difference?", true)
 
 
+
+
         RobotContainer
         OperatorControls
     }
@@ -117,6 +121,8 @@ object Robot : TimedRobot() {
         RobotContainer.startByUnclimbing = SmartDashboard.getBoolean("Start by unclimbing?", false)
 //        println(RobotContainer.startByUnclimbing)
 
+        RobotContainer.startWithADisabledClimb = SmartDashboard.getBoolean("Start with a disabled climb", false)
+
 
         RobotContainer.vision.updateOdometryFromDisabled()
     }
@@ -165,6 +171,12 @@ object Robot : TimedRobot() {
             println("unclilmbing")
             Superstructure.unclimb()
         }
+
+        if (RobotContainer.startWithADisabledClimb) {
+            println("starting with climb disabled")
+            Superstructure.disableClimb()
+        }
+
 //        } else if (!Superstructure.climbSystem.isStow() || !Superstructure.funnelSystem.isStow()) {
 //            Superstructure.climbStowThenStow()
 //        } else {

src/main/java/frc/robot/RobotContainer.kt

@@ -24,6 +24,8 @@ object RobotContainer {
 
     var startByUnclimbing = false
 
+    var startWithADisabledClimb = false
+
 
     val xbox = CommandXboxController(2)
 
@@ -59,7 +61,7 @@ object RobotContainer {
 //            NamedCommands.registerCommand(pathplannerCommand.name, pathplannerCommand.cmd)
 //        }
 
-        autoCommand = frc.robot.commands.auto.AutoBuilder.twoL4Left()
+        autoCommand = frc.robot.commands.auto.AutoBuilder.twoL4Right()
 //        autoCommand = getAutonomousCommand()
 //        SmartDashboard.putData("Auto Chooser", autoChooser)
     }

src/main/java/frc/robot/commands/GoToPose.kt

@@ -96,10 +96,16 @@ open class GoToPose(
         RobotContainer.drivetrain.applyDriveRequest(xVel, yVel, rotVel)
     }
 
-    override fun isFinished(): Boolean = endCondition()
+    override fun isFinished(): Boolean {
+
+        val isDone =endCondition()
+        SmartDashboard.putBoolean("GoToPose Is Done?", isDone)
+
+        return isDone
+    }
 
     override fun end(interrupted: Boolean) {
-        println("drive command and then ending")
+//        println("drive command and then ending")
         RobotState.autoDriving = false
         RobotContainer.currentDriveCommand = Optional.empty()
     }

src/main/java/frc/robot/commands/TeleopAutoAlign.kt

@@ -1,6 +1,7 @@
 package frc.robot.commands
 
 import cshcyberhawks.lib.math.MiscCalculations
+import edu.wpi.first.math.geometry.Rotation2d
 import edu.wpi.first.wpilibj2.command.Command
 import frc.robot.RobotContainer
 import frc.robot.constants.AutoScoringConstants
@@ -36,14 +37,18 @@ object TeleopAutoAlign {
             if (OperatorControls.noWalk) {
                 AllianceFlipUtil.apply(AutoScoringConstants.getReefPoseAtOffset(position.ordinal, side, 0.0))
             } else {
-                val goalPose = AllianceFlipUtil.apply(AutoScoringConstants.getReefPoseAtOffset(position.ordinal, side, 0.0))
+                val goalPose =
+                    AllianceFlipUtil.apply(AutoScoringConstants.getReefPoseAtOffset(position.ordinal, side, 0.0))
                 // Make the position get closer as it gets closer to the final goal
 //            val adjustX: Double = 0.3
-                var adjustX: Double = min(1.0, RobotContainer.drivetrain.getSwervePose().translation.getDistance(goalPose.translation) * 0.75)
+                var adjustX: Double = min(
+                    1.0,
+                    RobotContainer.drivetrain.getSwervePose().translation.getDistance(goalPose.translation) * 0.5
+                )
 //            val adjustX = 1.0
 
                 // Once we're close enough we can just jump to the goal
-                if (adjustX < 0.25) {
+                if (adjustX < 0.2) {
                     adjustX = 0.0
                 }
 
@@ -62,11 +67,14 @@ object TeleopAutoAlign {
                 val goalPose = AllianceFlipUtil.apply(AutoScoringConstants.getAlgaePoseAtOffset(position.ordinal, 0.0))
                 // Make the position get closer as it gets closer to the final goal
 //            val adjustX: Double = 0.3
-                var adjustX: Double = min(1.0, RobotContainer.drivetrain.getSwervePose().translation.getDistance(goalPose.translation) * 0.75)
+                var adjustX: Double = min(
+                    1.0,
+                    RobotContainer.drivetrain.getSwervePose().translation.getDistance(goalPose.translation) * .5
+                )
 //            val adjustX = 1.0
 
                 // Once we're close enough we can just jump to the goal
-                if (adjustX < 0.25) {
+                if (adjustX < 0.2) {
                     adjustX = 0.0
                 }
 
@@ -75,18 +83,48 @@ object TeleopAutoAlign {
         })
     }
 
-    fun processorAlign(): Command = GoToPose({
-        AllianceFlipUtil.apply(FieldConstants.processorPosition)
-    }, { false })
+        fun l1ReefAlign(): Command {
+            return GoToPose({
+                val position = OperatorControls.reefPosition
 
-    fun feederAlign(): Command = GoToPose({
-        val leftPos = AllianceFlipUtil.apply(FieldConstants.leftFeederPosition)
-        val rightPos = AllianceFlipUtil.apply(FieldConstants.rightFeederPosition)
+                if (OperatorControls.noWalk) {
+                    AllianceFlipUtil.apply(AutoScoringConstants.getL1PoseAtOffset(position.ordinal, 0.0))
+                } else {
+                    val goalPose =
+                        AllianceFlipUtil.apply(AutoScoringConstants.getL1PoseAtOffset(position.ordinal, 0.0))
+                    // Make the position get closer as it gets closer to the final goal
+//            val adjustX: Double = 0.3
+                    var adjustX: Double = min(
+                        1.0,
+                        RobotContainer.drivetrain.getSwervePose().translation.getDistance(goalPose.translation) * 0.5
+                    )
+//            val adjustX = 1.0
+
+                    // Once we're close enough we can just jump to the goal
+                    if (adjustX < 0.2) {
+                        adjustX = 0.0
+                    }
 
-        if (RobotContainer.drivetrain.getSwervePose().translation.getDistance(leftPos.translation) < RobotContainer.drivetrain.getSwervePose().translation.getDistance(rightPos.translation))  {
-            leftPos
-        } else {
-            rightPos
+                    AllianceFlipUtil.apply(AutoScoringConstants.getL1PoseAtOffset(position.ordinal, adjustX))
+                }
+            })
         }
-    }, { false })
-}
+
+        fun processorAlign(): Command = GoToPose({
+            AllianceFlipUtil.apply(FieldConstants.processorPosition)
+        }, { false })
+
+        fun feederAlign(): Command = GoToPose({
+            val leftPos = AllianceFlipUtil.apply(FieldConstants.leftFeederPosition)
+            val rightPos = AllianceFlipUtil.apply(FieldConstants.rightFeederPosition)
+
+            if (RobotContainer.drivetrain.getSwervePose().translation.getDistance(leftPos.translation) < RobotContainer.drivetrain.getSwervePose().translation.getDistance(
+                    rightPos.translation
+                )
+            ) {
+                leftPos
+            } else {
+                rightPos
+            }
+        }, { false })
+    }

src/main/java/frc/robot/commands/auto/AutoBuilder.kt

@@ -61,7 +61,7 @@ object AutoBuilder {
                         Commands.runOnce({ Superstructure.scoreL4() }),
                         AutoCommands.coralReefAlign(targets[i].position, targets[i].side)
                     ),
-                    WaitCommand(0.35),
+                    WaitCommand(0.7),
                     Commands.runOnce({
                         RobotState.actionConfirmed = true
                         println("Score $i: ${autoTimer.get()}")
@@ -132,6 +132,15 @@ object AutoBuilder {
             Commands.runOnce({ Superstructure.removeAlgaeLow() }),
             AutoCommands.algaeReefAlign(AutoScoringConstants.ReefPositions.D)
         ),
-        AutoCommands.safeReefExit(AutoScoringConstants.ReefPositions.D)
+        ParallelCommandGroup(
+            AutoCommands.safeReefExit(AutoScoringConstants.ReefPositions.D),
+            Commands.runOnce({ Superstructure.algaeSpit() }),
+        ),
+        ParallelDeadlineGroup(
+            Commands.waitUntil { RobotState.gamePieceState == GamePieceState.Algae },
+            Commands.runOnce({ Superstructure.removeAlgaeHigh() }),
+            AutoCommands.algaeReefAlign(AutoScoringConstants.ReefPositions.C)
+        ),
+        AutoCommands.safeReefExit(AutoScoringConstants.ReefPositions.C)
     )
 }

src/main/java/frc/robot/commands/auto/AutoCommands.kt

@@ -36,7 +36,7 @@ object AutoCommands {
 //            val adjustX = 1.0
 
             // Once we're close enough we can just jump to the goal
-            if (adjustX < 0.25) {
+            if (adjustX < 0.2) {
                 adjustX = 0.0
             }
 
@@ -93,7 +93,7 @@ object AutoCommands {
 //            val adjustX = 1.0
 
             // Once we're close enough we can just jump to the goal
-            if (adjustX < 0.3) {
+            if (adjustX < 0.2) {
                 adjustX = 0.0
             }
 

src/main/java/frc/robot/constants/AutoScoringConstants.kt

@@ -62,6 +62,27 @@ object AutoScoringConstants {
         )
     }
 
+    fun getL1PoseAtOffset(face: Int, x: Double): Pose2d {
+        val poseDirection =
+            Pose2d(FieldConstants.Reef.center, Rotation2d.fromDegrees((180 - (60 * face)).toDouble()))
+        val adjustX: Double = reefFaceOffset + x - 0.425// + Units.inchesToMeters(3.0)
+        val adjustY: Double = 1.075
+
+        return Pose2d(
+            Translation2d(
+                poseDirection
+                    .transformBy(Transform2d(adjustX, adjustY, Rotation2d()))
+                    .x,
+                poseDirection
+                    .transformBy(Transform2d(adjustX, adjustY, Rotation2d()))
+                    .y
+            ),
+            Rotation2d(
+                poseDirection.rotation.radians
+            ).rotateBy(Rotation2d.fromDegrees(76.0))
+        )
+    }
+
     enum class ReefPositions(var left: Pose2d, var right: Pose2d, var center: Pose2d) {
         A(Pose2d(), Pose2d(), Pose2d()),
         B(Pose2d(), Pose2d(), Pose2d()),