First version of PWM ready, proper FSM was the bug killer