Skip to content

Vicente Castellano Gómez / robotAmbientales

Go to a project
Commit 69f5582d by Vicente Castellano Gómez
Options

Integracion completa a falta de prueba de la ruta con los movimientos

parent 2861e027
Inline Side-by-side
Showing with 249 additions and 181 deletions
camino.py
from enum import Enum from enum import Enum
class Bloque(Enum): class Bloque(Enum):
Edificio = 0 Edificio = 0
Calle_Izquierda_Derecha = 1 Calle_Izquierda_Derecha = 1
...@@ -14,42 +15,33 @@ class Bloque(Enum): ...@@ -14,42 +15,33 @@ class Bloque(Enum):
Calle_Abajo_Izquierda_Arriba = 10 Calle_Abajo_Izquierda_Arriba = 10
Calle_Arriba_Derecha_Abajo_Izquierda = 11 Calle_Arriba_Derecha_Abajo_Izquierda = 11
class Direccion(Enum): class Direccion(Enum):
Arriba = 0 Arriba = 0
Derecha = 1 Derecha = 1
Abajo = 2 Abajo = 2
Izquierda = 3 Izquierda = 3
class MovGiro(Enum):
Izquierda = 0
Recto = 1
Derecha = 2
class Casilla: class Casilla:
def __init__(self,x:int,y:int,codigo_bloque:str): def __init__(self, x: int, y: int, codigo_bloque: str):
self.x = x self.x = x
self.y = y self.y = y
self.bloque = Bloque(int(codigo_bloque)) self.bloque = Bloque(int(codigo_bloque))
def __str__(self): def __str__(self):
return f'x : {self.x} | y : {self.y} | {self.bloque}' return f'x : {self.x} | y : {self.y} | {self.bloque}'
def encontrarCamino(stringMapa,posicionInicial, posicionFinal):
def mapaMatrix(mapa):
segmentos = [mapa[i:i+2] for i in range(0, len(mapa), 2)]
matriz = []
for i in range(7):
fila = segmentos[i*5:(i+1)*5]
matriz.append(fila)
return matriz
matriz = mapaMatrix(stringMapa)
#distOptima : int = maxsize def encontrarCamino(matriz, posicionInicial, posicionFinal):
# distOptima : int = maxsize
def matrizAdyacencia(matriz): def matrizAdyacencia(matriz):
def convertirMatrizCasillas(matriz): def convertirMatrizCasillas(matriz):
...@@ -57,16 +49,16 @@ def encontrarCamino(stringMapa,posicionInicial, posicionFinal): ...@@ -57,16 +49,16 @@ def encontrarCamino(stringMapa,posicionInicial, posicionFinal):
for i in range(len(matriz)): for i in range(len(matriz)):
fila = [] fila = []
for j in range(len(matriz[0])): for j in range(len(matriz[0])):
fila.append(Casilla(i,j,matriz[i][j])) fila.append(Casilla(i, j, matriz[i][j]))
matCas.append(fila) matCas.append(fila)
return matCas return matCas
def dentroTablero(coord): def dentroTablero(coord):
if(coord[0] < len(matriz) and coord[0] >= 0 and coord[1] < len(matriz[0]) and coord[1] >= 0): if (coord[0] < len(matriz) and coord[0] >= 0 and coord[1] < len(matriz[0]) and coord[1] >= 0):
return True return True
return False return False
matrizCasillas = convertirMatrizCasillas(matriz=matriz) matrizCasillas = convertirMatrizCasillas(matriz=matriz)
matrizAdy = [] matrizAdy = []
...@@ -78,177 +70,230 @@ def encontrarCamino(stringMapa,posicionInicial, posicionFinal): ...@@ -78,177 +70,230 @@ def encontrarCamino(stringMapa,posicionInicial, posicionFinal):
return matrizAdy return matrizAdy
matriz = matrizAdyacencia(matriz) matriz = matrizAdyacencia(matriz)
# print(f'filas {len(matriz)}')
# print(f'columnas {len(matriz[0])}')
# for fila in matriz:
# print(fila)
# print()
solOptima = [] solOptima = []
direcOptima = [] direcOptima = []
sol = [posicionInicial] sol = [posicionInicial]
direcciones = [] direcciones = []
def caminoVueltraAtras(direccion,posicion): def caminoVueltraAtras(direccion, posicion):
global distOptima global distOptima
#print(sol)
def factible(direc,pos): # print(direccion,posicion)
# print(sol)
def factible(direc, pos):
def movimientoAceptado(direccion : Direccion, bloque : int, bloqueSiguiente : int): def movimientoAceptado(direccion: Direccion, bloque: int, bloqueSiguiente: int):
def moviAcept(direccion : Direccion, bloque : int): def moviAcept(direccion: Direccion, bloque: int):
#print(direccion.name, bloque) # print(direccion.name, bloque)
if(direccion == Direccion.Arriba): if (direccion == Direccion.Arriba):
#arriba = [2,4,5,8,9,10,11] # arriba = [2,4,5,8,9,10,11]
arriba = [2,3,6,7,8,10,11] arriba = [2, 3, 6, 7, 8, 10, 11]
if(bloque in arriba): if (bloque in arriba):
return True return True
return False return False
if(direccion == Direccion.Abajo): if (direccion == Direccion.Abajo):
#abajo = [2,3,6,7,8,10,11] # abajo = [2,3,6,7,8,10,11]
abajo = [2,4,5,8,9,10,11] abajo = [2, 4, 5, 8, 9, 10, 11]
if(bloque in abajo): if (bloque in abajo):
return True return True
return False return False
#if(bloquePosterio.value == 2 or bloquePosterio.value == 3 # if(bloquePosterio.value == 2 or bloquePosterio.value == 3
# or bloquePosterio.value == 6 or bloquePosterio.value == 7 # or bloquePosterio.value == 6 or bloquePosterio.value == 7
# or bloquePosterio.value == 10 or bloquePosterio.value == 11): # or bloquePosterio.value == 10 or bloquePosterio.value == 11):
# return True # return True
#return False # return False
if(direccion == Direccion.Izquierda): if (direccion == Direccion.Izquierda):
#izquierda = [1,3,4,7,8,9,11] # izquierda = [1,3,4,7,8,9,11]
izquierda = [1,5,6,7,9,10,11] izquierda = [1, 5, 6, 7, 9, 10, 11]
if(bloque in izquierda): if (bloque in izquierda):
return True return True
return False return False
if(direccion == Direccion.Derecha): if (direccion == Direccion.Derecha):
#derecha = [1,6,7,9,10,11] # derecha = [1,6,7,9,10,11]
derecha = [1,3,4,7,8,9,11] derecha = [1, 3, 4, 7, 8, 9, 11]
if(bloque in derecha): if (bloque in derecha):
return True return True
return False return False
direcSiguiente = Direccion((direccion.value + 2) % 4) direcSiguiente = Direccion((direccion.value + 2) % 4)
if(moviAcept(direccion,bloque) and moviAcept(direcSiguiente,bloqueSiguiente)): if (moviAcept(direccion, bloque) and moviAcept(direcSiguiente, bloqueSiguiente)):
return True return True
return False return False
#pos = sol[-1]
# pos = sol[-1]
if(pos == posicionInicial):
#print(f"Descartado Inicio {direccion} : {pos}") if (pos == posicionInicial):
# print(f"Descartado Inicio {direccion} : {pos}")
return False return False
if(pos in sol): if (pos in sol):
#print(f"Descartado Usada {direccion} : {pos}") # print(f"Descartado Usada {direccion} : {pos}")
return False return False
if(pos[0] < 0 or pos[0] >= len(matriz) or pos[1] < 0 or pos[1] >= len(matriz[0])): if (pos[0] < 0 or pos[0] >= len(matriz) or pos[1] < 0 or pos[1] >= len(matriz[0])):
#print(f"Descartado Fuera de Rango {direccion} : {pos}") # print(f"Descartado Fuera de Rango {direccion} : {pos}")
return False return False
if(matriz[pos[0]][pos[1]] == 0):
#print(f"Descartado Edificio {direccion} : {pos}")
return False
if(not movimientoAceptado(direc,matriz[sol[-1][0]][sol[-1][1]],matriz[pos[0]][pos[1]])): if (matriz[pos[0]][pos[1]] == 0):
#print(f"Descartado Movimiento no aceptado {direccion} : {sol[-1]}") # print(f"Descartado Edificio {direccion} : {pos}")
return False return False
#if(len(sol)+1 >= distOptima): if (not movimientoAceptado(direc, matriz[sol[-1][0]][sol[-1][1]], matriz[pos[0]][pos[1]])):
if(len(solOptima) > 0): # print(f"Descartado Movimiento no aceptado {direccion} : {sol[-1]}")
if(len(sol)+1 >= len(solOptima)): return False
#print(f"Descartado Muy Largo {direccion} : {pos}")
return False
return True # if(len(sol)+1 >= distOptima):
if (len(solOptima) > 0):
if (len(sol) + 1 >= len(solOptima)):
# print(f"Descartado Muy Largo {direccion} : {pos}")
return False
return True
if(factible(direccion,posicion)): if (factible(direccion, posicion)):
# print("Factible")
sol.append(posicion) sol.append(posicion)
direcciones.append(direccion) direcciones.append(direccion)
pos = sol[-1] pos = sol[-1]
#bq = mAdy[pos[0]][pos[1]] # bq = mAdy[pos[0]][pos[1]]
#mAdy[pos[0]][pos[1]] = 0 # mAdy[pos[0]][pos[1]] = 0
if(pos != posicionFinal): if (pos != posicionFinal):
caminoVueltraAtras(Direccion.Arriba,(pos[0]-1,pos[1])) caminoVueltraAtras(Direccion.Arriba, (pos[0] - 1, pos[1]))
caminoVueltraAtras(Direccion.Abajo,(pos[0]+1,pos[1])) caminoVueltraAtras(Direccion.Abajo, (pos[0] + 1, pos[1]))
caminoVueltraAtras(Direccion.Derecha,(pos[0],pos[1]+1)) caminoVueltraAtras(Direccion.Derecha, (pos[0], pos[1] + 1))
caminoVueltraAtras(Direccion.Izquierda,(pos[0],pos[1]-1)) caminoVueltraAtras(Direccion.Izquierda, (pos[0], pos[1] - 1))
else: else:
if(len(solOptima) > 0): if (len(solOptima) > 0):
if(len(sol) < len(solOptima)): if (len(sol) < len(solOptima)):
#distOptima = len(sol) # distOptima = len(sol)
solOptima.clear() solOptima.clear()
direcOptima.clear() direcOptima.clear()
for i in range(len(direcciones)): for i in range(len(direcciones)):
solOptima.append(sol[i]) solOptima.append(sol[i])
direcOptima.append(direcciones[i]) direcOptima.append(direcciones[i])
solOptima.append(sol[-1]) solOptima.append(sol[-1])
else: else:
#distOptima = len(sol) # distOptima = len(sol)
solOptima.clear() solOptima.clear()
direcOptima.clear() direcOptima.clear()
for i in range(len(direcciones)): for i in range(len(direcciones)):
solOptima.append(sol[i]) solOptima.append(sol[i])
direcOptima.append(direcciones[i]) direcOptima.append(direcciones[i])
solOptima.append(sol[-1]) solOptima.append(sol[-1])
# mAdy[pos[0]][pos[1]] = bq
#mAdy[pos[0]][pos[1]] = bq
sol.pop() sol.pop()
direcciones.pop() direcciones.pop()
caminoVueltraAtras(Direccion.Arriba,(posicionInicial[0]-1,posicionInicial[1])) caminoVueltraAtras(Direccion.Arriba, (posicionInicial[0] - 1, posicionInicial[1]))
caminoVueltraAtras(Direccion.Abajo,(posicionInicial[0]+1,posicionInicial[1])) caminoVueltraAtras(Direccion.Abajo, (posicionInicial[0] + 1, posicionInicial[1]))
caminoVueltraAtras(Direccion.Derecha,(posicionInicial[0],posicionInicial[1]+1)) caminoVueltraAtras(Direccion.Derecha, (posicionInicial[0], posicionInicial[1] + 1))
caminoVueltraAtras(Direccion.Abajo,(posicionInicial[0],posicionInicial[1]-1)) caminoVueltraAtras(Direccion.Izquierda, (posicionInicial[0], posicionInicial[1] - 1))
# Devuelve el recogido realizado tanto en posiciones como en direcciones # Devuelve el recogido realizado tanto en posiciones como en direcciones
#return solOptima,direcOptima # return solOptima,direcOptima
return direcOptima def arriba(posicion):
return (posicion[0] - 1, posicion[1])
def abajo(posicion):
return (posicion[0] + 1, posicion[1])
def izquierda(posicion):
return (posicion[0], posicion[1] - 1)
def derecha(posicion):
return (posicion[0], posicion[1] + 1)
#02 02 00 01 05 def crearMov(orient: Direccion, dir: Direccion):
#03 07 05 00 02 if (orient is dir):
#00 04 11 09 06 return MovGiro.Recto
#01 10 03 10 00 if (orient is Direccion.Derecha):
#00 02 00 08 01 return MovGiro(dir.value)
#01 10 01 10 00 if (orient is Direccion.Izquierda):
#01 06 01 07 01 return MovGiro((dir.value + 2) % 4)
if (orient is Direccion.Arriba):
matriz = [["02","02","00","01","05"], return MovGiro((dir.value + 1) % 4)
["03","07","05","00","02"], if (orient is Direccion.Abajo):
["00","04","11","09","06"], return MovGiro((dir.value - 1) % 4)
["01","10","03","10","00"],
["00","02","00","08","01"], # print(f'Direcciones {len(direcOptima)}')
["01","10","01","10","00"], # print(f'Casillas {solOptima}')
["01","06","01","07","01"]]
pos = posicionInicial
mapa = "0202000105030705000200041109060110031000000200080101100110000106010701" listaCasillas = []
#mAdy = matrizAdyacencia(matriz) orientacion = direcOptima[0]
dirs = []
#pos = mAdy[6][0]
#mAdy[6][0] = 2 for direc in direcOptima:
sol = encontrarCamino(mapa,(5,2),(6,0)) dirs.append(crearMov(orientacion, direc))
#mAdy[6][0] = pos if (direc is Direccion.Arriba):
listaCasillas.append(arriba(pos))
for s in sol: orientacion = Direccion.Arriba
print(s)
\ No newline at end of file if (direc is Direccion.Abajo):
listaCasillas.append(abajo(pos))
orientacion = Direccion.Abajo
if (direc is Direccion.Izquierda):
listaCasillas.append(izquierda(pos))
orientacion = Direccion.Izquierda
if (direc is Direccion.Derecha):
listaCasillas.append(derecha(pos))
orientacion = Direccion.Derecha
# for d in dirs:
# print(d)
# print()
solOptima.pop(0)
# for d in solOptima:
# print(d)
# print()
return dirs, solOptima
# 02 02 00 01 05
# 03 07 05 00 02
# 00 04 11 09 06
# 01 10 03 10 00
# 00 02 00 08 01
# 01 10 01 10 00
# 01 06 01 07 01
matriz = [["02", "02", "00", "01", "05"],
["03", "07", "05", "00", "02"],
["00", "04", "11", "09", "06"],
["01", "10", "03", "10", "00"],
["00", "02", "00", "08", "01"],
["01", "10", "01", "10", "00"],
["01", "06", "01", "07", "01"]]
# mAdy = matrizAdyacencia(matriz)
# pos = mAdy[6][0]
# mAdy[6][0] = 2
direc, cas = encontrarCamino(matriz, (0, 0), (0, 3))
# mAdy[6][0] = pos
print(len(direc), len(cas))
for d, c in zip(direc, cas):
print(d, c)
\ No newline at end of file
main.py
#!/usr/bin/env pybricks-micropython #!/usr/bin/env pybricks-micropython
from robot import Robot from robot import Robot
from pybricks.hubs import EV3Brick
from pybricks.ev3devices import (Motor, TouchSensor, ColorSensor,InfraredSensor, UltrasonicSensor, GyroSensor)
from pybricks.parameters import Port, Stop, Direction, Button, Color
from pybricks.tools import wait, StopWatch, DataLog
from pybricks.robotics import DriveBase
from pybricks.media.ev3dev import SoundFile, ImageFile
robot = Robot() robot = Robot()
robot.sigueRecto() robot.calibraSensorColor()
robot.sigueRecto()
#robot.sigueLineaHastaNegro()
#robot.sigueLineaHastaNegro()
robot.giraDerecha()
robot.sigueRecto()
robot.giraIzquierda()
#robot.calibraSensorColor() robot.realizaPedido()
main_robot.py deleted 100644 → 0
import mensajeRobot as msg
msgRobot = msg.MensageRobot()
mapa = msgRobot.getMapa()
print(f"Mapa recibido {mapa}")
posicion= (0,6)
msgRobot.sendPosicion(posicion)
pedido = msgRobot.getPedido()
print(f"Pedido recibido {pedido}")
\ No newline at end of file
mensajeRobot.py
...@@ -6,7 +6,7 @@ import json ...@@ -6,7 +6,7 @@ import json
class MensageRobot: class MensajeRobot:
def __init__(self) -> None: def __init__(self) -> None:
......
robot.py
...@@ -4,6 +4,9 @@ from pybricks.parameters import Port, Stop, Direction, Button, Color ...@@ -4,6 +4,9 @@ from pybricks.parameters import Port, Stop, Direction, Button, Color
from pybricks.tools import wait, StopWatch, DataLog from pybricks.tools import wait, StopWatch, DataLog
from pybricks.robotics import DriveBase from pybricks.robotics import DriveBase
from pybricks.media.ev3dev import SoundFile, ImageFile from pybricks.media.ev3dev import SoundFile, ImageFile
import mensajeRobot as msg
import camino
from camino import MovGiro
class Robot: class Robot:
green_rgb = (36, 70, 38) green_rgb = (36, 70, 38)
...@@ -16,6 +19,11 @@ class Robot: ...@@ -16,6 +19,11 @@ class Robot:
velocidadNegro = 50 velocidadNegro = 50
negroFlag = False negroFlag = False
radiusCurve = 20 radiusCurve = 20
interfono = None
mapa = None
casillaActual = (6,0)
pedido = None
casillaSig = None
def __init__(self): def __init__(self):
self.ev3 = EV3Brick() self.ev3 = EV3Brick()
...@@ -25,7 +33,9 @@ class Robot: ...@@ -25,7 +33,9 @@ class Robot:
self.colorSensor = ColorSensor(Port.S4) self.colorSensor = ColorSensor(Port.S4)
self.robot = DriveBase(left_motor, right_motor, wheel_diameter=55.5, axle_track=124) self.robot = DriveBase(left_motor, right_motor, wheel_diameter=55.5, axle_track=124)
self.robot.settings(straight_speed=self.velocidad) self.robot.settings(straight_speed=self.velocidad)
self.colaMovimientos = [] self.interfono = msg.MensajeRobot()
self.mapa = self.interfono.getMapa()
#Metodos gestion negros #Metodos gestion negros
def queNegroEs(self,negro): def queNegroEs(self,negro):
...@@ -112,10 +122,15 @@ class Robot: ...@@ -112,10 +122,15 @@ class Robot:
def giraDerecha(self): def giraDerecha(self):
self.robot.straight(60) self.robot.straight(60)
self.giro(90) self.giro(90)
self.sigueRecto()
def giraIzquierda(self): def giraIzquierda(self):
self.robot.straight(60) self.robot.straight(60)
self.giro(-90) self.giro(-90)
self.sigueRecto()
def mediaVuelta(self):
self.giro(180)
def sigueRecto(self, color=green_rgb): def sigueRecto(self, color=green_rgb):
# Valores iniciales # Valores iniciales
...@@ -141,6 +156,7 @@ class Robot: ...@@ -141,6 +156,7 @@ class Robot:
self.negroFlag = True self.negroFlag = True
self.w = 0 self.w = 0
self.velocidad = self.velocidadNegro self.velocidad = self.velocidadNegro
else: else:
negroAnteriorFlag= self.negroFlag negroAnteriorFlag= self.negroFlag
self.negroFlag = False self.negroFlag = False
...@@ -152,26 +168,57 @@ class Robot: ...@@ -152,26 +168,57 @@ class Robot:
if not negroAnteriorFlag and self.negroFlag: if not negroAnteriorFlag and self.negroFlag:
contNegroAnterior = self.contadorNegros contNegroAnterior = self.contadorNegros
self.incrementaContNegros() self.incrementaContNegros()
# Actualizacion de casilla
if self.queNegroEs(self.contadorNegros) == 1:
self.actualizaCasilla()
#Pendiente comprobar si hilo va a su bola o se para
self.robot.drive(self.velocidad,self.w) self.robot.drive(self.velocidad,self.w)
print(self.contadorNegros,contNegroAnterior) print(self.contadorNegros,contNegroAnterior)
#Deficnición y realización de la ruta #def recogePaquete(self):
# Recogemos la lista de movimientos del algoritmo
#def recogeListaMovimientosPedido(self, lMovimientos):
# for movimiento in lMovimientos:
# self.colaMovimientos.put(movimiento)
def rutaPedido(self): #def soltarPaquete(self):
while self.colaMovimientos.not_empty():
movimientoAct = self.colaMovimientos.get()
#Aqui faltan los if correspondientes a cada tipo de movimiento
#Deficnición y realización de la ruta
# Recogemos la lista de movimientos del algoritmo
def recogePedido(self):
self.pedido = self.interfono.getPedido()
return True
def actualizaCasilla(self):
self.casillaActual = self.casillaSig
self.interfono.sendPosicion(self.casillaActual)
def recorreRuta(self,rutaDirecciones,rutaCoordenadas):
for dir,coor in zip(rutaDirecciones,rutaCoordenadas):
self.casillaSig = coor
if dir is MovGiro.Derecha:
self.giraDerecha()
elif dir is MovGiro.Izquierda:
self.giraIzquierda()
else:
self.sigueRecto()
def realizaPedido(self):
while self.recogePedido():
#posicion actual ---> A
rutaDirecciones,rutaCoordenadas = camino.encontrarCamino(self.mapa,self.casillaActual,self.pedido[0])
self.recorreRuta(rutaDirecciones,rutaCoordenadas)
#self.recogePaquete()
self.tono("cubo")
self.mediaVuelta()
#Ir desde punto A ---> B
rutaDirecciones,rutaCoordenadas = camino.encontrarCamino(self.mapa,self.pedido[0],self.pedido[1])
self.recorreRuta(rutaDirecciones, rutaCoordenadas)
#self.soltarPaquete()
self.tono("vic")
self.mediaVuelta()
# def actualizaPosicionActual(self,movimiento:Direccion):
#Extras totalmente innecesarios #Extras totalmente innecesarios
def tono(self,cancion): def tono(self,cancion):
......
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or sign in to comment