add correct pwm lvl into control algo
done #26