Rewrite program 

Fixes many bugs to allow the robot to work functionally