Skip to content

Commit

Permalink
feat: branch alignment switcher
Browse files Browse the repository at this point in the history
  • Loading branch information
@Craftzman7
Craftzman7 committed Jan 17, 2025
1 parent 3d822a5 commit 750a7d3
Show file tree
Hide file tree
Showing 3 changed files with 78 additions and 29 deletions.
25 changes: 24 additions & 1 deletion src/main/kotlin/com/frcteam3636/frc2025/Robot.kt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -2,6 +2,7 @@ package com.frcteam3636.frc2025

import com.ctre.phoenix6.StatusSignal
import com.frcteam3636.frc2025.subsystems.drivetrain.Drivetrain
import com.frcteam3636.frc2025.subsystems.drivetrain.poi.ReefBranchSide
import com.frcteam3636.frc2025.utils.Elastic
import com.frcteam3636.frc2025.utils.ElasticNotification
import com.frcteam3636.frc2025.utils.NotificationLevel
Expand Down Expand Up @@ -147,7 +148,29 @@ object Robot : LoggedRobot() {
private fun configureBindings() {
Drivetrain.defaultCommand = Drivetrain.driveWithJoysticks(joystickLeft, joystickRight)

controller.a().whileTrue(Drivetrain.alignToNearestPOI())
JoystickButton(joystickRight, 3).onTrue(Commands.runOnce({
println("Setting desired target node to left branch.")
Drivetrain.currentTargetSelection = ReefBranchSide.Left
}))

JoystickButton(joystickRight, 4).onTrue(Commands.runOnce({
println("Setting desired target node to right branch.")
Drivetrain.currentTargetSelection = ReefBranchSide.Right
}))

JoystickButton(joystickRight, 1).whileTrue(Drivetrain.alignToClosestPOV())

controller.a().whileTrue(Drivetrain.alignToClosestPOV())

controller.b().onTrue(Commands.runOnce({
println("Setting desired target node to left branch.")
Drivetrain.currentTargetSelection = ReefBranchSide.Left
}))

controller.x().onTrue(Commands.runOnce({
println("Setting desired target node to right branch.")
Drivetrain.currentTargetSelection = ReefBranchSide.Right
}))

// (The button with the yellow tape on it)
JoystickButton(joystickLeft, 8).onTrue(Commands.runOnce({
Expand Down
9 changes: 5 additions & 4 deletions src/main/kotlin/com/frcteam3636/frc2025/subsystems/drivetrain/Drivetrain.kt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -10,8 +10,7 @@ import com.frcteam3636.frc2025.subsystems.drivetrain.Drivetrain.Constants.JOYSTI
import com.frcteam3636.frc2025.subsystems.drivetrain.Drivetrain.Constants.ROTATION_PID_GAINS
import com.frcteam3636.frc2025.subsystems.drivetrain.Drivetrain.Constants.ROTATION_SENSITIVITY
import com.frcteam3636.frc2025.subsystems.drivetrain.Drivetrain.Constants.TRANSLATION_SENSITIVITY
import com.frcteam3636.frc2025.subsystems.drivetrain.poi.AprilTagTarget
import com.frcteam3636.frc2025.subsystems.drivetrain.poi.closestTargetTo
import com.frcteam3636.frc2025.subsystems.drivetrain.poi.*
import com.frcteam3636.frc2025.utils.ElasticWidgets
import com.frcteam3636.frc2025.utils.fieldRelativeTranslation2d
import com.frcteam3636.frc2025.utils.math.PIDController
Expand Down Expand Up @@ -64,6 +63,8 @@ object Drivetrain : Subsystem, Sendable {
private val questNavInputs = LoggedQuestNavInputs()
private var questNavCalibrated = false

var currentTargetSelection: ReefBranchSide = ReefBranchSide.Right

private val mt2Algo = LimelightAlgorithm.MegaTag2 ( {
poseEstimator.estimatedPosition.rotation
}, {
Expand Down Expand Up @@ -351,10 +352,10 @@ object Drivetrain : Subsystem, Sendable {
})
}

fun alignToNearestPOI(): Command = defer {
fun alignToClosestPOV(): Command = defer {
val target = AprilTagTarget.currentAllianceTargets
.asIterable()
.closestTargetTo(estimatedPose)
.closestTargetToWithSelection(estimatedPose, currentTargetSelection)

AutoBuilder.pathfindToPose(target.pose, DEFAULT_PATHING_CONSTRAINTS)
}
Expand Down
73 changes: 49 additions & 24 deletions src/main/kotlin/com/frcteam3636/frc2025/subsystems/drivetrain/poi/AlignTargets.kt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -20,6 +20,16 @@ interface AlignableTarget {
val pose: Pose2d
}

data class TargetGroup(val targets: Array<AprilTagTarget>)

data class TargetSelection(
val group: TargetGroup,
val idx: Int = 0,
) {
val pose: Pose2d
get() = group.targets[idx].pose
}

/**
* An alignment target relative to an April Tag's location.
*
Expand All @@ -36,8 +46,8 @@ class AprilTagTarget(aprilTagId: Int, offset: Translation2d) : AlignableTarget {
Meters.zero(),
// Move left/right from the april tag to get in front of the reef branch
when (side) {
ReefBranchSide.Right -> APRIL_TAG_HORIZONTAL_OFFSET
ReefBranchSide.Left -> -APRIL_TAG_HORIZONTAL_OFFSET
ReefBranchSide.Left -> APRIL_TAG_HORIZONTAL_OFFSET
ReefBranchSide.Right -> -APRIL_TAG_HORIZONTAL_OFFSET
}
),
)
Expand Down Expand Up @@ -68,38 +78,40 @@ class AprilTagTarget(aprilTagId: Int, offset: Translation2d) : AlignableTarget {
}

companion object {
/** Makes a list of reef side targets. The given april tags are used to determine the pose of the target. */
private fun branchTargetsFromIds(ids: IntRange): Array<AprilTagTarget> {
private fun branchTargetsFromIds(ids: IntRange): Array<TargetGroup> {
return ids
.flatMap { id ->
ReefBranchSide.entries.map { side ->
AprilTagTarget(id, side)
}
.map { id ->
TargetGroup(
arrayOf(
AprilTagTarget(id, ReefBranchSide.Left),
AprilTagTarget(id, ReefBranchSide.Right),
)
)
}
.toTypedArray()
}

val redAllianceTargets = arrayOf(
val redAllianceTargets: Array<TargetGroup> = arrayOf(
// Reef branches
*branchTargetsFromIds(6..11),
// Processor
AprilTagTarget(3, Translation2d()),
TargetGroup(arrayOf(AprilTagTarget(3, Translation2d()))),
// Human Player Stations
AprilTagTarget(1, Translation2d()),
AprilTagTarget(2, Translation2d())
TargetGroup(arrayOf(AprilTagTarget(1, Translation2d()))),
TargetGroup(arrayOf(AprilTagTarget(2, Translation2d())))
)

val blueAllianceTargets = arrayOf(
val blueAllianceTargets: Array<TargetGroup> = arrayOf(
// Reef branches
*branchTargetsFromIds(17..22),
// Processor
AprilTagTarget(16, Translation2d()),
TargetGroup(arrayOf(AprilTagTarget(16, Translation2d()))),
// Human Player Stations
AprilTagTarget(13, Translation2d()),
AprilTagTarget(12, Translation2d())
TargetGroup(arrayOf(AprilTagTarget(13, Translation2d()))),
TargetGroup(arrayOf(AprilTagTarget(12, Translation2d())))
)

val currentAllianceTargets: Array<AprilTagTarget>
val currentAllianceTargets: Array<TargetGroup>
get() {
return when (DriverStation.getAlliance().getOrNull()) {
DriverStation.Alliance.Red -> redAllianceTargets
Expand All @@ -109,12 +121,25 @@ class AprilTagTarget(aprilTagId: Int, offset: Translation2d) : AlignableTarget {
}
}

/**
* Returns the target closest to the given pose.
*/
fun Iterable<AprilTagTarget>.closestTargetTo(pose: Pose2d): AprilTagTarget =
minByOrNull { it.pose.relativeTo(pose).translation.norm }
?: error("Can't find closest target ")

@Suppress("unused")
fun Iterable<TargetGroup>.closestTargetTo(pose: Pose2d): TargetSelection =
flatMap { group ->
group.targets.indices.map {
TargetSelection(group, idx = it)
}
}.minByOrNull {
it.pose.relativeTo(pose).translation.norm
} ?: error("Can't find closest target")

fun Iterable<TargetGroup>.closestTargetToWithSelection(pose: Pose2d, reefBranchSide: ReefBranchSide): TargetSelection =
map { group ->
if (group.targets.size >= reefBranchSide.ordinal + 1) {
TargetSelection(group, idx = reefBranchSide.ordinal)
} else {
TargetSelection(group, idx = 0)
}
}.minByOrNull { it : TargetSelection ->
it.pose.relativeTo(pose).translation.norm
} ?: error("Can't find closest target")

private val APRIL_TAG_HORIZONTAL_OFFSET = Meters.of(0.147525)

0 comments on commit 750a7d3

Please sign in to comment.