def empurrar_cubo(): move.MoveDirectionPosition(tras, 0.12) garra.fechar_garra_total() while(sense.getDistanceIR(glob.irLeft) > 0.01 or sense.getDistanceIR(glob.irRight) > 0.01): move.MoveForward(2) move.Stop() move.MoveDirectionPosition(frente, 0.04)
def entregar_cubo_primeiro_andar(cube): garra.subir_elevador(PRIMEIRO_ANDAR) move.MoveDirectionPosition(frente, 0.1) chegar_perto_prateleira() leave(cube) garra.abrir_garra() #empurrar_cubo() align.AlignBack(3) garra.fechar_garra_total() garra.subir_elevador(SEGUNDO_ANDAR)
def entregar_cubo_colorido(cube):
align.Align()
garra.descer_elevador()
leave(cube)
garra.abrir_garra()
#empurrar_cubo()
garra.subir_elevador(SEGUNDO_ANDAR)
garra.fechar_garra_total()
move.MoveDirectionPosition(tras, 0.05)
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 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)