Encoder Function (Manipulator)

simplified version of last year's encoder code for arm.
TODO: review drive encoder functions