Fixed problems, code runs again. Mapped IO to correct ports on the physical robot.