Restructure PWM code