Optical Sensor, Autonomous, and ArmControl2 Edit

Added code to test optical sensor, added a small autonomous piece to
start making progress in that section as well. Edited the Armcontrol2
code to fix what I believe might be a cycle/loop problem.