Team 254 Presents: FRC 2016 Code

[Jared Russell]

Originally Posted by apache8080

Are you guys calculating distance from the goal to adjust the hood? If so, how?[/url]?

Yep. I'll point you to a few places in the code that help explain how.

First, in the Android app, we find the pixel coordinates corresponding to the center of the goal:
https://github.com/Team254/FRC-2016-...View.java#L131

...and then turn those pixel coordinates into a 3D vector representing the "ray" shooting out of the camera towards the target. The vector has an x component (+x is out towards the goal) that is always set to 1; a y component (+y is to the left in the camera image); and a z component (+z is up). This vector is unit-less, but the ratios between x, y, and z define angles relative to the back of the phone. The math behind how we create this vector is explained here.

The resulting vector is then sent over a network interface to the RoboRIO. The first interesting place it is used is here:
https://github.com/Team254/FRC-2016-...tate.java#L187

In that function, we turn the unit-less 3D vector from the phone into real-world range and bearing. We can measure pitch (angle above the plane of the floor) by using our vector with some simple trig; same thing for yaw (angle left/right). Since we know where the phone is on the robot (from CAD, and from reading the sensors on our turret), we can compensate for the fact that the camera is not mounted level, and the turret may be turned. Finally, we know how tall the goal should be (and how high the camera should be), so we can use more trigonometry to use our pitch and yaw angles to determine distance. We feed these values into a tracker (which smooths out our measurements by averaging recent goal detections that seem to correspond to the same goal).

The final part is to feed our distance measurement (and bearing) into our auto-aiming code. We do this here:
https://github.com/Team254/FRC-2016-...ture.java#L718

Notice that we use a function to convert between distance and hood angle. This function was tuned (many times throughout the season) by putting the robot on the field, shooting a bunch of balls from a bunch of different spots, and manually adjusting hood angle until the shots were optimized for each range. We'd record the angles that worked best, and then interpolate between the two nearest recorded angles for any given distance we want to shoot from.