Write some basic testing code for the robot.

Write basic [tests](http://wpilib.screenstepslive.com/s/3120/m/8564/l/88513-how-to-write-an-easy-to-test-robot-program) for the robot.