new encoder testing code and arm position control

Added new encoder test code as well as arm position control code using
current control.