MoCoBo Code Update

Finally pushing the code that is currently running on the demo robot.