Working code with tuned PID values