def TurnInSquare(angle): #gira no centro do quadrado e vai para ponta
print(angle)
align.Align()
MoveDirectionPosition(tras, 0.065)
if(angle > 0):
TurnDirectionAng(esquerda, abs(angle))
if(angle < 0):
TurnDirectionAng(direita, abs(angle))
#MoveDirectionPosition(frente, 0.025)
align.Align()
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 inicio_virar_NORTE(): # para a função SEM VISÃO; lembrar de adicionar no if(nao viu prataleira) virar 180.
d=0
while(True):
erro,b=sim.simxGetObjectOrientation(glob.clientID,glob.robo,-1,sim.simx_opmode_blocking)
gamma=(b[2]*180)/(np.pi)
if(gamma>=177 or gamma<=-177):
Stop()
break
if(d==0):
if(gamma>0 and gamma<179.99):
d = 1
else:
d =-1
v=4
sim.simxPauseCommunication(glob.clientID, True)
sim.simxSetJointTargetVelocity(glob.clientID,glob.robotRightMotor,d*v, sim.simx_opmode_oneshot)
sim.simxSetJointTargetVelocity(glob.clientID,glob.robotLeftMotor,(-1)*d*v, sim.simx_opmode_oneshot)
sim.simxPauseCommunication(glob.clientID, False)
align.Align()
return
def inicio_virar_SUL(): # para a função COM VISÃO
d=0
while(True):
erro,b=sim.simxGetObjectOrientation(glob.clientID,glob.robo,-1,sim.simx_opmode_blocking)
gamma=(b[2]*180)/(np.pi)
if(gamma>=-3 and gamma<=3):
Stop()
break
if(d==0):
if(gamma>0 and gamma<179.99):
d = 1
else:
d = -1
v=4
sim.simxPauseCommunication(glob.clientID, True)
sim.simxSetJointTargetVelocity(glob.clientID,glob.robotRightMotor,d*v, sim.simx_opmode_oneshot)
sim.simxSetJointTargetVelocity(glob.clientID,glob.robotLeftMotor,(-1)*d*v, sim.simx_opmode_oneshot)
sim.simxPauseCommunication(glob.clientID, False)
align.Align()
andar_em_metros(tras, 5, 0.065)
return
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 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 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
def MoveSquareForward():
andar_em_metros(frente, 6, 0.20)
align.Align()