Code compiles, working on getting it working on the robot