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 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 chegar_perto_prateleira():
a = sense.getDistanceIR(glob.irLeft)
b = sense.getDistanceIR(glob.irRight)
#print('chegando')
move.MoveForward(3)
while(a > 0.07 or b > 0.07):
#print(sense.getDistanceIR(glob.irLeft), sense.getDistanceIR(glob.irRight))
a = sense.getDistanceIR(glob.irLeft)
b = sense.getDistanceIR(glob.irRight)
move.Stop()
def AlignSpecial(v):
leftLine = True
rightLine = True
while (leftLine or rightLine):
if (sense.getColor(glob.color_sensor_Left) == PRETO):
leftLine = False
if (sense.getColor(glob.color_sensor_Right) == PRETO):
rightLine = False
move.MoveForward(v)
move.Stop()
def AlignBack(v):
while (sense.getColor(glob.color_sensor_Left) != PRETO
or sense.getColor(glob.color_sensor_Right) != PRETO):
move.MoveBack(v)
move.Stop()
def Align(): #em desenvolvimento
v = 2
direita_preto = False
esquerda_preto = False
sim.simxGetObjectPosition(glob.clientID, glob.color_sensor_Left, -1,
sim.simx_opmode_streaming)
sim.simxGetObjectPosition(glob.clientID, glob.color_sensor_Right, -1,
sim.simx_opmode_streaming)
while (True):
if (sense.getColor(glob.color_sensor_Left) == PRETO
or sense.getColor(glob.color_sensor_Right) == PRETO):
#print("To procurando a linha")
#print("Achei pela primeira vez")
break
move.MoveForward(v)
move.Stop()
[erro,
pri_pos_cor_dir] = sim.simxGetObjectPosition(glob.clientID,
glob.color_sensor_Right, -1,
sim.simx_opmode_buffer)
#print(erro, pri_pos_cor_dir)
[erro,
pri_pos_cor_esq] = sim.simxGetObjectPosition(glob.clientID,
glob.color_sensor_Left, -1,
sim.simx_opmode_buffer)
#print(erro, pri_pos_cor_esq)
if (sense.getColor(glob.color_sensor_Left) == PRETO):
esquerda_preto = True
while (True):
if (sense.getColor(glob.color_sensor_Right) == PRETO):
#print("Achei pela segunda vez direito")
break
move.MoveForward(v)
elif (sense.getColor(glob.color_sensor_Right) == PRETO):
direita_preto = True
while (True):
if (sense.getColor(glob.color_sensor_Left) == PRETO):
#print("Achei pela segunda vez esquerdo")
break
move.MoveForward(v)
move.Stop()
[erro,
seg_pos_cor_dir] = sim.simxGetObjectPosition(glob.clientID,
glob.color_sensor_Right, -1,
sim.simx_opmode_buffer)
#print(erro, seg_pos_cor_dir)
[erro,
seg_pos_cor_esq] = sim.simxGetObjectPosition(glob.clientID,
glob.color_sensor_Left, -1,
sim.simx_opmode_buffer)
#print(erro, seg_pos_cor_esq)
#A essa altura, o robô já andou, viu a linha com o primeiro sensor, andou mais e viu a linha com o segundo
linha_vertical = False
linha_horizontal = False
x_p_dif_trans = np.abs(pri_pos_cor_dir[0] - pri_pos_cor_esq[0])
x_s_dif_trans = np.abs(seg_pos_cor_dir[0] - seg_pos_cor_esq[0])
x_e_dif_longi = np.abs(pri_pos_cor_esq[0] - seg_pos_cor_esq[0])
x_d_dif_longi = np.abs(pri_pos_cor_dir[0] - seg_pos_cor_dir[0])
y_p_dif_trans = np.abs(pri_pos_cor_dir[1] - pri_pos_cor_esq[1])
y_s_dif_trans = np.abs(seg_pos_cor_dir[1] - seg_pos_cor_esq[1])
y_e_dif_longi = np.abs(pri_pos_cor_esq[1] - seg_pos_cor_esq[1])
y_d_dif_longi = np.abs(pri_pos_cor_dir[1] - seg_pos_cor_dir[1])
######################################################################
##### TABELA EXPLICATIVA #####
#Essa tabela vale para x e para y, mas separadamente
#S*n = "sensor" + lado (d=direita, e=esquerda) + vez que viu a linha (1a ou 2a)
#dif = diferenca
#trans = transversal (sempre compara direita e esquerda)
#longi = longitudinal (sempre compara o mesmo lado)
#cruz = cruzada (existe e eh diferente de zero, mas nao sera utilizada)
#p = primeira (1a = 1), s = segunda (2a = 2)
#
# | SE1 | SD1 | SE2 | SD2 |
#SE1 | 0 | p_dif_trans | e_dif_longi | -ed_dif_cruz- |
#SD1 | p_dif_trans | 0 | -de_dif_cruz- | d_dif_longi |
#SE2 | e_dif_longi | -de_dif_cruz- | 0 | s_dif_trans |
#SD2 | -ed_dif_cruz- | d_dif_longi | s_dif_trans | 0 |
if (x_s_dif_trans > y_s_dif_trans):
#print('desalinhado horizontal')
linha_horizontal = True
if (esquerda_preto == True):
if (y_e_dif_longi > 1):
print("Estou descentralizado para a esquerda")
#criar funcao que recentraliza
else:
while (True):
[erro, atual_pos_cor_esq
] = sim.simxGetObjectPosition(glob.clientID,
glob.color_sensor_Left, -1,
sim.simx_opmode_buffer)
ye_atual_dif_longi = np.abs(seg_pos_cor_esq[1] -
atual_pos_cor_esq[1])
if (int(ye_atual_dif_longi * 1000000) >= int(
y_s_dif_trans * 1000000)):
# print("Alinhando")
# print("Alinhei")
# print(int(ye_atual_dif_longi*100000))
# print(int(y_s_dif_trans*100000))
break
move.gira_livre_uma_roda(esquerda, 1, 0.3)
elif (direita_preto == True):
if (y_d_dif_longi > 1):
print("Estou descentralizado para a direita")
#criar funcao que recentraliza
else:
while (True):
[erro, atual_pos_cor_dir
] = sim.simxGetObjectPosition(glob.clientID,
glob.color_sensor_Right, -1,
sim.simx_opmode_buffer)
yd_atual_dif_longi = np.abs(seg_pos_cor_dir[1] -
atual_pos_cor_dir[1])
if (int(yd_atual_dif_longi * 1000000) >= int(
y_s_dif_trans * 1000000)):
# print("Alinhando")
# print("Alinhei")
# print(int(yd_atual_dif_longi*1000000))
# print(int(y_s_dif_trans*1000000))
break
move.gira_livre_uma_roda(direita, -1, 0.3)
elif (x_s_dif_trans < y_s_dif_trans):
# print('desalinhado vertical')
linha_vertical = True
if (esquerda_preto == True):
if (x_e_dif_longi > 1):
print("Estou descentralizado para a esquerda")
# criar funcao que recentraliza
else:
while (True):
[erro, atual_pos_cor_esq
] = sim.simxGetObjectPosition(glob.clientID,
glob.color_sensor_Left, -1,
sim.simx_opmode_buffer)
xe_atual_dif_longi = np.abs(seg_pos_cor_esq[0] -
atual_pos_cor_esq[0])
if (int(xe_atual_dif_longi * 1000000) >= int(
x_s_dif_trans * 1000000)):
# print("Alinhando")
# print("Alinhei")
# print(int(xe_atual_dif_longi*1000000))
# print(int(x_s_dif_trans*1000000))
break
move.gira_livre_uma_roda(esquerda, 1, 0.3)
elif (direita_preto == True):
if (x_d_dif_longi > 1):
print("Estou descentralizado para a direita")
# criar funcao que recentraliza
else:
while (True):
[erro, atual_pos_cor_dir
] = sim.simxGetObjectPosition(glob.clientID,
glob.color_sensor_Right, -1,
sim.simx_opmode_buffer)
xd_atual_dif_longi = np.abs(seg_pos_cor_dir[0] -
atual_pos_cor_dir[0])
if (int(xd_atual_dif_longi * 1000000) >= int(
x_s_dif_trans * 1000000)):
# print("Alinhando")
# print("Alinhei")
# print(xd_atual_dif_longi)
# print(x_s_dif_trans)
break
move.gira_livre_uma_roda(direita, -1, 0.3)
move.Stop()