Skip to content

Commit 85b09bd

Browse files
chore: auto-fix Spotless formatting
1 parent 4b23ed8 commit 85b09bd

1 file changed

Lines changed: 48 additions & 46 deletions

File tree

  • src/main/kotlin/com/team4099/robot2026/subsystems/vision/camera

src/main/kotlin/com/team4099/robot2026/subsystems/vision/camera/CameraIO.kt

Lines changed: 48 additions & 46 deletions
Original file line numberDiff line numberDiff line change
@@ -43,76 +43,78 @@ interface CameraIO {
4343
val photonEstimator: PhotonPoseEstimator
4444

4545
fun calculateTagTrustScore(
46-
tag: PhotonTrackedTarget,
47-
distanceToTarget: Double,
48-
robotTTag: org.team4099.lib.geometry.Transform3d,
49-
chassisSpeeds: org.team4099.lib.kinematics.ChassisSpeeds
46+
tag: PhotonTrackedTarget,
47+
distanceToTarget: Double,
48+
robotTTag: org.team4099.lib.geometry.Transform3d,
49+
chassisSpeeds: org.team4099.lib.kinematics.ChassisSpeeds
5050
): Double {
5151
// 1. Ambiguity trust (0-1, higher is better)
5252
val ambiguityTrust = (1.0 - tag.poseAmbiguity).coerceIn(0.0, 1.0)
5353

5454
// 2. Distance trust (0-1, closer is better)
55-
val distanceTrust = when {
56-
distanceToTarget <= 2.0 -> 1.0
57-
distanceToTarget <= 4.0 -> 1.0 - ((distanceToTarget - 2.0) / 2.0) * 0.3
58-
distanceToTarget <= 6.0 -> 0.7 - ((distanceToTarget - 4.0) / 2.0) * 0.35
59-
else -> 0.35
60-
}
55+
val distanceTrust =
56+
when {
57+
distanceToTarget <= 2.0 -> 1.0
58+
distanceToTarget <= 4.0 -> 1.0 - ((distanceToTarget - 2.0) / 2.0) * 0.3
59+
distanceToTarget <= 6.0 -> 0.7 - ((distanceToTarget - 4.0) / 2.0) * 0.35
60+
else -> 0.35
61+
}
6162

6263
// 3. Angle trust
6364
val angleToTag = kotlin.math.abs(kotlin.math.atan2(robotTTag.y.inMeters, robotTTag.x.inMeters))
64-
val angleTrust = when {
65-
angleToTag <= kotlin.math.PI / 6 -> 1.0
66-
angleToTag <= kotlin.math.PI / 3 ->
67-
1.0 - ((angleToTag - kotlin.math.PI / 6) / (kotlin.math.PI / 6)) * 0.3
68-
angleToTag <= kotlin.math.PI / 2 ->
69-
0.7 - ((angleToTag - kotlin.math.PI / 3) / (kotlin.math.PI / 6)) * 0.4
70-
else -> 0.3
71-
}
65+
val angleTrust =
66+
when {
67+
angleToTag <= kotlin.math.PI / 6 -> 1.0
68+
angleToTag <= kotlin.math.PI / 3 ->
69+
1.0 - ((angleToTag - kotlin.math.PI / 6) / (kotlin.math.PI / 6)) * 0.3
70+
angleToTag <= kotlin.math.PI / 2 ->
71+
0.7 - ((angleToTag - kotlin.math.PI / 3) / (kotlin.math.PI / 6)) * 0.4
72+
else -> 0.3
73+
}
7274

7375
// 4. Drivetrain velocity trust (slower is better)
7476
val linearVelocity =
75-
kotlin.math.hypot(chassisSpeeds.vx.inMetersPerSecond, chassisSpeeds.vy.inMetersPerSecond)
77+
kotlin.math.hypot(chassisSpeeds.vx.inMetersPerSecond, chassisSpeeds.vy.inMetersPerSecond)
7678
val angularVelocity = kotlin.math.abs(chassisSpeeds.omega.inRadiansPerSecond)
7779

78-
val linearVelocityTrust = when {
79-
linearVelocity <= 0.5 -> 1.0
80-
linearVelocity <= 2.0 -> 1.0 - ((linearVelocity - 0.5) / 1.5) * 0.4
81-
linearVelocity <= 4.0 -> 0.6 - ((linearVelocity - 2.0) / 2.0) * 0.3
82-
else -> 0.3
83-
}
80+
val linearVelocityTrust =
81+
when {
82+
linearVelocity <= 0.5 -> 1.0
83+
linearVelocity <= 2.0 -> 1.0 - ((linearVelocity - 0.5) / 1.5) * 0.4
84+
linearVelocity <= 4.0 -> 0.6 - ((linearVelocity - 2.0) / 2.0) * 0.3
85+
else -> 0.3
86+
}
8487

85-
val angularVelocityTrust = when {
86-
angularVelocity <= 0.5 -> 1.0
87-
angularVelocity <= 2.0 -> 1.0 - ((angularVelocity - 0.5) / 1.5) * 0.5
88-
angularVelocity <= 4.0 -> 0.5 - ((angularVelocity - 2.0) / 2.0) * 0.35
89-
else -> 0.15
90-
}
88+
val angularVelocityTrust =
89+
when {
90+
angularVelocity <= 0.5 -> 1.0
91+
angularVelocity <= 2.0 -> 1.0 - ((angularVelocity - 0.5) / 1.5) * 0.5
92+
angularVelocity <= 4.0 -> 0.5 - ((angularVelocity - 2.0) / 2.0) * 0.35
93+
else -> 0.15
94+
}
9195

9296
val velocityTrust =
93-
(linearVelocityTrust * VisionConstants.LINEAR_VELOCITY_TRUST_WEIGHT) +
94-
(angularVelocityTrust * VisionConstants.ANGULAR_VELOCITY_TRUST_WEIGHT)
95-
96-
97+
(linearVelocityTrust * VisionConstants.LINEAR_VELOCITY_TRUST_WEIGHT) +
98+
(angularVelocityTrust * VisionConstants.ANGULAR_VELOCITY_TRUST_WEIGHT)
9799

98100
val weightedTrust =
99-
(ambiguityTrust * VisionConstants.AMBIGUITY_TRUST_RATING) +
100-
(distanceTrust * VisionConstants.DISTANCE_TRUST_RATING) +
101-
(angleTrust * VisionConstants.ANGLE_TRUST_RATING) +
102-
(velocityTrust * VisionConstants.VELOCITY_TRUST_RATING)
103-
101+
(ambiguityTrust * VisionConstants.AMBIGUITY_TRUST_RATING) +
102+
(distanceTrust * VisionConstants.DISTANCE_TRUST_RATING) +
103+
(angleTrust * VisionConstants.ANGLE_TRUST_RATING) +
104+
(velocityTrust * VisionConstants.VELOCITY_TRUST_RATING)
104105

105106
return weightedTrust.coerceIn(0.0, 1.0)
106107
}
107108

108109
fun calculateTagTrust(
109-
tag: PhotonTrackedTarget,
110-
distanceToTarget: Double,
111-
robotTTag: org.team4099.lib.geometry.Transform3d,
112-
chassisSpeeds: org.team4099.lib.kinematics.ChassisSpeeds,
113-
minTrustThreshold: Double = VisionConstants.TAG_TRUST_THRESHOLD
110+
tag: PhotonTrackedTarget,
111+
distanceToTarget: Double,
112+
robotTTag: org.team4099.lib.geometry.Transform3d,
113+
chassisSpeeds: org.team4099.lib.kinematics.ChassisSpeeds,
114+
minTrustThreshold: Double = VisionConstants.TAG_TRUST_THRESHOLD
114115
): Boolean {
115-
return calculateTagTrustScore(tag, distanceToTarget, robotTTag, chassisSpeeds) >= minTrustThreshold
116+
return calculateTagTrustScore(tag, distanceToTarget, robotTTag, chassisSpeeds) >=
117+
minTrustThreshold
116118
}
117119

118120
class CameraInputs : LoggableInputs {

0 commit comments

Comments
 (0)