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)