def motion_update(p):
v, w = control
v_dt = v*dt
w_dt = w*dt
x, y, t = p
return (x + v_dt*cos(t),
y + v_dt*sin(t),
angles.r2r(t))
def set_heading(new_t):
new_t = angles.r2r(new_t)
clkwise = new_t - t
ct_clkwise = 2 * pi + t - new_t
return clkwise if abs(clkwise) < abs(ct_clkwise) else ct_clkwise
def set_heading(new_t):
new_t = angles.r2r(new_t)
clkwise = new_t - t
ct_clkwise = 2*pi + t - new_t
return clkwise if abs(clkwise) < abs(ct_clkwise) else ct_clkwise
