Initial version of Orange Team's EOPP code

Copied up after initial debugging on the robot. Had to disable motor
safety to make it work, so needs more review.