WORKING VERSION (main.c)

This is the working version in the robot as of 10/7/14