control loop with pwm problems fixed and periodic timing added