Skip to content

Commit ad42a5f

Browse files
00magikarp00magikarp
committed
shoot no otf cry emoji
1 parent e7d3c90 commit ad42a5f

11 files changed

Lines changed: 195 additions & 39 deletions

File tree

src/main/kotlin/com/team4099/lib/vision/TimestampedTrigVisionUpdate.kt renamed to src/main/kotlin/com/team4099/lib/vision/TimestampedTagVisionUpdate.kt

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -3,7 +3,7 @@ package com.team4099.lib.vision
33
import org.team4099.lib.geometry.Transform3d
44
import org.team4099.lib.units.base.Time
55

6-
data class TimestampedTrigVisionUpdate(
6+
data class TimestampedTagVisionUpdate(
77
val timestamp: Time,
88
val targetTagID: Int,
99
val robotTTargetTag: Transform3d,

‎src/main/kotlin/com/team4099/robot2026/RobotContainer.kt‎

Lines changed: 9 additions & 16 deletions
Original file line numberDiff line numberDiff line change
@@ -2,7 +2,7 @@ package com.team4099.robot2026
22

33
import com.ctre.phoenix6.signals.NeutralModeValue
44
import com.team4099.robot2026.auto.AutonomousSelector
5-
import com.team4099.robot2026.commands.drivetrain.AimOTFCommand
5+
import com.team4099.robot2026.commands.drivetrain.AimCommand
66
import com.team4099.robot2026.commands.drivetrain.DrivePathOTF
77
import com.team4099.robot2026.commands.drivetrain.ResetGyroYawCommand
88
import com.team4099.robot2026.commands.drivetrain.TeleopDriveCommand
@@ -77,6 +77,7 @@ object RobotContainer {
7777

7878
var driveSimulation: SwerveDriveSimulation? = null
7979
var isAligning = false
80+
var hasAligned = false
8081

8182
init {
8283
SimulatedArena.overrideInstance(Arena2026Rebuilt(false))
@@ -246,21 +247,13 @@ object RobotContainer {
246247
superstructure.requestForceIntakeCommand(IntakeConstants.ANGLES.FORCE_DOWN_ANGLE))
247248

248249
ControlBoard.score.whileTrue(
249-
ConditionalCommand(
250-
AimOTFCommand(
251-
drivetrain,
252-
{ ControlBoard.forward.smoothDeadband(Constants.Joysticks.THROTTLE_DEADBAND) },
253-
{ ControlBoard.strafe.smoothDeadband(Constants.Joysticks.THROTTLE_DEADBAND) },
254-
{ ControlBoard.slowMode },
255-
driver = Jessika(),
256-
),
257-
InstantCommand()) {
258-
true
259-
// superstructure.currentState ==
260-
// Superstructure.Companion.SuperstructureStates.SCORE ||
261-
// superstructure.currentState ==
262-
// Superstructure.Companion.SuperstructureStates.PREP_SCORE
263-
})
250+
ConditionalCommand(AimCommand(drivetrain, vision), InstantCommand()) {
251+
true
252+
// superstructure.currentState ==
253+
// Superstructure.Companion.SuperstructureStates.SCORE ||
254+
// superstructure.currentState ==
255+
// Superstructure.Companion.SuperstructureStates.PREP_SCORE
256+
})
264257

265258
ControlBoard.leftTrenchOTF.whileTrue(
266259
ConditionalCommand(

‎src/main/kotlin/com/team4099/robot2026/auto/mode/IntakeQuadrantL1.kt‎

Lines changed: 2 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -2,6 +2,7 @@ package com.team4099.robot2026.auto.mode
22

33
import choreo.Choreo
44
import choreo.trajectory.SwerveSample
5+
import com.team4099.robot2026.RobotContainer.hasAligned
56
import com.team4099.robot2026.commands.drivetrain.AimOTFCommand
67
import com.team4099.robot2026.commands.drivetrain.FollowChoreoPath
78
import com.team4099.robot2026.config.constants.IntakeConstants
@@ -35,7 +36,7 @@ class IntakeQuadrantL1(
3536
superstructure.requestIdleCommand(),
3637
WaitCommand(3.5),
3738
superstructure.requestPrepScoreCommand())),
38-
AimOTFCommand(drivetrain, 1.seconds).until { AimOTFCommand.hasAligned },
39+
AimOTFCommand(drivetrain, 1.seconds).until { hasAligned },
3940
superstructure.requestScoreCommand(),
4041
RepeatCommand(
4142
SequentialCommandGroup(

‎src/main/kotlin/com/team4099/robot2026/commands/drivetrain/AimCommand.kt‎

Lines changed: 169 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,169 @@
1+
package com.team4099.robot2026.commands.drivetrain
2+
3+
import com.team4099.lib.hal.Clock
4+
import com.team4099.robot2026.RobotContainer
5+
import com.team4099.robot2026.RobotContainer.hasAligned
6+
import com.team4099.robot2026.config.constants.DrivetrainConstants
7+
import com.team4099.robot2026.config.constants.ShooterConstants
8+
import com.team4099.robot2026.subsystems.drivetrain.Drive
9+
import com.team4099.robot2026.subsystems.superstructure.Superstructure
10+
import com.team4099.robot2026.subsystems.superstructure.shooter.Shooter
11+
import com.team4099.robot2026.subsystems.vision.Vision
12+
import com.team4099.robot2026.util.CustomLogger
13+
import edu.wpi.first.units.LinearVelocityUnit
14+
import edu.wpi.first.units.Units.Degrees
15+
import edu.wpi.first.units.Units.Meters
16+
import edu.wpi.first.units.Units.Seconds
17+
import edu.wpi.first.units.measure.LinearVelocity as WPILinearVelocity
18+
import edu.wpi.first.wpilibj.DriverStation
19+
import edu.wpi.first.wpilibj.RobotBase
20+
import edu.wpi.first.wpilibj2.command.Command
21+
import kotlin.math.PI
22+
import org.ironmaple.simulation.SimulatedArena
23+
import org.ironmaple.simulation.seasonspecific.rebuilt2026.RebuiltFuelOnFly
24+
import org.team4099.lib.controller.PIDController
25+
import org.team4099.lib.geometry.Pose2d
26+
import org.team4099.lib.geometry.Pose3d
27+
import org.team4099.lib.kinematics.ChassisSpeeds
28+
import org.team4099.lib.units.Velocity
29+
import org.team4099.lib.units.base.inMeters
30+
import org.team4099.lib.units.base.inSeconds
31+
import org.team4099.lib.units.base.inches
32+
import org.team4099.lib.units.base.meters
33+
import org.team4099.lib.units.base.seconds
34+
import org.team4099.lib.units.derived.Radian
35+
import org.team4099.lib.units.derived.degrees
36+
import org.team4099.lib.units.derived.inDegrees
37+
import org.team4099.lib.units.derived.inRotation2ds
38+
import org.team4099.lib.units.derived.radians
39+
import org.team4099.lib.units.inMetersPerSecond
40+
import org.team4099.lib.units.perSecond
41+
42+
/**
43+
* Aim for the HUB or to pass, depending on current position on the field and which option is legal.
44+
* This command does NOT activate the shooter; that is the responsibility of the superstructure's
45+
* state machine.
46+
*
47+
* Note: This command assumes a flat drivetrain on z = 0.
48+
*
49+
* Note: This command never ends.
50+
*
51+
* @param drivetrain
52+
* @see com.team4099.robot2026.subsystems.superstructure.shooter.Shooter.calculateLaunchData
53+
* @author Nathan Arega, Ryan Chung
54+
*/
55+
class AimCommand(
56+
private val drivetrain: Drive,
57+
private val vision: Vision,
58+
) : Command() {
59+
60+
private val thetaPID: PIDController<Radian, Velocity<Radian>>
61+
62+
private var timeout = -1.seconds
63+
private var startTime = -1.seconds
64+
65+
private var startedInAuto = false
66+
67+
private var convergedPose = Pose3d()
68+
69+
init {
70+
thetaPID =
71+
PIDController(
72+
DrivetrainConstants.PID.TELEOP_THETA_PID_KP,
73+
DrivetrainConstants.PID.TELEOP_THETA_PID_KI,
74+
DrivetrainConstants.PID.TELEOP_THETA_PID_KD)
75+
76+
thetaPID.enableContinuousInput(-PI.radians, PI.radians)
77+
}
78+
79+
override fun initialize() {
80+
thetaPID.reset()
81+
82+
hasAligned = false
83+
startTime = Clock.timestamp
84+
startedInAuto = DriverStation.isAutonomous()
85+
86+
RobotContainer.isAligning = true
87+
}
88+
89+
override fun execute() {
90+
val currentlyTrackedPose: Pose3d
91+
92+
val expectedPose = drivetrain.pose + vision.lastTagVisionUpdate.robotTTargetTag
93+
94+
if (convergedPose == Pose3d()) {
95+
val robotTExpected = drivetrain.pose.relativeTo(expectedPose)
96+
if (robotTExpected.x.absoluteValue < 3.inches &&
97+
robotTExpected.y.absoluteValue < 3.inches &&
98+
(robotTExpected.rotation.z < 3.degrees ||
99+
robotTExpected.rotation.z > (-180.degrees + 3.degrees))) {
100+
currentlyTrackedPose = expectedPose
101+
convergedPose = expectedPose
102+
} else {
103+
currentlyTrackedPose = drivetrain.pose
104+
}
105+
} else {
106+
currentlyTrackedPose = convergedPose
107+
}
108+
109+
CustomLogger.recordOutput("ActiveCommands/FaceHubCommand", true)
110+
111+
val (distanceToHub, launchSpeed, timeOfFlight, wantedRotation) =
112+
Shooter.calculateLaunchData(currentlyTrackedPose.toPose2d(), ChassisSpeeds())
113+
114+
val thetaVel = thetaPID.calculate(currentlyTrackedPose.rotation.z, wantedRotation)
115+
116+
CustomLogger.recordOutput("FaceHubCommand/thetaError", thetaPID.error.inDegrees)
117+
118+
CustomLogger.recordOutput(
119+
"FaceHubCommand/wantedPose",
120+
Pose2d(currentlyTrackedPose.x, currentlyTrackedPose.y, wantedRotation).pose2d)
121+
122+
hasAligned = thetaPID.error.absoluteValue < 3.degrees
123+
124+
CustomLogger.recordOutput("FaceHubCommand/hasAligned", hasAligned)
125+
126+
if (!hasAligned) {
127+
drivetrain.runSpeeds(
128+
ChassisSpeeds.fromFieldRelativeSpeeds(
129+
0.meters.perSecond, 0.meters.perSecond, thetaVel, drivetrain.pose.rotation.z))
130+
} else {
131+
drivetrain.stopWithX()
132+
}
133+
134+
if (RobotBase.isSimulation() &&
135+
(hasAligned &&
136+
Clock.timestamp.inSeconds % 1 < 0.04 &&
137+
RobotContainer.superstructure.currentState ==
138+
Superstructure.Companion.SuperstructureStates.SCORE ||
139+
DriverStation.isAutonomous())) {
140+
SimulatedArena.getInstance()
141+
.addGamePieceProjectile(
142+
RebuiltFuelOnFly(
143+
drivetrain.pose.translation.toTranslation2d().translation2d,
144+
ShooterConstants.SHOOTER_OFFSET.translation.translation2d,
145+
edu.wpi.first.math.kinematics.ChassisSpeeds.fromRobotRelativeSpeeds(
146+
drivetrain.chassisSpeeds.chassisSpeedsWPILIB,
147+
drivetrain.rotation.z.inRotation2ds),
148+
(drivetrain.pose.rotation.z + ShooterConstants.SHOOTER_OFFSET.rotation)
149+
.inRotation2ds,
150+
Meters.of(ShooterConstants.SHOOTER_HEIGHT.inMeters),
151+
WPILinearVelocity.ofBaseUnits(
152+
launchSpeed.inMetersPerSecond, LinearVelocityUnit.combine(Meters, Seconds)),
153+
Degrees.of(ShooterConstants.SHOOTER_ANGLE.inDegrees)))
154+
}
155+
}
156+
157+
override fun isFinished(): Boolean {
158+
return startedInAuto xor DriverStation.isAutonomous()
159+
}
160+
161+
override fun end(interrupted: Boolean) {
162+
CustomLogger.recordOutput("FaceHubCommand/interrupted", interrupted)
163+
164+
drivetrain.runSpeeds(ChassisSpeeds())
165+
RobotContainer.isAligning = false
166+
167+
CustomLogger.recordOutput("ActiveCommands/FaceHubCommand", false)
168+
}
169+
}

‎src/main/kotlin/com/team4099/robot2026/commands/drivetrain/AimOTFCommand.kt‎

Lines changed: 1 addition & 6 deletions
Original file line numberDiff line numberDiff line change
@@ -2,6 +2,7 @@ package com.team4099.robot2026.commands.drivetrain
22

33
import com.team4099.lib.hal.Clock
44
import com.team4099.robot2026.RobotContainer
5+
import com.team4099.robot2026.RobotContainer.hasAligned
56
import com.team4099.robot2026.config.constants.DrivetrainConstants
67
import com.team4099.robot2026.config.constants.ShooterConstants
78
import com.team4099.robot2026.subsystems.drivetrain.Drive
@@ -49,8 +50,6 @@ import org.team4099.lib.units.perSecond
4950
*
5051
* Note: This command assumes a flat drivetrain on z = 0.
5152
*
52-
* Note: This command never ends.
53-
*
5453
* @param drivetrain
5554
* @param vSup Supplier for drivetrain field-relative chassis speeds
5655
* @see com.team4099.robot2026.subsystems.superstructure.shooter.Shooter.calculateLaunchData
@@ -209,8 +208,4 @@ class AimOTFCommand(
209208

210209
CustomLogger.recordOutput("ActiveCommands/FaceHubCommand", false)
211210
}
212-
213-
companion object {
214-
var hasAligned: Boolean = false
215-
}
216211
}

‎src/main/kotlin/com/team4099/robot2026/commands/drivetrain/AimTagCommand.kt‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -49,7 +49,7 @@ class AimTagCommand(val drivetrain: Drive, val vision: Vision) : Command() {
4949
}
5050

5151
override fun execute() {
52-
val lastUpdate = vision.lastTrigVisionUpdate
52+
val lastUpdate = vision.lastTagVisionUpdate
5353
if (lastUpdate.timestamp <= 0.seconds ||
5454
lastUpdate.robotTTargetTag.transform3d == Transform3d())
5555
return

‎src/main/kotlin/com/team4099/robot2026/commands/drivetrain/CoolerTargetTagCommand.kt‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -128,7 +128,7 @@ class CoolerTargetTagCommand(
128128
override fun execute() {
129129
CustomLogger.recordOutput("ActiveCommands/CoolerTargetTagCommand", true)
130130

131-
val lastUpdate = vision.lastTrigVisionUpdate
131+
val lastUpdate = vision.lastTagVisionUpdate
132132
val odomTTag = lastUpdate.robotTTargetTag
133133

134134
val exists = odomTTag.translation.norm != 0.meters

‎src/main/kotlin/com/team4099/robot2026/config/constants/Constants.kt‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -27,7 +27,7 @@ object Constants {
2727
val LOOP_PERIOD_TIME = 20.milli.seconds
2828
val POWER_DISTRIBUTION_HUB_ID = 1
2929

30-
const val SIMULATE_VISION = false
30+
const val SIMULATE_VISION = true
3131
const val DISABLE_COLLISIONS = false
3232

3333
val ROBOT_WEIGHT = 125.pounds

‎src/main/kotlin/com/team4099/robot2026/config/constants/VisionConstants.kt‎

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -16,8 +16,8 @@ import org.team4099.lib.units.derived.degrees
1616
object VisionConstants {
1717
val CONTROLLER_RUMBLE_DIST = 2.25.meters
1818

19-
val BLUE_TARGET_TAGS = arrayOf<Int>()
20-
val RED_TARGET_TAGS = arrayOf<Int>()
19+
val BLUE_TARGET_TAGS = arrayOf<Int>(18, 19, 20, 21, 24, 25, 26, 27)
20+
val RED_TARGET_TAGS = arrayOf<Int>(2, 3, 4, 5, 8, 9, 10, 11)
2121

2222
val AMBIGUITY_THESHOLD = 1.0
2323
val CONFIDENCE_THRESHOLD = 0.75

‎src/main/kotlin/com/team4099/robot2026/subsystems/superstructure/Superstructure.kt‎

Lines changed: 3 additions & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -2,7 +2,7 @@ package com.team4099.robot2026.subsystems.superstructure
22

33
import com.team4099.lib.hal.Clock
44
import com.team4099.robot2026.RobotContainer
5-
import com.team4099.robot2026.commands.drivetrain.AimOTFCommand
5+
import com.team4099.robot2026.RobotContainer.hasAligned
66
import com.team4099.robot2026.config.constants.ClimbConstants
77
import com.team4099.robot2026.config.constants.Constants
88
import com.team4099.robot2026.config.constants.FeederConstants
@@ -211,8 +211,7 @@ class Superstructure(
211211
SuperstructureStates.SCORE -> {
212212
shooter.currentRequest = Request.ShooterRequest.TargetVelocity(shooterTargetRPM)
213213

214-
if (shooter.isAtTargetedVelocity &&
215-
(AimOTFCommand.hasAligned || !RobotContainer.isAligning)) {
214+
if (shooter.isAtTargetedVelocity && (hasAligned || !RobotContainer.isAligning)) {
216215
feeder.currentRequest =
217216
Request.FeederRequest.TargetVelocity(FeederConstants.SCORE_VELOCITY)
218217
hopper.currentRequest =
@@ -235,8 +234,7 @@ class Superstructure(
235234
Request.RollersRequest.OpenLoop(RollersConstants.INTAKE_VOLTAGE)
236235
shooter.currentRequest = Request.ShooterRequest.TargetVelocity(shooterTargetRPM)
237236

238-
if (shooter.isAtTargetedVelocity &&
239-
(AimOTFCommand.hasAligned || !RobotContainer.isAligning)) {
237+
if (shooter.isAtTargetedVelocity && (hasAligned || !RobotContainer.isAligning)) {
240238
feeder.currentRequest =
241239
Request.FeederRequest.TargetVelocity(FeederConstants.SCORE_VELOCITY)
242240
hopper.currentRequest =

0 commit comments

Comments
 (0)