Step 1: Drive forward X Feet. (PID loop with wheel encoders).
Step 2: Wait for Camera Lock. (Kevin Watson's camera code modified to only search at a specific tilt - angle for better speed).
Step 3: Based on the PWM value of the tilt servo we used a lookup table to determine how much we needed to drive.
Step 4: Based on the angle of the pan servo we used a PID loop with the kit Gyroscope to turn to the correct angle
Step 5: We drove forward the amount named in Step 3.
Step 6: Release tube and back up.
You will notice we used a counter for timing some of the events - things like backing up, or making sure the robot stopped. We could have used the encoders to do it, but this was this year's quick and dirty. We had started doing it this way when we realize the other way would be more accurate. Next year we'll just check encoder values and potentiometer values instead of time values - it will help save some time.
Some tricks that we used: We knew that each field would be different, and we knew the camera would get knocked around a bit during the competition. To compensate, we added an "offset" value to both the distance and the turn angle that we would determine by actually running the robot.
Prior to them allowing us to calibrate on the field, we used the practice matches to calibrate. If we stopped 5 inches short, we'd add 5 inches to the offset to drive.
We adjusted the turning offset on the practice field by having the robot turn left and checking how far we were off, then turn right.
The key for us to get the camera to sustain a good lock was to shield the servo cables from all power sources, and then get the lock while the robot was stationary.
This was obviously preceded by an enormous amount of work on the robot to adjust gains, work around deadbands, etc to get it to drive the distance we told it and the angle we told it. Our robot couldn't drive less than 1 foot accurately, and couldn't turn less than 2 degrees accurately. Anything over those values and we were within an inch and within about 1/2 a degree of being dead on.
Hopefully this code helps some teams. Please - if you do take ideas from it give us some credit. Our egos are in desperate need of help after not making it to Atlanta
