def turnAlignOrNot(angle, alinhar):
if(alinhar):
move.TurnInSquare(angle)
else:
if(angle > 0):
move.TurnDirectionAng(esquerda, abs(angle))
if(angle < 0):
move.TurnDirectionAng(direita, abs(angle))
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 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