|result_t||control (pilot_command_t &pcmd, float topspeed=3.0)|
|StopLine (Navigator *navptr, int _verbose)|
This is based on the unpublished "Control Tutorial" draft dated January 26, 2004 by Dr. Benjamin Kuypers, section 5: "The Stopping Controller". He recommends a constant deceleration instead of the simpler exponential decay. The dynamical system is:
x_dot = -k * sqrt(x)
Solving analytically with initial condition x(0) = D and v(0) = V yields these equations of motion:
x(t) = (sqrt(D) - V*t/(2*sqrt(D)))**2 (parabolic drop) v(t) = dx/dt = (V**2/2*D)*t + V (linear velocity) a(t) = dv/dt = V**2/(2*D) = A (constant deceleration)
Note that the initial velocity V is negative in these equations, because it represents motion from positive x to zero. The system stops in finite time T = -2*D/V, with x(T) = 0, and v(T) = 0.
For example, when D = 10m from stop line and V = -5m/s, the vehicle stops in 4 seconds at a constant 1.25m/s/s deceleration.
Set speed for steady deceleration to stop way-point
|pcmd||contains desired heading and speed, assuming it is not yet time to stop, updated on exit|