def GAMMA(v, alpha, beta, alpha_controller, beta_controller, L=1, dt=1):
a = alpha_controller.input(alpha, dt=dt)
b = beta_controller .input(beta, dt=dt)
# Weighted change in gamma = desired dh
dh = a + b
# Turn gamma is measured from the front wheel, not the back wheel;
# Get front wheel turn gamma
s = abs(v) # make sure we have unsigned speed; direction is already handles
gamma = atan(dh*L/s)
# Bound between [-pi, pi]
return under_pi(gamma)
def _next_worldstate(self, old_worldstate=None, dt=1):
# If no old_worldstate input, take worldstate from parent object
if old_worldstate is None:
old_worldstate = self._my_worldstate
# Calculate change in world state
dx = dX( v=self._v, h=old_worldstate.h )
dy = dY( v=self._v, h=old_worldstate.h )
dh = dH( v=self._v, gamma=self._gamma, L=self._L )
# Update world state
new_worldstate = old_worldstate + WorldState(xyh=[dx, dy, dh])*dt
# Keep h in [-pi, pi]
new_worldstate.h = under_pi(new_worldstate.h)
return new_worldstate
def _GAMMA(self, v, dt=1):
'''
Use PID control to calculate turn gamma.
'''
gamma = GAMMA(v=v, alpha=self._BM.alpha, beta=self._BM.beta,
alpha_controller=self._alphaPID, beta_controller=self._betaPID,
L=self._L, dt=dt)
# Check if no turn is required
if gamma<3:
return 0
gamma *= self.get_drive_direction()
# Clip to max
bound = self._picar_turn_to_gamma( self.MAX_PICAR_TURN );
gamma = clip(gamma, -bound, bound)
# Bound between [-pi, pi]
return under_pi(gamma)
def dH(v,gamma,L):
return under_pi(v*tan(gamma)/L)
def dBETA(v,rho,alpha):
return under_pi(-v*cos(alpha)/rho)
def ALPHA(robot,goal):
'''
Angle from the robot's gamma to the vector from (robot location) to (goal location).
'''
difference = goal - robot
return under_pi(difference.theta())