Ok, so first, full disclosure, I have no idea exactly how 254 did theirs and its been a while since I have messed with either vision processing, programming, or trig, but the fundamentals are fairly basic and we have google.
There are two ways of doing this, one which requires trig and another which doesn’t, the latter I’ll save for the end.
Now for the first method, I need to make an assumption: your camera is not centered above your robot’s center of rotation (ie: the point over which your robot will pivot if one side of your drivetrain is in full forward and the other is in full reverse). This is important, since the angle you want your robot to rotate to face a ball will not be the same as the angle require to rotate your camera to face the ball.
Looking at the picture, we can see that theta, the angle from straight ahead to the center of the ball, is not the same as phi, the angle from straight ahead to the center of the ball. We want to find phi given theta, so lets look at it in a more trigonometric kind of way.
If you have taken a geometry or maybe a pre-calc class, you may recognize this triangle can be solved if we know a couple of sides lengths and an angle. Remember, we are looking for phi, angel A in the diagram, given theta. Speaking of givens, lets list them.
-
Theta = 45 deg
The value of theta will change on the balls position, but let’s call it 45 degrees
-
c = 12"
This is the distance from the center of rotation to the center of the camera. Let’s say its 12 inches, but this will depend on your robot.
-
a = 20"
The distance from the ball to the camera, we will discuss how to find this later, but let’s call it 20 inches.
If you have ever solved triangles before, you may recognize that what we have here is an SAS, or side-angle-side, triangle. To solve this, we will use the Law of Cosines and Law of Sines. If you haven’t heard of these before, don’t worry, they are quite simple to solve in code. Let’s solve it:
-
Since we know theta, we also know angle B by subtracting theta from 180:
B = 180 - theta = 180 - 45 = 135 deg
-
Now for the Law of Cosines to find side b (Note that I switched some variable names for this particular example, however the structure remains the same):
b^2 = a^2 + c^2 - 2ac cos(B)
or simplified to find b:
b = sqrt(a^2 + c^2 - 2ac cos(B))
b = 29.72"
-
Now of Law of Sines to find angle A:
sin(B / b) = sin(A / a)
or simplified to find A:
A = arcsin((a sin(B)) / b)
A = 28.41 deg
Thus, from theta, we have found A, or phi.
Now, like I said previously, we have to find side a. Since you are already doing some vision processing, you will probably be able to measure the diameter of the ball and use its known diameter compared to its perceived diameter to find its distance from the camera. I have never done this before, but it can probably be done like this.
Since we have side a, we can find theta. If the center of the camera is facing straight ahead, we measure the horizontal distance (d) from the center of the ball to the center of the camera in pixels and convert it to inches or whatever using the processes in the link above. Then its just a simple matter of arcsin( a / d ) to find the angle theta.
And now for the second method. While method one was a lot of fun to figure out, this may be easier to implement. Basically, if the ball is to the left of center of your camera, rotate the robot counterclockwise until the ball is in the middle of the camera. Control loops may also be implemented to prevent overreactions and stalling.
Anyway, hope this helps!
