def followExplorePath(clientID, sensorHandles, explorePaths, orientations):
print("Current state: follow next explore path state")
print("Start following explore path")
if not explorePaths.empty():
nextPath = explorePaths.get()
while not nextPath.empty():
targetPos = nextPath.get()
nextState = -1
isNotGoal = True
while (isNotGoal):
isNotGoal, hitRay = headTowardsModel(clientID, targetPos,
sensorHandles)
# if headTowardsModel returned False it means it successfully got to the goal point
if (not isNotGoal):
print("REACHED POINT")
# youBot encountered an little bot -> headTowardsModel returned True -> wait a few seconds and try again to reach the next position
else:
print("encountered dick head -> i will wait")
# wait 5 seconds
time.sleep(5)
# orient to blob
move.rotateUntilOrientation(clientID, orientations.get())
nextState = 2 # 2 = detect blob state
print("End following explore path")
else:
nextState = 0 # 0 = finish state
print("No paths left")
return nextState
def wallOrient(clientID, rangeSensorHandles, rayHit, isInOrientState):
print("WallOrient start")
# get sensor data from range sensors and the current bot orientation
rangeData = rangeSensor.getSensorData(clientID, rangeSensorHandles)
botOrient = move.getOrientation(clientID)
if (not isInOrientState):
print("Orient to nearest wall")
# to know how to align to the nearest wall on the side we
# use 2 rays to calculate the orientation of the wall -> x1, y2, and x2, y2
# start calc index of x2, y2 in rangeData
indexOfSndOrientPoint = rayHit + 10
for i in range(rayHit + 10, rayHit + 50):
if ((rangeData[i][3] - rangeData[rayHit][3]) < 0.5):
indexOfSndOrientPoint = i
break
# end calc index
x1 = rangeData[rayHit][0]
y1 = rangeData[rayHit][1]
x2 = rangeData[indexOfSndOrientPoint][0]
y2 = rangeData[indexOfSndOrientPoint][1]
# calculate the 2 possiblies of the 2 rays used so we can take the shorter ray, which
# will be our wanted target orientation
a1 = calcTargetOrient(clientID, x2, y2, x1, y1)
a2 = calcTargetOrient(clientID, x1, y1, x2, y2)
if abs(move.substractOrientation(botOrient, a1)) < abs(
move.substractOrientation(botOrient, a2)):
move.rotateUntilOrientation(clientID, a1)
isRight = True
else:
move.rotateUntilOrientation(clientID, a2)
isRight = False
else:
print("Turn because of a corner")
# Determine where the nearest wall on the left and right side is, to know in which direction to turn
if (rangeData[LEFT_RAY_NINETY][3] < 2):
move.rotate(90.0, clientID, True)
isRight = True
elif (rangeData[RIGHT_RAY_NINETY][3] < 2):
move.rotate(90.0, clientID, False)
isRight = False
print("WallOrient end")
return isRight
def followBasketPath(clientID, sensorHandles, basketPaths, blockColors):
print("Current state: following next basket path")
print("Start following basket path")
if not basketPaths.empty():
nextPath = basketPaths.get()
while not nextPath.empty():
targetPos = nextPath.get()
nextState = -1
isNotGoal = True
while (isNotGoal):
isNotGoal, hitRay = headTowardsModel(clientID, targetPos,
sensorHandles)
# if headTowardsModel returned False it means it successfully got to the goal point
if (not isNotGoal):
print("REACHED POINT")
# youBot encountered an little bot -> headTowardsModel returned True -> wait a few seconds and try again to reach the next position
else:
print("encountered dick head -> i will wait")
# wait 5 seconds
time.sleep(5)
# align to basket to drop the block
color = blockColors.get()
# if color = 0 -> red block -> align to red basket
# if color = 1 -> blue block -> align to blue basket
x, y, ori = c.basketCoordinate[color]
headTowardsModel(clientID, (x, y), sensorHandles)
move.rotateUntilOrientation(clientID, ori)
move.sideway(c.sidewardDistToBasket, clientID, False)
nextState = 8 # 8 = drop blob state
print("End following basket path")
else:
print("Error -> no basket paths left")
nextState = -1 # -1 = fail state, which shouldn't happen
return nextState
def headTowardsModel(clientID, modelName, rangeSensorHandles):
print("HeadTowardsModel: start")
# get the needed position information's of youBot and modelName (target)
res, objHandle = vrep.simxGetObjectHandle(clientID, modelName,
vrep.simx_opmode_oneshot_wait)
targetPosition = vrep.simxGetObjectPosition(clientID, objHandle, -1,
vrep.simx_opmode_oneshot_wait)
xTarget = targetPosition[1][0]
yTarget = targetPosition[1][1]
pos, ori = move.getPos(clientID)
# print where the youBot is and where the target
print("youBot: x = {}, y = {}".format(pos[0], pos[1]))
print("{}: x = {}, y = {}".format(modelName, xTarget, yTarget))
# calculate the target orientation based on the current youBot position
targetOrientation = calcTargetOrient(clientID, pos[0], pos[1], xTarget,
yTarget)
# rotate towards modelName until target orientation is reached
move.rotateUntilOrientation(clientID, targetOrientation)
# calculate the distance between youBot and target
dist = calcDistanceToTarget(pos[0], pos[1], xTarget, yTarget)
#dirve for dist meters or stop when encountering an obstacle
case, hit = move.forwardUntilObstacleAnywhere(dist, clientID,
rangeSensorHandles)
pos, ori = move.getPos(clientID) # get new position of bot for the output
print("Current position of bot after headTowardsModel; x = {} , y = {}".
format(pos[0], pos[1]))
print("HeadTowardsModel: end, Finished? {}".format(not case))
return case, hit