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
class Bloque(Enum):
Edificio = 0
Calle_Izquierda_Derecha = 1
......@@ -14,42 +15,33 @@ class Bloque(Enum):
Calle_Abajo_Izquierda_Arriba = 10
Calle_Arriba_Derecha_Abajo_Izquierda = 11
class Direccion(Enum):
Arriba = 0
Derecha = 1
Abajo = 2
Izquierda = 3
class MovGiro(Enum):
Izquierda = 0
Recto = 1
Derecha = 2
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.y = y
self.bloque = Bloque(int(codigo_bloque))
def __str__(self):
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 convertirMatrizCasillas(matriz):
......@@ -57,16 +49,16 @@ def encontrarCamino(stringMapa,posicionInicial, posicionFinal):
for i in range(len(matriz)):
fila = []
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)
return matCas
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 False
matrizCasillas = convertirMatrizCasillas(matriz=matriz)
matrizAdy = []
......@@ -78,177 +70,230 @@ def encontrarCamino(stringMapa,posicionInicial, posicionFinal):
return matrizAdy
matriz = matrizAdyacencia(matriz)
# print(f'filas {len(matriz)}')
# print(f'columnas {len(matriz[0])}')
# for fila in matriz:
# print(fila)
# print()
solOptima = []
direcOptima = []
sol = [posicionInicial]
direcciones = []
def caminoVueltraAtras(direccion,posicion):
def caminoVueltraAtras(direccion, posicion):
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):
#print(direccion.name, bloque)
if(direccion == Direccion.Arriba):
def moviAcept(direccion: Direccion, bloque: int):
# print(direccion.name, bloque)
if (direccion == Direccion.Arriba):
#arriba = [2,4,5,8,9,10,11]
arriba = [2,3,6,7,8,10,11]
if(bloque in arriba):
# arriba = [2,4,5,8,9,10,11]
arriba = [2, 3, 6, 7, 8, 10, 11]
if (bloque in arriba):
return True
return False
if(direccion == Direccion.Abajo):
#abajo = [2,3,6,7,8,10,11]
abajo = [2,4,5,8,9,10,11]
if(bloque in abajo):
if (direccion == Direccion.Abajo):
# abajo = [2,3,6,7,8,10,11]
abajo = [2, 4, 5, 8, 9, 10, 11]
if (bloque in abajo):
return True
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 == 10 or bloquePosterio.value == 11):
# return True
#return False
if(direccion == Direccion.Izquierda):
#izquierda = [1,3,4,7,8,9,11]
izquierda = [1,5,6,7,9,10,11]
if(bloque in izquierda):
# return False
if (direccion == Direccion.Izquierda):
# izquierda = [1,3,4,7,8,9,11]
izquierda = [1, 5, 6, 7, 9, 10, 11]
if (bloque in izquierda):
return True
return False
if(direccion == Direccion.Derecha):
#derecha = [1,6,7,9,10,11]
derecha = [1,3,4,7,8,9,11]
if(bloque in derecha):
if (direccion == Direccion.Derecha):
# derecha = [1,6,7,9,10,11]
derecha = [1, 3, 4, 7, 8, 9, 11]
if (bloque in derecha):
return True
return False
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 False
#pos = sol[-1]
if(pos == posicionInicial):
#print(f"Descartado Inicio {direccion} : {pos}")
# pos = sol[-1]
if (pos == posicionInicial):
# print(f"Descartado Inicio {direccion} : {pos}")
return False
if(pos in sol):
#print(f"Descartado Usada {direccion} : {pos}")
if (pos in sol):
# print(f"Descartado Usada {direccion} : {pos}")
return False
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}")
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}")
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]])):
#print(f"Descartado Movimiento no aceptado {direccion} : {sol[-1]}")
if (matriz[pos[0]][pos[1]] == 0):
# print(f"Descartado Edificio {direccion} : {pos}")
return False
#if(len(sol)+1 >= distOptima):
if(len(solOptima) > 0):
if(len(sol)+1 >= len(solOptima)):
#print(f"Descartado Muy Largo {direccion} : {pos}")
return False
if (not movimientoAceptado(direc, matriz[sol[-1][0]][sol[-1][1]], matriz[pos[0]][pos[1]])):
# print(f"Descartado Movimiento no aceptado {direccion} : {sol[-1]}")
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)
direcciones.append(direccion)
pos = sol[-1]
#bq = mAdy[pos[0]][pos[1]]
#mAdy[pos[0]][pos[1]] = 0
if(pos != posicionFinal):
caminoVueltraAtras(Direccion.Arriba,(pos[0]-1,pos[1]))
caminoVueltraAtras(Direccion.Abajo,(pos[0]+1,pos[1]))
caminoVueltraAtras(Direccion.Derecha,(pos[0],pos[1]+1))
caminoVueltraAtras(Direccion.Izquierda,(pos[0],pos[1]-1))
# bq = mAdy[pos[0]][pos[1]]
# mAdy[pos[0]][pos[1]] = 0
if (pos != posicionFinal):
caminoVueltraAtras(Direccion.Arriba, (pos[0] - 1, pos[1]))
caminoVueltraAtras(Direccion.Abajo, (pos[0] + 1, pos[1]))
caminoVueltraAtras(Direccion.Derecha, (pos[0], pos[1] + 1))
caminoVueltraAtras(Direccion.Izquierda, (pos[0], pos[1] - 1))
else:
if(len(solOptima) > 0):
if(len(sol) < len(solOptima)):
#distOptima = len(sol)
if (len(solOptima) > 0):
if (len(sol) < len(solOptima)):
# distOptima = len(sol)
solOptima.clear()
direcOptima.clear()
for i in range(len(direcciones)):
solOptima.append(sol[i])
direcOptima.append(direcciones[i])
direcOptima.append(direcciones[i])
solOptima.append(sol[-1])
else:
#distOptima = len(sol)
# distOptima = len(sol)
solOptima.clear()
direcOptima.clear()
for i in range(len(direcciones)):
solOptima.append(sol[i])
direcOptima.append(direcciones[i])
direcOptima.append(direcciones[i])
solOptima.append(sol[-1])
#mAdy[pos[0]][pos[1]] = bq
# mAdy[pos[0]][pos[1]] = bq
sol.pop()
direcciones.pop()
caminoVueltraAtras(Direccion.Arriba,(posicionInicial[0]-1,posicionInicial[1]))
caminoVueltraAtras(Direccion.Abajo,(posicionInicial[0]+1,posicionInicial[1]))
caminoVueltraAtras(Direccion.Derecha,(posicionInicial[0],posicionInicial[1]+1))
caminoVueltraAtras(Direccion.Abajo,(posicionInicial[0],posicionInicial[1]-1))
caminoVueltraAtras(Direccion.Arriba, (posicionInicial[0] - 1, posicionInicial[1]))
caminoVueltraAtras(Direccion.Abajo, (posicionInicial[0] + 1, posicionInicial[1]))
caminoVueltraAtras(Direccion.Derecha, (posicionInicial[0], posicionInicial[1] + 1))
caminoVueltraAtras(Direccion.Izquierda, (posicionInicial[0], posicionInicial[1] - 1))
# Devuelve el recogido realizado tanto en posiciones como en direcciones
#return solOptima,direcOptima
return direcOptima
#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"]]
mapa = "0202000105030705000200041109060110031000000200080101100110000106010701"
#mAdy = matrizAdyacencia(matriz)
#pos = mAdy[6][0]
#mAdy[6][0] = 2
sol = encontrarCamino(mapa,(5,2),(6,0))
#mAdy[6][0] = pos
for s in sol:
print(s)
\ No newline at end of file
# return solOptima,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)
def crearMov(orient: Direccion, dir: Direccion):
if (orient is dir):
return MovGiro.Recto
if (orient is Direccion.Derecha):
return MovGiro(dir.value)
if (orient is Direccion.Izquierda):
return MovGiro((dir.value + 2) % 4)
if (orient is Direccion.Arriba):
return MovGiro((dir.value + 1) % 4)
if (orient is Direccion.Abajo):
return MovGiro((dir.value - 1) % 4)
# print(f'Direcciones {len(direcOptima)}')
# print(f'Casillas {solOptima}')
pos = posicionInicial
listaCasillas = []
orientacion = direcOptima[0]
dirs = []
for direc in direcOptima:
dirs.append(crearMov(orientacion, direc))
if (direc is Direccion.Arriba):
listaCasillas.append(arriba(pos))
orientacion = Direccion.Arriba
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
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.sigueRecto()
robot.sigueRecto()
#robot.sigueLineaHastaNegro()
#robot.sigueLineaHastaNegro()
robot.giraDerecha()
robot.sigueRecto()
robot.giraIzquierda()
robot.calibraSensorColor()
#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
class MensageRobot:
class MensajeRobot:
def __init__(self) -> None:
......
robot.py
......@@ -4,6 +4,9 @@ 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
import mensajeRobot as msg
import camino
from camino import MovGiro
class Robot:
green_rgb = (36, 70, 38)
......@@ -16,6 +19,11 @@ class Robot:
velocidadNegro = 50
negroFlag = False
radiusCurve = 20
interfono = None
mapa = None
casillaActual = (6,0)
pedido = None
casillaSig = None
def __init__(self):
self.ev3 = EV3Brick()
......@@ -25,7 +33,9 @@ class Robot:
self.colorSensor = ColorSensor(Port.S4)
self.robot = DriveBase(left_motor, right_motor, wheel_diameter=55.5, axle_track=124)
self.robot.settings(straight_speed=self.velocidad)
self.colaMovimientos = []
self.interfono = msg.MensajeRobot()
self.mapa = self.interfono.getMapa()
#Metodos gestion negros
def queNegroEs(self,negro):
......@@ -112,10 +122,15 @@ class Robot:
def giraDerecha(self):
self.robot.straight(60)
self.giro(90)
self.sigueRecto()
def giraIzquierda(self):
self.robot.straight(60)
self.giro(-90)
self.sigueRecto()
def mediaVuelta(self):
self.giro(180)
def sigueRecto(self, color=green_rgb):
# Valores iniciales
......@@ -141,6 +156,7 @@ class Robot:
self.negroFlag = True
self.w = 0
self.velocidad = self.velocidadNegro
else:
negroAnteriorFlag= self.negroFlag
self.negroFlag = False
......@@ -152,26 +168,57 @@ class Robot:
if not negroAnteriorFlag and self.negroFlag:
contNegroAnterior = self.contadorNegros
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)
print(self.contadorNegros,contNegroAnterior)
#Deficnición y realización de la ruta
# Recogemos la lista de movimientos del algoritmo
#def recogeListaMovimientosPedido(self, lMovimientos):
# for movimiento in lMovimientos:
# self.colaMovimientos.put(movimiento)
#def recogePaquete(self):
def rutaPedido(self):
while self.colaMovimientos.not_empty():
movimientoAct = self.colaMovimientos.get()
#Aqui faltan los if correspondientes a cada tipo de movimiento
#def soltarPaquete(self):
#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
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