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 goToShelfDeliver(block, currentPosition, myDirection, cube):
shelf1 = [1, 6, 11]
shelf2 = [2, 7, 12]
shelf3 = [3, 8, 13]
shelf4 = [4, 9, 14]
shelf5 = [5, 10, 15]
if (block in shelf1):
currentPosition, myDirection = goFromTo(currentPosition, 22, myDirection)
if (block in shelf2):
currentPosition, myDirection = goFromTo(currentPosition, 23, myDirection)
if (block in shelf3):
currentPosition, myDirection = goFromTo(currentPosition, 24, myDirection)
if (block in shelf4):
currentPosition, myDirection = goFromTo(currentPosition, 25, myDirection)
if (block in shelf5):
currentPosition, myDirection = goFromTo(currentPosition, 26, myDirection)
myDirection = shelfPosition(block, myDirection)
myDirection = turnTo(myDirection, NORTH, True)
if(block <= 5):
cubo.entregar_cubo_primeiro_andar(cube)
elif(block <= 10):
cubo.entregar_cubo_segundo_andar(cube)
elif(block <= 15):
cubo.entregar_cubo_terceiro_andar(cube)
Prateleiras[block] += 1
move.andar_em_metros(tras, 3, 0.065)
return currentPosition, myDirection
def alinhar_cubo_na_direita_e_pegar():
move.andar_em_metros(tras, 2, 0.01)
garra.fechar_garra_total()
garra.descer_elevador()
while True :
a = sense.getDistanceIR(glob.irRight)
b = sense.getDistanceIR(glob.irLeft)
#print(a,b)
if(b<0.03 or a < 0.03):
break
move.MoveForward(2)
move.Stop()
a = sense.getDistanceIR(glob.irRight)
b = sense.getDistanceIR(glob.irLeft)
if(b < a):
cube = sense.getCubeHandle(glob.irLeft)
dist = b
else:
cube = sense.getCubeHandle(glob.irRight)
dist = a
garra.abrir_garra()
dirt=0
# while True :
# a = sense.getDistanceIR(glob.irRight)
# b = sense.getDistanceIR(glob.irLeft)
# print(a,b)
# giro_livre(esquerda, 2)
# # if(a<1):
# # dirt=dirt+1
# # if(b<1 and dirt>0):
# # break
# if(a < 0.15):
# break
move.Stop()
#TurnDirectionAng(direita, 5)
# move.Stop()
# TurnLeft()
# time.sleep(0.08)
# move.Stop()
move.andar_em_metros(frente, 2, dist+0.01)
garra.fechar_garra_cubo(cube)
grab(cube)
print('vou subir')
garra.subir_elevador(SEGUNDO_ANDAR)
print('subi')
#align.AlignSpecial(2)
#time.sleep(2)
#move.MoveDirectionPosition(tras, dist)
return cube
def goToHalfSquare(moveX, moveY, currentPosition, myDirection):
if(moveY != 0 and notStockLocal(currentPosition, moveY, axisY)):
myDirection = correctDirection(myDirection, moveY, axisY, True)
if(moveY > 0): #robô andou para a direita
currentPosition += 1
else: #robô andou para a esquerda
currentPosition -= 1
elif(moveX != 0 and notStockLocal(currentPosition, moveX, axisX)):
myDirection = correctDirection(myDirection, moveX, axisX, True)
if(moveX > 0): #robô andou para baixo
currentPosition += 10
else: #robô andou para cima
currentPosition -= 10
move.andar_em_metros(frente, 5, 0.15)
return currentPosition, myDirection
def reposicionarRobo(position):
if position == direita:
# Gira direita
giro.Girar_X_graus(glob.clientID, glob.robotRightMotor,
glob.robotLeftMotor, glob.robo, -1, 5)
move.andar_em_metros(1, 1, 0.01) # Anda pra frente
# Gira esquerda
giro.Girar_X_graus(glob.clientID, glob.robotRightMotor,
glob.robotLeftMotor, glob.robo, 1, 5)
elif position == esquerda:
# Gira esquerda
giro.Girar_X_graus(glob.clientID, glob.robotRightMotor,
glob.robotLeftMotor, glob.robo, 1, 5)
move.andar_em_metros(1, 1, 0.01) # Anda pra frente
# Gira direita
giro.Girar_X_graus(glob.clientID, glob.robotRightMotor,
glob.robotLeftMotor, glob.robo, -1, 5)
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 goToSquareSide(myDirection, firstDirection, finalTurn, hiddenBlock):
#move.MoveDirectionPosition(tras, 0.01)
if(myDirection == -firstDirection):
#if(False):
move.andar_em_metros(tras, 5, 0.10)
align.AlignBack(2)
move.andar_em_metros(frente, 2, 0.16)
move.TurnDirectionAng(-finalTurn, 90)
else:
print('virando', firstDirection)
turnTo(myDirection, firstDirection, True)
align.Align()
move.MoveDirectionPosition(tras, 0.002)
print('virando', finalTurn)
move.TurnDirectionAng(finalTurn, 90)
align.AlignSpecial(2)
if not hiddenBlock:
move.andar_em_metros(tras, 5, 0.06)
def winOPEN():
move.inicio_virar_SUL()
initialPosition = firstSq.identifyFirstPos(clientID)
if (initialPosition[1] == -1):
move.MoveSquareForward()
move.andar_em_metros(tras, 5, 0.065)
iniY, iniX = firstSq.identifyFirstPos(clientID)
initialPosition = (iniY + 1) * 10 + (iniX + 1)
print(initialPosition)
initialDirection = SOUTH
currentPosition, myDirection, order, matrix = getBlocksInformation(
initialPosition, initialDirection)
#time.sleep(1000)
#order = [1, 2, 3]
pickLater = []
blockZero = []
#APENAS TESTE
#order = gb.get_path(gb.createGraphBlocks(matrix))
#currentPosition = initialPosition
#myDirection = initialDirection
#FIM DE TESTE
n = len(order)
for i in range(n):
#blockLocalPickup, blockLocalDelivery, blockColor, hiddenBlock, blockPosition, blockSquare = course(order[i] - 2, matrix) #AQUI FUNCIONA COM O CODIGO SIMPLES!!!!!
order, matrix = solvePath(matrix, currentPosition)
blockLocalPickup, blockLocalDelivery, blockColor, hiddenBlock, blockPosition, blockSquare = course(
order[0], matrix)
print(blockLocalPickup, blockLocalDelivery, blockColor, hiddenBlock,
blockPosition)
if (not hiddenBlock):
myDirection, currentPosition = firstCorrection(
i, myDirection, currentPosition, blockLocalPickup)
currentPosition, myDirection = shift.goFromTo(
currentPosition, blockLocalPickup, myDirection)
myDirection, cube, blockNumber = grabBlock(currentPosition,
blockPosition,
myDirection, blockColor,
blockSquare,
hiddenBlock)
print(currentPosition, myDirection, cube, blockNumber)
if (blockNumber == 0):
garra.abrir_garra()
garra.fechar_garra_total()
garra.abrir_garra()
garra.fechar_garra_total()
garra.abrir_garra()
garra.fechar_garra_total()
blockZero.append([blockSquare, blockPosition])
matrix = np.delete(matrix, order[0], axis=0)
elif (blockNumber == -1):
# Cubo não identificado
# Desiste do bloco atual, troca pelo segundo, recalcula, GO.
if (len(order) >= 2):
second = matrix[order[1]]
matrix[order[1]] = matrix[order[0]]
matrix[order[0]] = second
blockLocalPickup, blockLocalDelivery, blockColor, hiddenBlock, blockPosition, blockSquare = course(
order[0], matrix)
# Pega bloco
myDirection, currentPosition = firstCorrection(
i, myDirection, currentPosition, blockLocalPickup)
currentPosition, myDirection = shift.goFromTo(
currentPosition, blockLocalPickup, myDirection)
myDirection, cube, blockNumber = grabBlock(
currentPosition, blockPosition, myDirection, blockColor,
blockSquare, hiddenBlock)
if ((blockColor == 'K' or blockColor == 'W') and blockNumber > 0):
#identifica número
##### MODIFICAR QUANDO IDENTIFICAR
if (currentPosition > 50):
currentPosition, myDirection = shift.goFromTo(
currentPosition, 44, myDirection)
currentPosition, myDirection = shift.goToShelfDeliver(
blockNumber, currentPosition, myDirection, cube)
matrix = np.delete(matrix, order[0], axis=0)
elif (blockColor == 'R' or blockColor == 'G' or blockColor == 'B'
or blockColor == 'Y'):
currentPosition, myDirection = shift.goFromTo(
currentPosition, blockLocalDelivery, myDirection)
cubo.entregar_cubo_colorido(cube)
matrix = np.delete(matrix, order[0], axis=0)
else:
pickLater.append([
blockColor, blockLocalPickup, blockLocalDelivery, blockSquare,
blockPosition
])
matrix = np.delete(matrix, order[0], axis=0)
print(pickLater)
for i in range(len(pickLater)):
print(i, "A")
blockColor = pickLater[i][0]
blockLocalPickup = pickLater[i][1]
blockLocalDelivery = pickLater[i][2]
blockSquare = pickLater[i][3]
blockPosition = pickLater[i][4]
blockLocalPickup = courseLastBlocks(blockSquare, blockPosition,
blockZero, blockLocalPickup)
currentPosition, myDirection = shift.goFromTo(currentPosition,
blockLocalPickup,
myDirection)
myDirection, cube, blockNumber = grabBlock(
currentPosition, blockPosition, myDirection, blockColor,
blockSquare, True) #### modificar para casos islolados
if (blockNumber == 0):
garra.abrir_garra()
garra.fechar_garra_total()
garra.abrir_garra()
garra.fechar_garra_total()
garra.abrir_garra()
garra.fechar_garra_total()
blockZero.append([blockSquare, blockPosition])
elif (blockNumber > 0):
move.andar_em_metros(tras, 3, 0.05)
align.AlignBack(3)
if ((blockColor == 'K' or blockColor == 'W') and blockNumber > 0):
if (currentPosition > 50):
currentPosition, myDirection = shift.goFromTo(
currentPosition, 44, myDirection)
currentPosition, myDirection = shift.goToShelfDeliver(
blockNumber, currentPosition, myDirection, cube)
elif (blockColor == 'R' or blockColor == 'G' or blockColor == 'B'
or blockColor == 'Y'):
currentPosition, myDirection = shift.goFromTo(
currentPosition, blockLocalDelivery, myDirection)
cubo.entregar_cubo_colorido(cube)