Finished the test program for the coordinate system.

This should start the coordinate system and drive the robot.