Imported old PID algo