[Beaker]

You might find that using a camera and a LED light ring is beneficial in this situation.

What you can do is detect the vision targets on a peg, find the center of the target, and calculate how much you need to turn based on the image. With that information it shouldn't be too difficult to program a drivetrain mode that auto-locks onto the peg using a PID.

There is plenty of example code floating around on this subject from last year (I would take a look at the recent code postings). Also, FIRST provides a lot of example code for LabVIEW teams, so if you are one of those, you should take advantage of that.*

*That might be the same for other languages, but my team used LabVIEW, so I wouldn't know for sure.