working version but stripped of most unnecessary code like print statements

Dependencies:   HIDScope MODSERIAL biquadFilter mbed FastPWM QEI