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