Rewrite program

Fixes many bugs to allow the robot to work functionally