Created simple control function

function computes the deviation from the expected speed and resets the
output voltage to compensate.