@@ -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