Cleaned up the code a bit

Dependencies:   mbed QEI HIDScope biquadFilter MODSERIAL FastPWM