Near Final

Added the emergency stop back into the arm. Code works on test board.
Will see if it will actually work on the robot. Will most likely add two
motor modes a little later on