Fixed compilation... (some warning remain) + fixed motor readback in driver