added goTo function

added in a goto function and replaced returnToOrigin with goto(0,0,0)
fixed robot moving to far on returnToOrigin

for loops are inclusive so i subtracted one from them in goTo
wrapped the robot.swing funcitons

added wrappers for swing() swingUp() and swingDown() to check for
duribility