better path and tuned PID controller