def fourthAreaCubes(currentPosition, myDirection, order):
#Vai para a segunda área
# myDirection = turnTo(myDirection ,EAST)
# #MoveDirectionPosition(frente, 0.020)
# currentPosition += 1
if (order == 2):
destine = 55
direction = EAST
lastTurn = esquerda
if (order == 1):
destine = 56
direction = WEST
lastTurn = direita
currentPosition, myDirection = shift.goFromTo(currentPosition, destine,
myDirection)
#Se posiciona da melhor forma para enxergar os blocos
myDirection = shift.turnTo(myDirection, direction, True)
#Align()
move.andar_em_metros(frente, 2, 0.04)
move.TurnDirectionAng(lastTurn, 90)
myDirection = NORTH
align.Align()
move.MoveDirectionPosition(tras, 0.065)
matrix1 = vis.resolveVision(clientID, 1) #MODIFICAR MATRIZ
matrix1 = gbb.invertMatrix(matrix1)
return matrix1, currentPosition, myDirection
def firstCorrection(i, myDirection, currentPosition, blockLocalPickup):
#print(i, myDirection, currentPosition, blockLocalPickup)
if (i == 0):
if (currentPosition < 30):
if (currentPosition % 10 > 4):
if (blockLocalPickup % 10 >= 6):
print('east')
myDirection = shift.turnTo(myDirection, EAST, False)
currentPosition = 26
else:
print('west')
myDirection = shift.turnTo(myDirection, WEST, False)
currentPosition = 25
else:
if (blockLocalPickup % 10 > 2):
print('east')
myDirection = shift.turnTo(myDirection, EAST, False)
currentPosition = 23
else:
print('west')
myDirection = shift.turnTo(myDirection, WEST, False)
currentPosition = 22
else:
if (currentPosition % 10 > 4):
if (blockLocalPickup % 10 >= 6):
print('east')
myDirection = shift.turnTo(myDirection, EAST, False)
currentPosition = 56
else:
print('west')
myDirection = shift.turnTo(myDirection, WEST, False)
currentPosition = 55
else:
if (blockLocalPickup % 10 > 2):
print('east')
myDirection = shift.turnTo(myDirection, EAST, False)
currentPosition = 53
else:
print('west')
myDirection = shift.turnTo(myDirection, WEST, False)
currentPosition = 52
#andar_em_metros(frente, 5, 0.15)
align.Align()
print(currentPosition)
return myDirection, currentPosition
def firstAreaCubes(currentPosition, myDirection, order):
if (order == 1):
destine = 22
direction = EAST
lastTurn = direita
if (order == 2):
destine = 23
direction = WEST
lastTurn = esquerda
#Vai para a primeira área
currentPosition, myDirection = shift.goFromTo(currentPosition, destine,
myDirection)
#Se posiciona da melhor forma para enxergar os blocos
myDirection = shift.turnTo(myDirection, direction, True)
#Align() #TurnTo ja alinha
move.andar_em_metros(frente, 2, 0.04)
move.TurnDirectionAng(lastTurn, 90)
myDirection = SOUTH
align.Align()
move.MoveDirectionPosition(tras, 0.065)
matrix0 = vis.resolveVision(clientID, 0)
#time.sleep(3)
return matrix0, currentPosition, myDirection
def getBlocksInformation(currentPosition, myDirection):
print(currentPosition)
if (currentPosition < 40): #Ta na parte de cima
if (currentPosition % 10 <= 2):
matrix0, currentPosition, myDirection = firstAreaCubes(
currentPosition, myDirection, 1)
#Vai para a segunda área
myDirection = shift.turnTo(myDirection, EAST, True)
#MoveDirectionPosition(frente, 0.020)
currentPosition += 1
matrix1, currentPosition, myDirection = secondAreaCubes(
currentPosition, myDirection, 2)
elif (currentPosition % 10 <= 4):
matrix0, currentPosition, myDirection = firstAreaCubes(
currentPosition, myDirection, 2)
#Vai para a segunda área
myDirection = shift.turnTo(myDirection, EAST, True)
#MoveDirectionPosition(frente, 0.020)
matrix1, currentPosition, myDirection = secondAreaCubes(
currentPosition, myDirection, 2)
else:
matrix1, currentPosition, myDirection = secondAreaCubes(
currentPosition, myDirection, 1)
myDirection = shift.turnTo(myDirection, WEST, True)
currentPosition -= 1
matrix0, currentPosition, myDirection = firstAreaCubes(
currentPosition, myDirection, 2)
else: #Ta na parte de baixo
if (currentPosition % 10 <= 2):
matrix0, currentPosition, myDirection = thirdAreaCubes(
currentPosition, myDirection, 1)
#Vai para a segunda área
myDirection = shift.turnTo(myDirection, EAST, True)
#MoveDirectionPosition(frente, 0.020)
currentPosition += 1
matrix1, currentPosition, myDirection = fourthAreaCubes(
currentPosition, myDirection, 2)
elif (currentPosition % 10 <= 4):
matrix0, currentPosition, myDirection = thirdAreaCubes(
currentPosition, myDirection, 2)
#Vai para a segunda área
myDirection = shift.turnTo(myDirection, EAST, True)
#MoveDirectionPosition(frente, 0.020)
matrix1, currentPosition, myDirection = fourthAreaCubes(
currentPosition, myDirection, 2)
else:
matrix1, currentPosition, myDirection = fourthAreaCubes(
currentPosition, myDirection, 1)
myDirection = shift.turnTo(myDirection, WEST, True)
currentPosition -= 1
matrix0, currentPosition, myDirection = thirdAreaCubes(
currentPosition, myDirection, 2)
#time.sleep(3)
#myDirection = turnTo(myDirection ,WEST)
#MoveDirectionPosition(frente, 0.020)
#currentPosition += 1
# print(matrix0)
# print(matrix1)
matrix = np.concatenate((matrix0, matrix1), axis=0)
#order = gb.get_path(gb.createGraphBlocks(matrix)) #AQUI FUNCIONA COM O CODIGO SIMPLES!!!!!
order, matrixFinal = solvePath(matrix, currentPosition)
#print(order, matrixFinal)
return currentPosition, myDirection, order, matrixFinal