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)