STABLE: Bugs fixed + print new position feature

All controlls and functionality has been returned to the arm. Only new
positions should be printed to the computer now. However, the gripper
seems to be having some issues and twitching at startup.