Working PWM, but code is now very ugly