Todo: create PWM controller function