def getOmniWalkParam(my, dest):
# we use distance and bearing to get relX, relY which we already have
# for the ball. be nice not to recalculate it.
relX, relY, relH = 0, 0, 0
if hasattr(dest, "relX") and \
hasattr(dest, "relY"):
relX = dest.relX
relY = dest.relY
else:
bearingDeg = my.getRelativeBearing(dest)
distToDest = my.distTo(dest)
relX = MyMath.getRelativeX(distToDest, bearingDeg)
relY = MyMath.getRelativeY(distToDest, bearingDeg)
if hasattr(dest, "relH"):
relH = dest.relH
elif hasattr(dest, "bearing"):
relH = dest.bearing
else:
relH = MyMath.sub180Angle(dest.h - my.h)
# calculate forward speed
forwardGain = constants.APPROACH_X_WITH_GAIN_DIST
sX = relX * forwardGain
if fabs(sX) < constants.OMNI_MIN_X_MAGNITUDE:
sX = 0
else:
sX = MyMath.clip(sX,
constants.OMNI_REV_MAX_SPEED,
constants.OMNI_FWD_MAX_SPEED)
# calculate sideways speed
strafeGain = constants.APPROACH_Y_WITH_GAIN_DIST
sY = relY * strafeGain
if fabs(sY) < constants.OMNI_MIN_Y_MAGNITUDE:
sY = 0
else:
sY = MyMath.clip(sY,
constants.OMNI_RIGHT_MAX_SPEED,
constants.OMNI_LEFT_MAX_SPEED,)
# calculate spin speed
if (fabs(relH) < 10.0):
sTheta = 0.0
else:
spinGain = constants.APPROACH_THETA_WITH_GAIN_DIST
sTheta = relH * spinGain
sTheta = MyMath.clip(sTheta,
constants.OMNI_MAX_RIGHT_SPIN_SPEED,
constants.OMNI_MAX_LEFT_SPIN_SPEED)
denom = sqrt(sX*sX + sY*sY + sTheta*sTheta) / constants.OMNI_GAIN
if denom != 0:
sX /= denom
sY /= denom
sTheta /= denom
return (sX, sY, sTheta)
def getOmniWalkParam(my, dest):
# we use distance and bearing to get relX, relY which we already have
# for the ball. be nice not to recalculate it.
relX, relY, relH = 0, 0, 0
if hasattr(dest, "relX") and \
hasattr(dest, "relY") and \
hasattr(dest, "relH"):
relX = dest.relX
relY = dest.relY
relH = dest.relH
else:
bearingDeg = my.getRelativeBearing(dest)
distToDest = my.distTo(dest)
relX = MyMath.getRelativeX(distToDest, bearingDeg)
relY = MyMath.getRelativeY(distToDest, bearingDeg)
relH = MyMath.sub180Angle(dest.h - my.h)
# calculate forward speed
forwardGain = 20. / constants.APPROACH_X_WITH_GAIN_DIST # 20cm/sec in x direction
sX = relX * forwardGain
if fabs(sX) < constants.OMNI_MIN_X_MAGNITUDE:
sX = 0
else:
sX = MyMath.clip(sX,
constants.OMNI_REV_MAX_SPEED,
constants.OMNI_FWD_MAX_SPEED)
# calculate sideways speed
strafeGain = 15. / constants.APPROACH_Y_WITH_GAIN_DIST # 15cm/sec in y direction
sY = relY * strafeGain
if fabs(sY) < constants.OMNI_MIN_Y_MAGNITUDE:
sY = 0
else:
sY = MyMath.clip(sY,
constants.OMNI_RIGHT_MAX_SPEED,
constants.OMNI_LEFT_MAX_SPEED,)
# calculate spin speed
if (fabs(relH) < 25.0):
sTheta = 0.0
else:
spinGain = 20. / constants.APPROACH_THETA_WITH_GAIN_DIST # 20degs/sec in theta
sTheta = relH * spinGain
sTheta = MyMath.clip(sTheta,
constants.OMNI_MAX_RIGHT_SPIN_SPEED,
constants.OMNI_MAX_LEFT_SPIN_SPEED)
# refine x and y speeds
if (fabs(relH) > 50):
sX = 0
sY = 0
elif (fabs(relH) > 35):
sY = 0
return (sX, sY, sTheta)
def getOmniWalkParam(my, dest):
# we use distance and bearing to get relX, relY which we already have
# for the ball. be nice not to recalculate it.
relX, relY = 0, 0
if hasattr(dest, "relX") and \
hasattr(dest, "relY") and \
hasattr(dest, "relH"):
relX = dest.relX
relY = dest.relY
relH = dest.relH
else:
bearingDeg = my.getRelativeBearing(dest)
distToDest = my.distTo(dest)
relX = MyMath.getRelativeX(distToDest, bearingDeg)
relY = MyMath.getRelativeY(distToDest, bearingDeg)
relH = MyMath.sub180Angle(dest.h - my.h)
# calculate forward speed
forwardGain = constants.OMNI_GOTO_X_GAIN * relX
sX = constants.OMNI_GOTO_FORWARD_SPEED * forwardGain
sX = MyMath.clip(sX,
constants.OMNI_MIN_X_SPEED,
constants.OMNI_MAX_X_SPEED)
if fabs(sX) < constants.OMNI_MIN_X_MAGNITUDE:
sX = 0
# calculate sideways speed
strafeGain = constants.OMNI_GOTO_Y_GAIN * relY
sY = constants.OMNI_GOTO_STRAFE_SPEED * strafeGain
sY = MyMath.clip(sY,
constants.OMNI_MIN_Y_SPEED,
constants.OMNI_MAX_Y_SPEED,)
if fabs(sY) < constants.OMNI_MIN_Y_MAGNITUDE:
sY = 0
# calculate spin speed
spinGain = constants.GOTO_SPIN_GAIN
hDiff = MyMath.sub180Angle(dest.h - my.h)
if (fabs(hDiff) < 2.0):
sTheta = 0.0
else:
sTheta = MyMath.sign(hDiff) * getRotScale(hDiff) * \
constants.OMNI_MAX_SPIN_SPEED * spinGain
sTheta = MyMath.clip(sTheta,
constants.OMNI_MIN_SPIN_SPEED,
constants.OMNI_MAX_SPIN_SPEED)
return (sX, sY, sTheta)
