Fixed up all of the code. Made it look really nice. Also fixed constants in RobotMap