Pre Tech Valley Changes

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/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() && 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)
 

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

@@ -99,7 +99,7 @@ open class GoToPose(
     override fun isFinished(): Boolean = endCondition()
 
     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.3) {
                     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.3) {
                     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.3) {
+                        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/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()),

src/main/java/frc/robot/subsystems/superstructure/Superstructure.kt

@@ -27,6 +27,7 @@ import frc.robot.subsystems.superstructure.pivot.implementation.PivotIOEmpty
 import frc.robot.subsystems.superstructure.pivot.implementation.PivotIOPID
 import frc.robot.subsystems.superstructure.pivot.implementation.PivotIOSim
 import frc.robot.util.AllianceFlipUtil
+import frc.robot.util.input.OperatorControls
 import java.util.*
 
 object Superstructure : SubsystemBase() {
@@ -178,7 +179,13 @@ object Superstructure : SubsystemBase() {
                 ),
                 EmptyRequest(),
                 ParallelRequest(
-                    stowRequest(),
+                    IfRequest(
+                        {RobotState.gamePieceState == GamePieceState.Coral && OperatorControls.highStowPosition},
+                        ParallelRequest(
+                            elevatorSystem.stowPosition(), pivotSystem.highStowAngle()
+                        ),
+                        stowRequest()
+                    ),
                     IfRequest(
                         { RobotState.gamePieceState == GamePieceState.Coral },
                         intakeSystem.coralHolding(),
@@ -201,6 +208,14 @@ object Superstructure : SubsystemBase() {
         )
     )
 
+    fun scoreL1() = requestSuperstructureAction(
+        SuperstructureAction.create(
+            ParallelRequest(pivotSystem.l1Angle(), elevatorSystem.l1Position()),
+            intakeSystem.coralScore(),
+            stowRequest()
+        )
+    )
+
     fun scoreL2() =
         requestSuperstructureAction(
             SuperstructureAction.create(
@@ -214,7 +229,8 @@ object Superstructure : SubsystemBase() {
     fun scoreL3() =
         requestSuperstructureAction(
             SuperstructureAction.create(
-                ParallelRequest(pivotSystem.l3Angle(), elevatorSystem.l3Position(), intakeSystem.coralHalfSpit()),
+                ParallelRequest(pivotSystem.l3Angle(), elevatorSystem.l3Position(), intakeSystem.coralHalfSpit().withPrerequisite(
+                    elevatorSystem.closeL3())),
                 intakeSystem.coralScore(),
                 stowRequest(),
                 safeRetract = true
@@ -225,11 +241,11 @@ object Superstructure : SubsystemBase() {
         ParallelRequest(
             pivotSystem.l4Angle(),
             elevatorSystem.safeUpPosition(),
-            intakeSystem.coralHalfSpit()
         ),
         elevatorSystem
             .l4Position()
             .withPrerequisite(pivotSystem.safeTravelUp()),
+        //intakeSystem.coralHalfSpit().withPrerequisite(elevatorSystem.closeL4())
     )
 
     fun scoreL4() =
@@ -331,11 +347,13 @@ object Superstructure : SubsystemBase() {
     fun climb() = requestSuperstructureAction(
         SuperstructureAction.create(
                 ParallelRequest(
-                pivotSystem.oldClimbAngle(),
-                climbSystem.deploy(),
-                funnelSystem.deploy()
+                    climbSystem.unspoolForTime(),
+//                    pivotSystem.climbAngle(),
+                    //TODO: a timed climb system unspool
+//                    climbSystem.deploy(),
+                    funnelSystem.deploy()
                 ),
-            climbSystem.climb(),
+            SequentialRequest(pivotSystem.oldClimbAngleWithPID(), WaitRequest(1.5), climbSystem.climb()),
             EmptyRequest()//IfRequest({RobotState.actionCancelled}, SequentialRequest(ParallelRequest(funnelSystem.stow(), climbSystem.stow())), pivotSystem.stowAngle())
         )
     )
@@ -352,6 +370,10 @@ object Superstructure : SubsystemBase() {
         )
     )
 
+    fun disableClimb() = requestSuperstructureAction(
+        Superstructure.climbSystem.setDisable(RobotContainer.startWithADisabledClimb)
+    )
+
     fun climbStowThenStow() = requestSuperstructureAction(
         SequentialRequest(
             ParallelRequest(climbSystem.stow(), pivotSystem.climbStowAngle()),

src/main/java/frc/robot/subsystems/superstructure/climb/ClimbConstants.kt

@@ -2,8 +2,8 @@ package frc.robot.subsystems.superstructure.climb
 
 object ClimbConstants {
     val stowAngle = 223.0
-    val deployAngle = 80.0
+    val deployAngle = 70.0
 //    val climbAngle = 145.0
-    var climbAngle = 125.0
+    var climbAngle = 141.0
 
 }

src/main/java/frc/robot/subsystems/superstructure/climb/ClimbIO.kt

@@ -3,6 +3,7 @@ package frc.robot.subsystems.superstructure.climb
 interface ClimbIO {
     fun getAngle(): Double
     fun angleDegrees(angleDegrees: Double)
+    fun setMotor(current: Double)
     fun periodic()
 
     var climbing: Boolean

src/main/java/frc/robot/subsystems/superstructure/climb/ClimbSystem.kt

@@ -3,19 +3,31 @@ package frc.robot.subsystems.superstructure.climb
 import cshcyberhawks.lib.math.MiscCalculations
 import cshcyberhawks.lib.requests.Request
 import cshcyberhawks.lib.requests.SequentialRequest
+import cshcyberhawks.lib.requests.WaitRequest
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard
 import edu.wpi.first.wpilibj2.command.SubsystemBase
 
 class ClimbSystem(private val io: ClimbIO): SubsystemBase() {
     private fun setAngle(angleDegrees: Double) = Request.withAction { io.angleDegrees(angleDegrees) }
     private fun setClimbing(climbing: Boolean) = Request.withAction { io.climbing = climbing }
-    private fun setDisable(disable: Boolean) = Request.withAction {io.disable = disable}
+    fun setDisable(disable: Boolean) = Request.withAction {
+        println("set disable to: " + disable)
+        io.disable = disable
+    }
+    fun setMotor(current: Double) = Request.withAction { io.setMotor(current) }
 
     fun deploy() = SequentialRequest(setClimbing(false), setDisable(false), setAngle(ClimbConstants.deployAngle))
     fun climb() = SequentialRequest(setClimbing(true), setDisable(false), setAngle(ClimbConstants.climbAngle))
 
     fun stow() = SequentialRequest(setClimbing(false), setDisable(false), setAngle(ClimbConstants.stowAngle))
 
+    fun unspoolForTime() = SequentialRequest(
+        setDisable(true),
+        setMotor(-40.0),
+        WaitRequest(8.0),
+        setMotor(0.0)
+    )
+
     fun isStow(): Boolean {
         return MiscCalculations.calculateDeadzone(io.getAngle() - ClimbConstants.stowAngle, 5.0) == 0.0
     }

src/main/java/frc/robot/subsystems/superstructure/climb/implementation/ClimbIOEmpty.kt

@@ -10,6 +10,10 @@ class ClimbIOEmpty(): ClimbIO {
         return 0.0
     }
 
+    override fun setMotor(current: Double) {
+
+    }
+
     override fun angleDegrees(angleDegrees: Double) {
     }