Control works better now, due to changing error scaler and not using integreal gain.