def pick_direction(logger, hHeadAngle, obstruction, maxAngle, bCtrBias):
if bCtrBias and hHeadAngle > 0:
hMax = hOffset * math.cos(hHeadAngle)
hMin = (-2 * hOffset) + hMax
elif bCtrBias and hHeadAngle < 0:
hMin = -hOffset * math.cos(hHeadAngle)
hMax = (2 * hOffset) + hMin
else:
hMin = -hOffset
hMax = hOffset
# do we need to avoid an obstacle?
if not obstruction is None:
if obstruction == Directions.Left:
logger.log("there is obstruction on right ")
hMin = 0
elif obstruction == Directions.Right:
logger.log("there is obstruction on right ")
hMax = 0
elif obstruction == Directions.Centre:
logger.log("there is obstruction in centre ")
else:
logger.log("no obstruction")
logger.log("hMin = " + str(hMin) + ", hMax = " + str(hMax) + ", head angle = " + str(hHeadAngle) + ", head offset = " + str(hOffset))
hDeg = random.uniform(hMin, hMax)
return to_radians(hDeg)
def pick_direction(caller, memProxy, motionProxy, hOffset, bCtrBias):
head = motionProxy.getAngles("Head", True)
hHeadAngle = head[0]
if bCtrBias and hHeadAngle > 0:
hMax = hOffset * math.cos(hHeadAngle)
hMin = (-2 * hOffset) + hMax
elif bCtrBias and hHeadAngle < 0:
hMin = -hOffset * math.cos(hHeadAngle)
hMax = (2 * hOffset) + hMin
else:
hMin = -hOffset
hMax = hOffset
# do we need to avoid an obstacle?
obstruction = memProxy.getData(MEM_OBSTACLE_LOCATION)
if obstruction == "left":
caller.log("there is obstruction on right ")
hMin = 0
elif obstruction == "right":
caller.log("there is obstruction on right ")
hMax = 0
elif obstruction == "centre":
caller.log("there is obstruction in centre ")
else:
caller.log("no obstruction")
caller.log(
"hMin = "
+ str(hMin)
+ ", hMax = "
+ str(hMax)
+ ", head angle = "
+ str(hHeadAngle)
+ ", head offset = "
+ str(hOffset)
)
hDeg = random.uniform(hMin, hMax)
return to_radians(hDeg)
def handleStart(self, event, state):
direction = to_radians(self.rng.randint(0, 360))
return [Turn(direction), WalkForwardsIndefinitely()]
