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