Skip to content

Vicente Castellano Gómez / robotAmbientales

Go to a project
Commit 6b9b7537 by Vicente Castellano Gómez
Options

- Funcando de una vezzzzzz

parent 69f5582d
Inline Side-by-side
Showing with 101 additions and 97 deletions
camino.py
from enum import Enum class Bloque:
class Bloque(Enum):
Edificio = 0 Edificio = 0
Calle_Izquierda_Derecha = 1 Calle_Izquierda_Derecha = 1
Calle_Arriba_Abajo = 2 Calle_Arriba_Abajo = 2
...@@ -15,19 +12,32 @@ class Bloque(Enum): ...@@ -15,19 +12,32 @@ 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
#value
def __init__(self,cod):
self.value = cod
class Direccion(Enum): class Direccion:
Arriba = 0 Arriba = 0
Derecha = 1 Derecha = 1
Abajo = 2 Abajo = 2
Izquierda = 3 Izquierda = 3
#value
def __init__(self,direccion):
self.value = direccion
class MovGiro(Enum):
class MovGiro:
Izquierda = 0 Izquierda = 0
Recto = 1 Recto = 1
Derecha = 2 Derecha = 2
#value
def __init__(self,MovGiro):
self.value = MovGiro
class Casilla: class Casilla:
...@@ -36,11 +46,19 @@ class Casilla: ...@@ -36,11 +46,19 @@ class Casilla:
self.y = y self.y = y
self.bloque = Bloque(int(codigo_bloque)) 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)
def encontrarCamino(matriz, posicionInicial, posicionFinal):
# distOptima : int = maxsize # distOptima : int = maxsize
def matrizAdyacencia(matriz): def matrizAdyacencia(matriz):
...@@ -71,11 +89,8 @@ def encontrarCamino(matriz, posicionInicial, posicionFinal): ...@@ -71,11 +89,8 @@ def encontrarCamino(matriz, posicionInicial, posicionFinal):
return matrizAdy return matrizAdy
matriz = matrizAdyacencia(matriz) matriz = matrizAdyacencia(matriz)
# print(f'filas {len(matriz)}')
# print(f'columnas {len(matriz[0])}') #return matriz
# for fila in matriz:
# print(fila)
# print()
solOptima = [] solOptima = []
direcOptima = [] direcOptima = []
...@@ -83,17 +98,16 @@ def encontrarCamino(matriz, posicionInicial, posicionFinal): ...@@ -83,17 +98,16 @@ def encontrarCamino(matriz, posicionInicial, posicionFinal):
direcciones = [] direcciones = []
def caminoVueltraAtras(direccion, posicion): def caminoVueltraAtras(direccion, posicion):
global distOptima #global distOptima
# print(direccion,posicion) # print(direccion,posicion)
# print(sol) # print(sol)
def factible(direc, pos): def factible(direc, pos):
def movimientoAceptado(direccion: Direccion, bloque: int, bloqueSiguiente: int): def movimientoAceptado(direccion, bloque: int, bloqueSiguiente: int):
def moviAcept(direccion: Direccion, bloque: int): def moviAcept(direccion, bloque: int):
# 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]
...@@ -125,37 +139,38 @@ def encontrarCamino(matriz, posicionInicial, posicionFinal): ...@@ -125,37 +139,38 @@ def encontrarCamino(matriz, posicionInicial, posicionFinal):
return True return True
return False return False
direcSiguiente = Direccion((direccion.value + 2) % 4) #direcSiguiente = Direccion((direccion + 2) % 4)
direcSiguiente = (direccion + 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]
#print(pos)
if (pos == posicionInicial): if (pos == posicionInicial):
# print(f"Descartado Inicio {direccion} : {pos}") #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): if (matriz[pos[0]][pos[1]] == 0):
# print(f"Descartado Edificio {direccion} : {pos}") #print(f"Descartado Edificio {direccion} : {pos}")
return False return False
if (not movimientoAceptado(direc, matriz[sol[-1][0]][sol[-1][1]], matriz[pos[0]][pos[1]])): 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]}") #print(f"Descartado Movimiento no aceptado {direccion} : {sol[-1]}")
return False return False
# if(len(sol)+1 >= distOptima): # if(len(sol)+1 >= distOptima):
if (len(solOptima) > 0): if (len(solOptima) > 0):
if (len(sol) + 1 >= len(solOptima)): if (len(sol) + 1 >= len(solOptima)):
# print(f"Descartado Muy Largo {direccion} : {pos}") #print(f"Descartado Muy Largo {direccion} : {pos}")
return False return False
return True return True
...@@ -205,7 +220,8 @@ def encontrarCamino(matriz, posicionInicial, posicionFinal): ...@@ -205,7 +220,8 @@ def encontrarCamino(matriz, posicionInicial, posicionFinal):
caminoVueltraAtras(Direccion.Izquierda, (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 #print(solOptima,direcOptima)
#return solOptima,direcOptima
def arriba(posicion): def arriba(posicion):
return (posicion[0] - 1, posicion[1]) return (posicion[0] - 1, posicion[1])
...@@ -219,17 +235,21 @@ def encontrarCamino(matriz, posicionInicial, posicionFinal): ...@@ -219,17 +235,21 @@ def encontrarCamino(matriz, posicionInicial, posicionFinal):
def derecha(posicion): def derecha(posicion):
return (posicion[0], posicion[1] + 1) return (posicion[0], posicion[1] + 1)
def crearMov(orient: Direccion, dir: Direccion): def crearMov(orient: Direccion, dir):
if (orient is dir): if (orient is dir):
return MovGiro.Recto return MovGiro.Recto
if (orient is Direccion.Derecha): if (orient is Direccion.Derecha):
return MovGiro(dir.value) #return MovGiro(dir)
return dir
if (orient is Direccion.Izquierda): if (orient is Direccion.Izquierda):
return MovGiro((dir.value + 2) % 4) #return MovGiro((dir + 2) % 4)
return (dir + 2) % 4
if (orient is Direccion.Arriba): if (orient is Direccion.Arriba):
return MovGiro((dir.value + 1) % 4) #return MovGiro((dir + 1) % 4)
return (dir + 1) % 4
if (orient is Direccion.Abajo): if (orient is Direccion.Abajo):
return MovGiro((dir.value - 1) % 4) #return MovGiro((dir - 1) % 4)
return (dir - 1) % 4
# print(f'Direcciones {len(direcOptima)}') # print(f'Direcciones {len(direcOptima)}')
# print(f'Casillas {solOptima}') # print(f'Casillas {solOptima}')
...@@ -258,11 +278,12 @@ def encontrarCamino(matriz, posicionInicial, posicionFinal): ...@@ -258,11 +278,12 @@ def encontrarCamino(matriz, posicionInicial, posicionFinal):
if (direc is Direccion.Derecha): if (direc is Direccion.Derecha):
listaCasillas.append(derecha(pos)) listaCasillas.append(derecha(pos))
orientacion = Direccion.Derecha orientacion = Direccion.Derecha
"""
# for d in dirs: for d in dirs:
# print(d) print(d)
# print() print()
solOptima.pop(0) solOptima.pop(0)
"""
# for d in solOptima: # for d in solOptima:
# print(d) # print(d)
...@@ -270,30 +291,8 @@ def encontrarCamino(matriz, posicionInicial, posicionFinal): ...@@ -270,30 +291,8 @@ def encontrarCamino(matriz, posicionInicial, posicionFinal):
return dirs, solOptima return dirs, solOptima
# 02 02 00 01 05 mapa = "0202000105030705000200041109060110031000000200080101100110000106010701"
# 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] d,c = encontrarCamino(mapa,(0,0),(6,4))
# 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
import camino
from camino import MovGiro as mov
robot = Robot() robot = Robot()
mapa = "0202000105030705000200041109060110031000000200080101100110000106010701"
#robot.calibraSensorColor()
mov,cas = camino.encontrarCamino(mapa,(0,0),(3,0))
robot.recorreRuta(mov,cas)
robot.recogePaquete()
robot.calibraSensorColor()
robot.realizaPedido()
media/sound/cubo.mp3 deleted 100644 → 0
No preview for this file type
media/sound/vic.mp3 deleted 100644 → 0
No preview for this file type
robot.py
...@@ -4,18 +4,18 @@ from pybricks.parameters import Port, Stop, Direction, Button, Color ...@@ -4,18 +4,18 @@ 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 mensajeRobot as msg
import camino import camino
from camino import MovGiro from camino import MovGiro as mov
class Robot: class Robot:
green_rgb = (36, 70, 38) green_rgb = (35, 55, 37)
black_rgb = (5, 5, 16) black_rgb = (6, 3, 18)
tolerancia = 10 tolerancia = 10
contadorNegros = 0 contadorNegros = 0
valorW = 30 valorW = 30
w = 0 w = 0
velocidad = 50 velocidad = 60
velocidadNegro = 50 velocidadNegro = 50
negroFlag = False negroFlag = False
radiusCurve = 20 radiusCurve = 20
...@@ -29,12 +29,13 @@ class Robot: ...@@ -29,12 +29,13 @@ class Robot:
self.ev3 = EV3Brick() self.ev3 = EV3Brick()
left_motor = Motor(Port.A) left_motor = Motor(Port.A)
right_motor = Motor(Port.D) right_motor = Motor(Port.D)
self.pala = Motor(Port.B)
self.giroSensor = GyroSensor(Port.S1) self.giroSensor = GyroSensor(Port.S1)
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.interfono = msg.MensajeRobot() #self.interfono = msg.MensajeRobot()
self.mapa = self.interfono.getMapa() #self.mapa = self.interfono.getMapa()
#Metodos gestion negros #Metodos gestion negros
...@@ -74,7 +75,7 @@ class Robot: ...@@ -74,7 +75,7 @@ class Robot:
def calibraSensorColorReal(self): def calibraSensorColorReal(self):
self.ev3.screen.print("Presiona el botón para calibrar el color verde") self.ev3.screen.print("Presiona el botón para calibrar el color verde")
while not Button().any(): while not Button.CENTER in self.ev3.buttons.pressed():
wait(10) wait(10)
# Medir color verde # Medir color verde
...@@ -82,7 +83,7 @@ class Robot: ...@@ -82,7 +83,7 @@ class Robot:
self.ev3.screen.print("Color verde calibrado:", green_rgb_medido) self.ev3.screen.print("Color verde calibrado:", green_rgb_medido)
self.ev3.screen.print("Presiona el botón para calibrar el color negro") self.ev3.screen.print("Presiona el botón para calibrar el color negro")
while not Button().any(): while not self.ev3.buttons.pressed():
wait(10) wait(10)
# Medir color negro # Medir color negro
...@@ -120,17 +121,19 @@ class Robot: ...@@ -120,17 +121,19 @@ class Robot:
self.robot.turn(-1) self.robot.turn(-1)
def giraDerecha(self): def giraDerecha(self):
self.robot.straight(60) self.robot.straight(62) #60
self.giro(90) self.giro(90)
self.sigueRecto() self.sigueRecto()
def giraIzquierda(self): def giraIzquierda(self):
self.robot.straight(60) self.robot.straight(62) #60
self.giro(-90) self.giro(-90)
self.sigueRecto() self.sigueRecto()
def mediaVuelta(self): def mediaVuelta(self):
self.giro(180) self.robot.straight(60)
self.giro(90)
self.giro(90)
def sigueRecto(self, color=green_rgb): def sigueRecto(self, color=green_rgb):
# Valores iniciales # Valores iniciales
...@@ -169,17 +172,23 @@ class Robot: ...@@ -169,17 +172,23 @@ class Robot:
contNegroAnterior = self.contadorNegros contNegroAnterior = self.contadorNegros
self.incrementaContNegros() self.incrementaContNegros()
# Actualizacion de casilla # Actualizacion de casilla
if self.queNegroEs(self.contadorNegros) == 1: #if self.queNegroEs(self.contadorNegros) == 1:
self.actualizaCasilla() #self.actualizaCasilla()
#Pendiente comprobar si hilo va a su bola o se para #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)
def recogePaquete(self):
self.robot.stop()
self.pala.run_target(70,-100)
#def recogePaquete(self):
#def soltarPaquete(self): def soltarPaquete(self):
self.robot.stop()
self.pala.run_target(70,0)
self.robot.straight(-30)
#Deficnición y realización de la ruta #Deficnición y realización de la ruta
...@@ -195,36 +204,25 @@ class Robot: ...@@ -195,36 +204,25 @@ class Robot:
def recorreRuta(self,rutaDirecciones,rutaCoordenadas): def recorreRuta(self,rutaDirecciones,rutaCoordenadas):
for dir,coor in zip(rutaDirecciones,rutaCoordenadas): for dir,coor in zip(rutaDirecciones,rutaCoordenadas):
self.casillaSig = coor self.casillaSig = coor
if dir is MovGiro.Derecha: if dir is 2:
self.giraDerecha() self.giraDerecha()
elif dir is MovGiro.Izquierda: elif dir is 0:
self.giraIzquierda() self.giraIzquierda()
else: else:
self.sigueRecto() self.sigueRecto()
"""
def realizaPedido(self): def realizaPedido(self):
while self.recogePedido(): while self.recogePedido():
#posicion actual ---> A #posicion actual ---> A
rutaDirecciones,rutaCoordenadas = camino.encontrarCamino(self.mapa,self.casillaActual,self.pedido[0]) rutaDirecciones,rutaCoordenadas = camino.encontrarCamino(self.mapa,self.casillaActual,self.pedido[0])
self.recorreRuta(rutaDirecciones,rutaCoordenadas) self.recorreRuta(rutaDirecciones,rutaCoordenadas)
#self.recogePaquete() self.recogePaquete()
self.tono("cubo")
self.mediaVuelta() self.mediaVuelta()
#Ir desde punto A ---> B #Ir desde punto A ---> B
rutaDirecciones,rutaCoordenadas = camino.encontrarCamino(self.mapa,self.pedido[0],self.pedido[1]) rutaDirecciones,rutaCoordenadas = camino.encontrarCamino(self.mapa,self.pedido[0],self.pedido[1])
self.recorreRuta(rutaDirecciones, rutaCoordenadas) self.recorreRuta(rutaDirecciones, rutaCoordenadas)
#self.soltarPaquete() self.soltarPaquete()
self.tono("vic")
self.mediaVuelta() self.mediaVuelta()
"""
# def actualizaPosicionActual(self,movimiento:Direccion): \ No newline at end of file
#Extras totalmente innecesarios
def tono(self,cancion):
if cancion == "vic":
self.ev3.speaker.play_file("media/sound/vic.mp3")
elif cancion == "cubo":
self.ev3.speaker.play_file("media/sound/cubo.mp3")
else:
self.ev3.speaker.beep()
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