def calculateRedBlockAngle(blocksArray):
import math
dist2NearestBlock, blockId, pioneer2BlockPos = env.getNearestConcretBlockDist(clientID, blocksArray, pioneerRobotHandle)
#ret, Coordinates = vrep.simxGetObjectPosition(clientID, pioneerRobotHandle, blockId, vrep.simx_opmode_oneshot)
x = pioneer2BlockPos[1][0]
y = pioneer2BlockPos[1][1]
angle = math.atan2(y,x)
print('angle to block: ', angle)
print('block id: ', blockId)
angle = convertAngle(angle)
if angle < 0:
angle = angle + math.pi
else:
angle = angle - math.pi
return angle
ret_rm, rightMotorHandle = vrep.simxGetObjectHandle(clientID, 'Pioneer_p3dx_rightMotor', vrep.simx_opmode_oneshot_wait)
ret_pr, pioneerRobotHandle = vrep.simxGetObjectHandle(clientID, 'Pioneer_p3dx', vrep.simx_opmode_oneshot_wait)
ret_sl, ultraSonicSensorLeft = vrep.simxGetObjectHandle(clientID, 'Pioneer_p3dx_ultrasonicSensor' + str(3),vrep.simx_opmode_oneshot_wait)
ret_sr, ultraSonicSensorRight = vrep.simxGetObjectHandle(clientID, 'Pioneer_p3dx_ultrasonicSensor' + str(5),vrep.simx_opmode_oneshot_wait)
blocksArray = env.getConcretBlockHandle(clientID)
env.initConcretBlockPosition(clientID, blocksArray) # initialize position of the blocks
while True: # main Control loop
#######################################################
# Perception Phase: Get information about environment #
#######################################################
# get distance to nearest block
dist2NearestBlock, blockId, pioneer2BlockPos = env.getNearestConcretBlockDist(clientID, blocksArray, pioneerRobotHandle)
print('dist2nearestBlock', blockId, dist2NearestBlock)
print('Pos2nearestBlock', pioneer2BlockPos)
# get distance to obstacle, return [distLeft, distRight]
dist2Obstacle_LR = [getObstacleDist(ultraSonicSensorLeft), getObstacleDist(ultraSonicSensorRight)]
print('dist2obstacle', dist2Obstacle_LR)
##############################################
# Reasoning: figure out which action to take #
##############################################
# get motor speed using IF-THEN rule
motorSpeed = dict(speedLeft=3, speedRight=4)
#motorSpeed = getMotorSpeed(dist2Obstacle_LR)
