Some attempts to fix PID algorithm other fixes