def __init__(self, motorA, motorB, velocidade, posx, posy, ref):
self.coordenadas = Coordenadas(posx, posy, ref)
self.modoDeJogo = 0 # mododeJogo igual a 0 para modo autonomo e 1 para modo manual
self.velocidade = velocidade
self.partida = Partida()
self.l = LargeMotor(motorA)
self.r = LargeMotor(motorB)
self.cl = ColorSensor()
self.colors = ('unknown', 'black', 'blue', 'green', 'yellow', 'red',
'white', 'brown')
self.enviar = 0
def dibujarRectangulo(fila, columna, image, color):
#Coordenada Inicial
x1, y1 = Coordenadas.coordenadaInicial(fila, columna)
#Coordenada Final
x2, y2 = Coordenadas.coordenadaFinal(x1, y1, fila, columna)
#Thickness -> Queremos rellenar el recangulo por eso le ponemos -1
thickness = -1
#Agregar rectangulo relleno a la imagen
image = cv2.rectangle(image, (x1, y1), (x2, y2), color, thickness)
return image
def dibujarRegla(fila, columna, image, numero):
#Coordenada Inicial
x1, y1 = Coordenadas.coordenadaInicial(fila, columna)
#Coordenada Final
x2, y2 = Coordenadas.coordenadaFinal(x1, y1, fila, columna)
font = cv2.FONT_HERSHEY_SIMPLEX
fontScale = 0.3
if (fila == 0):
color = (0, 255, 0) #Color Verde
else:
color = (255, 0, 0) #Color Azul
thickness = 1
x = x1
y = y1 - 1
image = cv2.putText(image, str(numero), (x, y), font, fontScale, color,
thickness, cv2.LINE_AA)
return image
def pathMendoza():
x = list()
y = list()
x, y = Coordenadas.lineaHorizontal(13, 52, x, y, 6)
x, y = Coordenadas.asignarValores(x, y, 7,
[13, 14, 29, 35, 36, 44, 45, 52])
x, y = Coordenadas.asignarValores(x, y, 8, [13, 14, 43, 52])
x, y = Coordenadas.lineaHorizontal(29, 36, x, y, 8)
x, y = Coordenadas.asignarValores(x, y, 9, [13, 15, 29, 36, 42, 52])
x, y = Coordenadas.asignarValores(x, y, 10, [13, 16, 29, 36, 41, 52])
x, y = Coordenadas.asignarValores(x, y, 11, [13, 17, 29, 36, 40, 52])
x, y = Coordenadas.asignarValores(x, y, 12,
[13, 18, 19, 20, 29, 36, 40, 52])
x, y = Coordenadas.asignarValores(x, y, 13, [13, 29, 36, 40, 52])
x, y = Coordenadas.lineaHorizontal(19, 25, x, y, 13)
x, y = Coordenadas.asignarValores(x, y, 14, [13, 29, 36, 40, 52])
x, y = Coordenadas.lineaHorizontal(21, 25, x, y, 14)
x, y = Coordenadas.lineaHorizontal(21, 25, x, y, 15)
x, y = Coordenadas.asignarValores(x, y, 15, [13, 24, 29, 36, 40, 52])
x, y = Coordenadas.lineaVertical(16, 48, x, y, 13)
x, y = Coordenadas.lineaVertical(16, 48, x, y, 52)
x, y = Coordenadas.lineaHorizontal(13, 52, x, y, 49)
x, y = Coordenadas.lineaHorizontal(13, 24, x, y, 26)
x, y = Coordenadas.lineaHorizontal(41, 51, x, y, 26)
x, y = Coordenadas.lineaHorizontal(23, 41, x, y, 17)
x, y = Coordenadas.lineaHorizontal(23, 41, x, y, 18)
x, y = Coordenadas.lineaHorizontal(24, 41, x, y, 38)
x, y = Coordenadas.asignarValores(x, y, 16, [24, 29, 36, 40])
x, y = Coordenadas.lineaVertical(19, 38, x, y, 23)
x, y = Coordenadas.lineaVertical(19, 38, x, y, 24)
x, y = Coordenadas.lineaVertical(19, 38, x, y, 41)
x, y = Coordenadas.lineaVertical(28, 38, x, y, 25)
x, y = Coordenadas.lineaVertical(28, 37, x, y, 26)
x, y = Coordenadas.lineaVertical(38, 48, x, y, 31)
x, y = Coordenadas.lineaVertical(38, 47, x, y, 34)
x, y = Coordenadas.lineaVertical(33, 38, x, y, 38)
x, y = Coordenadas.lineaVertical(33, 38, x, y, 39)
x, y = Coordenadas.lineaVertical(34, 38, x, y, 40)
x, y = Coordenadas.lineaVertical(34, 37, x, y, 42)
x, y = Coordenadas.lineaVertical(34, 38, x, y, 48)
x, y = Coordenadas.lineaVertical(32, 37, x, y, 32)
x, y = Coordenadas.lineaVertical(32, 37, x, y, 33)
x, y = Coordenadas.lineaHorizontal(13, 21, x, y, 42)
x, y = Coordenadas.asignarValores(x, y, 48,
[25, 32, 33, 36, 37, 45, 50, 51])
x, y = Coordenadas.asignarValores(x, y, 47,
[24, 32, 33, 38, 45, 49, 50, 51])
x, y = Coordenadas.asignarValores(x, y, 46, [23, 39, 46, 49])
x, y = Coordenadas.asignarValores(x, y, 18, [42, 43, 49, 50, 51])
x, y = Coordenadas.asignarValores(x, y, 45, [22, 40, 41, 46, 48])
x, y = Coordenadas.asignarValores(x, y, 44, [20, 21, 41, 42, 43, 46, 47])
x, y = Coordenadas.asignarValores(x, y, 43,
[17, 18, 19, 20, 21, 43, 44, 46])
x, y = Coordenadas.asignarValores(x, y, 42, [44, 45])
x, y = Coordenadas.asignarValores(x, y, 41, [18, 19, 22, 43, 45, 46])
x, y = Coordenadas.asignarValores(x, y, 40, [17, 20, 23, 42, 44, 45, 46])
x, y = Coordenadas.asignarValores(x, y, 39,
[17, 21, 23, 24, 41, 43, 44, 46, 47])
x, y = Coordenadas.asignarValores(x, y, 38, [16, 22, 43, 47])
x, y = Coordenadas.asignarValores(x, y, 37, [15, 31, 34])
x, y = Coordenadas.asignarValores(x, y, 36, [14, 34])
x, y = Coordenadas.lineaVertical(33, 36, x, y, 22)
x, y = Coordenadas.lineaVertical(34, 36, x, y, 49)
x, y = Coordenadas.lineaHorizontal(43, 47, x, y, 34)
x, y = Coordenadas.asignarValores(x, y, 33, [27])
x, y = Coordenadas.asignarValores(x, y, 32, [29, 30, 31, 34, 35])
x, y = Coordenadas.asignarValores(x, y, 31, [28, 29, 35, 36, 37])
x, y = Coordenadas.asignarValores(x, y, 30, [28, 36, 37, 39, 40])
x, y = Coordenadas.asignarValores(x, y, 29, [27, 28, 37, 38, 39, 40])
x, y = Coordenadas.asignarValores(x, y, 28,
[14, 15, 27, 28, 37, 38, 39, 40])
x, y = Coordenadas.asignarValores(x, y, 27, [14, 15, 38])
return x, y
import Coordenadas, Data, Aestrella, Funciones
import cv2
#Definir Grilla
matriz = Coordenadas.crearCuadricula(50,67)
#Definir camino
x,y = Data.pathMendoza()
rango = len(x)
for i in range(rango):
matriz = Coordenadas.camino(x[i],y[i],matriz)
#Definir condiciones
start = (26, 49)
end = (13, 39)
if ((start[0] in x) and (start[1] in y) and (end[0] in x) and (end[1]in y)):
path = Aestrella.aEstrella(matriz, start, end)
print(path)
else:
print("Los puntos ingresados no son recorrible/caminables")
#Dibujar path, punto inicial y final
imagen=cv2.imread("./Mapas/photo2.png")
rango = len(path)
for i in range(rango):
imagen = Funciones.dibujarRectangulo(path[i][0],path[i][1],imagen,(0,256,0))
imagen = Funciones.dibujarRectangulo(start[0],start[1],imagen,(256,0,0))
class Robo:
def __init__(self, motorA, motorB, velocidade, posx, posy, ref):
self.coordenadas = Coordenadas(posx, posy, ref)
self.modoDeJogo = 0 # mododeJogo igual a 0 para modo autonomo e 1 para modo manual
self.velocidade = velocidade
self.partida = Partida()
self.l = LargeMotor(motorA)
self.r = LargeMotor(motorB)
self.cl = ColorSensor()
self.colors = ('unknown', 'black', 'blue', 'green', 'yellow', 'red',
'white', 'brown')
self.enviar = 0
def setVol(self):
return self.velocidade
def setFrente(self):
self.cl.mode = 'COL-COLOR'
if self.colors[self.cl.value()] == "green" or self.colors[
self.cl.value()] == "yellow" or self.colors[
self.cl.value()] == "blue":
while self.colors[self.cl.value()] == "green" or self.colors[self.cl.value()] == "yellow" or \
self.colors[
self.cl.value()] == "blue":
self.r.run_forever(speed_sp=self.velocidade)
self.l.run_forever(speed_sp=self.velocidade)
else:
self.setParar()
if self.colors[self.cl.value()] == "unknown":
while self.colors[self.cl.value()] != "black":
self.r.run_forever(speed_sp=self.velocidade)
while self.colors[self.cl.value()] != "green":
while self.colors[self.cl.value()] == "black":
self.r.run_forever(speed_sp=self.velocidade / 2)
self.l.run_forever(speed_sp=self.velocidade)
while self.colors[self.cl.value()] == "white":
self.r.run_forever(speed_sp=self.velocidade)
self.l.run_forever(speed_sp=self.velocidade / 2)
if self.colors[self.cl.value()] == "yellow":
self.l.run_forever(speed_sp=self.velocidade)
self.r.run_forever(speed_sp=self.velocidade)
sleep(0.1)
break
if self.colors[self.cl.value()] == "blue":
self.l.run_forever(speed_sp=self.velocidade)
self.r.run_forever(speed_sp=self.velocidade)
sleep(0.1)
break
else:
self.l.run_forever(speed_sp=self.velocidade)
self.r.run_forever(speed_sp=self.velocidade)
sleep(0.1)
self.setParar()
def setEsquerda(self):
self.cl.mode = 'COL-COLOR'
while self.colors[self.cl.value()] == "green":
self.r.run_forever(speed_sp=self.velocidade)
self.l.run_forever(speed_sp=self.velocidade * 0)
else:
self.l.stop(stop_action="hold")
while self.colors[self.cl.value()] == "black":
self.r.run_forever(speed_sp=self.velocidade)
while self.colors[self.cl.value()] != "black":
self.r.run_forever(speed_sp=self.velocidade)
else:
self.r.run_forever(speed_sp=self.velocidade)
self.setFrente()
def setDireita(self):
self.cl.mode = 'COL-COLOR'
while self.colors[self.cl.value()] == "green":
self.l.run_forever(speed_sp=self.velocidade)
self.r.run_forever(speed_sp=self.velocidade / 2)
else:
self.r.stop(stop_action="hold")
while self.colors[self.cl.value()] == "black":
self.l.run_forever(speed_sp=self.velocidade)
while self.colors[self.cl.value()] != "black":
self.l.run_forever(speed_sp=self.velocidade)
else:
self.l.run_forever(speed_sp=self.velocidade)
self.setFrente()
def setRetornar(self):
self.cl.mode = 'COL-COLOR'
while self.colors[self.cl.value()] == "green":
self.l.run_forever(speed_sp=-self.velocidade)
while self.colors[self.cl.value()] == "black":
self.l.run_forever(speed_sp=-self.velocidade)
while self.colors[self.cl.value()] != "black":
self.l.run_forever(speed_sp=-self.velocidade)
self.setParar()
self.setFrente()
def setVelocidade(self, setV):
self.velocidade = setV
def setParar(self):
self.r.stop(stop_action="hold")
self.l.stop(stop_action="hold")
def autoEsquerda(self):
x, y = self.coordenadas.verificandoPos('esquerda')
lista = []
lista.append((x, y))
if ((lista in self.partida.localizacaoRobo) or x < 0 or x > (6)
or y < 0 or y > 6):
pass
else:
self.coordenadas.trocarPosicao(x, y)
self.setEsquerda()
print(self.coordenadas.enviarCoordenadas())
self.enviar = 1
def autoDireita(self):
x, y = self.coordenadas.verificandoPos('direita')
lista = []
lista.append((x, y))
if ((lista in self.partida.localizacaoRobo) or x < 0 or x > 6 or y < 0
or y > 6):
pass
else:
self.coordenadas.trocarPosicao(x, y)
self.setDireita()
print(self.coordenadas.enviarCoordenadas())
self.enviar = 1
def autoFrente(self):
x, y = self.coordenadas.verificandoPos('frente')
lista = []
lista.append((x, y))
if ((lista in self.partida.localizacaoRobo) or x < 0 or x > 6 or y < 0
or y > 6):
pass
else:
self.coordenadas.trocarPosicao(x, y)
self.setFrente()
print(self.coordenadas.enviarCoordenadas())
self.enviar = 1
def autoRetornar(self):
x, y = self.coordenadas.verificandoPos('retornar')
lista = []
lista.append((x, y))
if ((lista in self.partida.localizacaoRobo) or x < 0 or x > 6 or y < 0
or y > 6):
pass
else:
self.coordenadas.trocarPosicao(x, y)
self.setRetornar()
print(self.coordenadas.enviarCoordenadas())
self.enviar = 1
def auto(self, _lista):
x, y = _lista[0]
aux1 = abs(self.coordenadas.posx - x)
aux2 = abs(self.coordenadas.posy - y)
if (aux1 < aux2):
print(self.coordenadas.posx, self.coordenadas.posy)
if ((self.coordenadas.posx == x) and (self.coordenadas.posy == y)):
time.sleep(2)
self.enviar = 2
elif (self.coordenadas.posx > x):
if (self.coordenadas.ref == 'N'):
self.autoEsquerda()
elif (self.coordenadas.ref == 'S'):
self.autoDireita()
elif (self.coordenadas.ref == 'L'):
self.autoRetornar()
elif (self.coordenadas.ref == 'O'):
self.autoFrente()
elif (self.coordenadas.posx < x):
if (self.coordenadas.ref == 'N'):
self.autoDireita()
elif (self.coordenadas.ref == 'S'):
self.autoEsquerda()
elif (self.coordenadas.ref == 'L'):
self.autoFrente()
elif (self.coordenadas.ref == 'O'):
self.autoRetornar()
elif (self.coordenadas.posy > y):
if (self.coordenadas.ref == 'N'):
self.autoRetornar()
elif (self.coordenadas.ref == 'S'):
self.autoFrente()
elif (self.coordenadas.ref == 'L'):
self.autoDireita()
elif (self.coordenadas.ref == 'O'):
self.autoEsquerda()
elif (self.coordenadas.posy < y):
if (self.coordenadas.ref == 'N'):
self.autoFrente()
elif (self.coordenadas.ref == 'S'):
self.autoRetornar()
elif (self.coordenadas.ref == 'L'):
self.autoEsquerda()
elif (self.coordenadas.ref == 'O'):
self.autoDireita()
else:
print(self.coordenadas.posx, self.coordenadas.posy)
if ((self.coordenadas.posx == x) and (self.coordenadas.posy == y)):
self.enviar = 2
time.sleep(2)
elif (self.coordenadas.posy > y):
if (self.coordenadas.ref == 'N'):
self.autoRetornar()
elif (self.coordenadas.ref == 'S'):
self.autoFrente()
elif (self.coordenadas.ref == 'L'):
self.autoDireita()
elif (self.coordenadas.ref == 'O'):
self.autoEsquerda()
elif (self.coordenadas.posy < y):
if (self.coordenadas.ref == 'N'):
self.autoFrente()
elif (self.coordenadas.ref == 'S'):
self.autoRetornar()
elif (self.coordenadas.ref == 'L'):
self.autoEsquerda()
elif (self.coordenadas.ref == 'O'):
self.autoDireita()
elif (self.coordenadas.posx > x):
if (self.coordenadas.ref == 'N'):
self.autoEsquerda()
elif (self.coordenadas.ref == 'S'):
self.autoDireita()
elif (self.coordenadas.ref == 'L'):
self.autoRetornar()
elif (self.coordenadas.ref == 'O'):
self.autoFrente()
elif (self.coordenadas.posx < x):
if (self.coordenadas.ref == 'N'):
self.autoDireita()
elif (self.coordenadas.ref == 'S'):
self.autoEsquerda()
elif (self.coordenadas.ref == 'L'):
self.autoFrente()
elif (self.coordenadas.ref == 'O'):
self.autoRetornar()