Fixed more more mathematical errors

In addition to fixing more math, I also added a gotoCoordinates()
function to add more functionality, I am yet to learn RobotC methods for
moving the robot.