read encoder and Dynamixel angle

Dependencies:   MX28 PID mbed