testing, optimization to PID control constants