P controller working

Dependencies:   QEI mbed