working PID + Kinematics + Motorcontrol

Dependencies:   HIDScope MODSERIAL QEI biquadFilter mbed