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 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 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 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 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