Skip to content

Commit 28dc75e

Browse files
PY44NPY44N
authored
Merge pull request #22 from CshCyberhawks/tech_valley
2 parents 318668f + 9779aa6 commit 28dc75e

30 files changed

Lines changed: 302 additions & 73 deletions

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

Lines changed: 14 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -35,6 +35,20 @@ import edu.wpi.first.wpilibj2.command.SequentialCommandGroup
3535
// }
3636
//}
3737

38+
/**
39+
* A command that executes a sequence of commands based on a loop counter.
40+
*
41+
* @param max The maximum value for the loop counter (exclusive)
42+
* @param callback A function that takes the loop counter as input and returns a Command to execute
43+
*
44+
* Example usage:
45+
* ```
46+
* ForCommand(5) { i ->
47+
* Commands.runOnce { println("Loop iteration $i") }
48+
* }
49+
* ```
50+
* This would print the loop counter 5 times in sequence upon scheduling the ForCommand.
51+
*/
3852
class ForCommand(private val max: Int, private val callback: (i: Int) -> Command) : SequentialCommandGroup(
3953
*(0..<max).map { callback(it) }.toTypedArray()
4054
)

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

Lines changed: 11 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -1,5 +1,7 @@
11
package cshcyberhawks.lib.requests
22

3+
import java.util.Optional
4+
35
/**
46
* Abstract base class representing a generic request.
57
* Requests system inspired by the one in Citrus Circuits' 2024 Robot Code
@@ -24,6 +26,7 @@ abstract class Request {
2426
open fun isFinished(): Boolean = true
2527

2628
private var prerequisites: MutableList<Prerequisite> = mutableListOf()
29+
private var deadline: Optional<() -> Boolean> = Optional.empty()
2730

2831
/**
2932
* Adds a prerequisite to the current request.
@@ -51,7 +54,15 @@ abstract class Request {
5154
return this
5255
}
5356

57+
fun withDeadline(deadline: () -> Boolean): Request {
58+
this.deadline = Optional.of(deadline)
59+
60+
return this
61+
}
62+
5463
fun allowed(): Boolean {
5564
return prerequisites.all { prerequisite -> prerequisite.met() }
5665
}
66+
67+
fun deadlineHit() = deadline.isPresent && deadline.get()()
5768
}

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

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -13,7 +13,7 @@ class SequentialRequest(vararg requests: Request) : Request() {
1313
private var startedCurrentRequest = false
1414

1515
private fun startNextRequest() {
16-
if (idleRequests.isNotEmpty() && currentRequest.isFinished()) {
16+
if (idleRequests.isNotEmpty() && (currentRequest.isFinished() || currentRequest.deadlineHit()) && startedCurrentRequest) {
1717
currentRequest = idleRequests.removeAt(0)
1818
startedCurrentRequest = false
1919
startIfAllowed()

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

Lines changed: 12 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -55,6 +55,8 @@ object Robot : TimedRobot() {
5555

5656
SmartDashboard.putBoolean("Start by unclimbing?", false)
5757

58+
SmartDashboard.putBoolean("Start with a disabled climb", false)
59+
5860
CanBridge.runTCP()
5961
}
6062

@@ -75,6 +77,8 @@ object Robot : TimedRobot() {
7577
SmartDashboard.putBoolean("Teleop pose difference?", true)
7678

7779

80+
81+
7882
RobotContainer
7983
OperatorControls
8084
}
@@ -117,6 +121,8 @@ object Robot : TimedRobot() {
117121
RobotContainer.startByUnclimbing = SmartDashboard.getBoolean("Start by unclimbing?", false)
118122
// println(RobotContainer.startByUnclimbing)
119123

124+
RobotContainer.startWithADisabledClimb = SmartDashboard.getBoolean("Start with a disabled climb", false)
125+
120126

121127
RobotContainer.vision.updateOdometryFromDisabled()
122128
}
@@ -165,6 +171,12 @@ object Robot : TimedRobot() {
165171
println("unclilmbing")
166172
Superstructure.unclimb()
167173
}
174+
175+
if (RobotContainer.startWithADisabledClimb) {
176+
println("starting with climb disabled")
177+
Superstructure.disableClimb()
178+
}
179+
168180
// } else if (!Superstructure.climbSystem.isStow() || !Superstructure.funnelSystem.isStow()) {
169181
// Superstructure.climbStowThenStow()
170182
// } else {

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

Lines changed: 3 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -24,6 +24,8 @@ object RobotContainer {
2424

2525
var startByUnclimbing = false
2626

27+
var startWithADisabledClimb = false
28+
2729

2830
val xbox = CommandXboxController(2)
2931

@@ -59,7 +61,7 @@ object RobotContainer {
5961
// NamedCommands.registerCommand(pathplannerCommand.name, pathplannerCommand.cmd)
6062
// }
6163

62-
autoCommand = frc.robot.commands.auto.AutoBuilder.twoL4Left()
64+
autoCommand = frc.robot.commands.auto.AutoBuilder.twoL4Right()
6365
// autoCommand = getAutonomousCommand()
6466
// SmartDashboard.putData("Auto Chooser", autoChooser)
6567
}

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

Lines changed: 8 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -96,10 +96,16 @@ open class GoToPose(
9696
RobotContainer.drivetrain.applyDriveRequest(xVel, yVel, rotVel)
9797
}
9898

99-
override fun isFinished(): Boolean = endCondition()
99+
override fun isFinished(): Boolean {
100+
101+
val isDone =endCondition()
102+
SmartDashboard.putBoolean("GoToPose Is Done?", isDone)
103+
104+
return isDone
105+
}
100106

101107
override fun end(interrupted: Boolean) {
102-
println("drive command and then ending")
108+
// println("drive command and then ending")
103109
RobotState.autoDriving = false
104110
RobotContainer.currentDriveCommand = Optional.empty()
105111
}

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

Lines changed: 55 additions & 17 deletions
Original file line numberDiff line numberDiff line change
@@ -1,6 +1,7 @@
11
package frc.robot.commands
22

33
import cshcyberhawks.lib.math.MiscCalculations
4+
import edu.wpi.first.math.geometry.Rotation2d
45
import edu.wpi.first.wpilibj2.command.Command
56
import frc.robot.RobotContainer
67
import frc.robot.constants.AutoScoringConstants
@@ -36,14 +37,18 @@ object TeleopAutoAlign {
3637
if (OperatorControls.noWalk) {
3738
AllianceFlipUtil.apply(AutoScoringConstants.getReefPoseAtOffset(position.ordinal, side, 0.0))
3839
} else {
39-
val goalPose = AllianceFlipUtil.apply(AutoScoringConstants.getReefPoseAtOffset(position.ordinal, side, 0.0))
40+
val goalPose =
41+
AllianceFlipUtil.apply(AutoScoringConstants.getReefPoseAtOffset(position.ordinal, side, 0.0))
4042
// Make the position get closer as it gets closer to the final goal
4143
// val adjustX: Double = 0.3
42-
var adjustX: Double = min(1.0, RobotContainer.drivetrain.getSwervePose().translation.getDistance(goalPose.translation) * 0.75)
44+
var adjustX: Double = min(
45+
1.0,
46+
RobotContainer.drivetrain.getSwervePose().translation.getDistance(goalPose.translation) * 0.5
47+
)
4348
// val adjustX = 1.0
4449

4550
// Once we're close enough we can just jump to the goal
46-
if (adjustX < 0.25) {
51+
if (adjustX < 0.2) {
4752
adjustX = 0.0
4853
}
4954

@@ -62,11 +67,14 @@ object TeleopAutoAlign {
6267
val goalPose = AllianceFlipUtil.apply(AutoScoringConstants.getAlgaePoseAtOffset(position.ordinal, 0.0))
6368
// Make the position get closer as it gets closer to the final goal
6469
// val adjustX: Double = 0.3
65-
var adjustX: Double = min(1.0, RobotContainer.drivetrain.getSwervePose().translation.getDistance(goalPose.translation) * 0.75)
70+
var adjustX: Double = min(
71+
1.0,
72+
RobotContainer.drivetrain.getSwervePose().translation.getDistance(goalPose.translation) * .5
73+
)
6674
// val adjustX = 1.0
6775

6876
// Once we're close enough we can just jump to the goal
69-
if (adjustX < 0.25) {
77+
if (adjustX < 0.2) {
7078
adjustX = 0.0
7179
}
7280

@@ -75,18 +83,48 @@ object TeleopAutoAlign {
7583
})
7684
}
7785

78-
fun processorAlign(): Command = GoToPose({
79-
AllianceFlipUtil.apply(FieldConstants.processorPosition)
80-
}, { false })
86+
fun l1ReefAlign(): Command {
87+
return GoToPose({
88+
val position = OperatorControls.reefPosition
8189

82-
fun feederAlign(): Command = GoToPose({
83-
val leftPos = AllianceFlipUtil.apply(FieldConstants.leftFeederPosition)
84-
val rightPos = AllianceFlipUtil.apply(FieldConstants.rightFeederPosition)
90+
if (OperatorControls.noWalk) {
91+
AllianceFlipUtil.apply(AutoScoringConstants.getL1PoseAtOffset(position.ordinal, 0.0))
92+
} else {
93+
val goalPose =
94+
AllianceFlipUtil.apply(AutoScoringConstants.getL1PoseAtOffset(position.ordinal, 0.0))
95+
// Make the position get closer as it gets closer to the final goal
96+
// val adjustX: Double = 0.3
97+
var adjustX: Double = min(
98+
1.0,
99+
RobotContainer.drivetrain.getSwervePose().translation.getDistance(goalPose.translation) * 0.5
100+
)
101+
// val adjustX = 1.0
102+
103+
// Once we're close enough we can just jump to the goal
104+
if (adjustX < 0.2) {
105+
adjustX = 0.0
106+
}
85107

86-
if (RobotContainer.drivetrain.getSwervePose().translation.getDistance(leftPos.translation) < RobotContainer.drivetrain.getSwervePose().translation.getDistance(rightPos.translation)) {
87-
leftPos
88-
} else {
89-
rightPos
108+
AllianceFlipUtil.apply(AutoScoringConstants.getL1PoseAtOffset(position.ordinal, adjustX))
109+
}
110+
})
90111
}
91-
}, { false })
92-
}
112+
113+
fun processorAlign(): Command = GoToPose({
114+
AllianceFlipUtil.apply(FieldConstants.processorPosition)
115+
}, { false })
116+
117+
fun feederAlign(): Command = GoToPose({
118+
val leftPos = AllianceFlipUtil.apply(FieldConstants.leftFeederPosition)
119+
val rightPos = AllianceFlipUtil.apply(FieldConstants.rightFeederPosition)
120+
121+
if (RobotContainer.drivetrain.getSwervePose().translation.getDistance(leftPos.translation) < RobotContainer.drivetrain.getSwervePose().translation.getDistance(
122+
rightPos.translation
123+
)
124+
) {
125+
leftPos
126+
} else {
127+
rightPos
128+
}
129+
}, { false })
130+
}

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

Lines changed: 11 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -61,7 +61,7 @@ object AutoBuilder {
6161
Commands.runOnce({ Superstructure.scoreL4() }),
6262
AutoCommands.coralReefAlign(targets[i].position, targets[i].side)
6363
),
64-
WaitCommand(0.35),
64+
WaitCommand(0.7),
6565
Commands.runOnce({
6666
RobotState.actionConfirmed = true
6767
println("Score $i: ${autoTimer.get()}")
@@ -132,6 +132,15 @@ object AutoBuilder {
132132
Commands.runOnce({ Superstructure.removeAlgaeLow() }),
133133
AutoCommands.algaeReefAlign(AutoScoringConstants.ReefPositions.D)
134134
),
135-
AutoCommands.safeReefExit(AutoScoringConstants.ReefPositions.D)
135+
ParallelCommandGroup(
136+
AutoCommands.safeReefExit(AutoScoringConstants.ReefPositions.D),
137+
Commands.runOnce({ Superstructure.algaeSpit() }),
138+
),
139+
ParallelDeadlineGroup(
140+
Commands.waitUntil { RobotState.gamePieceState == GamePieceState.Algae },
141+
Commands.runOnce({ Superstructure.removeAlgaeHigh() }),
142+
AutoCommands.algaeReefAlign(AutoScoringConstants.ReefPositions.C)
143+
),
144+
AutoCommands.safeReefExit(AutoScoringConstants.ReefPositions.C)
136145
)
137146
}

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

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -36,7 +36,7 @@ object AutoCommands {
3636
// val adjustX = 1.0
3737

3838
// Once we're close enough we can just jump to the goal
39-
if (adjustX < 0.25) {
39+
if (adjustX < 0.2) {
4040
adjustX = 0.0
4141
}
4242

@@ -93,7 +93,7 @@ object AutoCommands {
9393
// val adjustX = 1.0
9494

9595
// Once we're close enough we can just jump to the goal
96-
if (adjustX < 0.3) {
96+
if (adjustX < 0.2) {
9797
adjustX = 0.0
9898
}
9999

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

Lines changed: 21 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -62,6 +62,27 @@ object AutoScoringConstants {
6262
)
6363
}
6464

65+
fun getL1PoseAtOffset(face: Int, x: Double): Pose2d {
66+
val poseDirection =
67+
Pose2d(FieldConstants.Reef.center, Rotation2d.fromDegrees((180 - (60 * face)).toDouble()))
68+
val adjustX: Double = reefFaceOffset + x - 0.425// + Units.inchesToMeters(3.0)
69+
val adjustY: Double = 1.075
70+
71+
return Pose2d(
72+
Translation2d(
73+
poseDirection
74+
.transformBy(Transform2d(adjustX, adjustY, Rotation2d()))
75+
.x,
76+
poseDirection
77+
.transformBy(Transform2d(adjustX, adjustY, Rotation2d()))
78+
.y
79+
),
80+
Rotation2d(
81+
poseDirection.rotation.radians
82+
).rotateBy(Rotation2d.fromDegrees(76.0))
83+
)
84+
}
85+
6586
enum class ReefPositions(var left: Pose2d, var right: Pose2d, var center: Pose2d) {
6687
A(Pose2d(), Pose2d(), Pose2d()),
6788
B(Pose2d(), Pose2d(), Pose2d()),

0 commit comments

Comments
 (0)