Here's my own personal track() routine, since dockkit's track() doesn't meet my needs:
var errorIsLarge = false
func track(rectangles: [CGRect], fov: Double) async {
guard !rectangles.isEmpty else {
await setVelocity(pitch: 0, yaw: 0, roll: 0)
return
}
let r = rectangles.reduce(CGRect.null) { $0.union($1) }
let xOffset = 2 * (r.midX - 0.5)
var thetaOffset = xOffset * fov
print("Tracking: \(thetaOffset.asDegrees) degrees off, midX = \(r.midX)")
if abs(thetaOffset) > (3.0).asRadians {
print("Error is large set true")
errorIsLarge = true
}
if abs(thetaOffset) < (3.0).asRadians {
if !errorIsLarge {
thetaOffset = 0
}
}
if abs(thetaOffset) < (1.0).asRadians {
errorIsLarge = false
print("error is large is FALSE")
thetaOffset = 0
}
print("Setting velocity to \(-thetaOffset * 3)")
await setVelocity(pitch: 0, yaw: -thetaOffset * 3, roll: 0)
What are we doing here? Whenever we get more than 3 degrees off target, that's enough error to try to correct it. If our error is less than 3 degrees, and we're not in a "large error state", ignore the error. We want smooth tracking.
We leave being in a "large" error state when we reduce the error to less than 1 degree.
We set the velocity to the opposite of 3 X thetaOffset, where thetaOffset is how far off target we are, noting that as I said above, at times we want to tolerate small errors. The idea is that if you get close enough, stop trying to micro-correct.
In my case, I'm taking all my observation rectangles, unioning them, and taking the center. However you make observations, at the end of the day, you're going to get some absolute theta error, and that's what you deal with.
Note: I'm not trying to pitch up or down (or, god forbid, roll!) the camera. Just yaw (i.e. rotate around the vertical axis).
