Fixed bug when regulator loop takes to long time