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(Enum):
class Bloque:
Edificio = 0
Calle_Izquierda_Derecha = 1
Calle_Arriba_Abajo = 2
......@@ -15,19 +12,32 @@ class Bloque(Enum):
Calle_Abajo_Izquierda_Arriba = 10
Calle_Arriba_Derecha_Abajo_Izquierda = 11
#value
def __init__(self,cod):
self.value = cod
class Direccion(Enum):
class Direccion:
Arriba = 0
Derecha = 1
Abajo = 2
Izquierda = 3
#value
def __init__(self,direccion):
self.value = direccion
class MovGiro(Enum):
class MovGiro:
Izquierda = 0
Recto = 1
Derecha = 2
#value
def __init__(self,MovGiro):
self.value = MovGiro
class Casilla:
......@@ -36,11 +46,19 @@ class Casilla:
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)
def encontrarCamino(matriz, posicionInicial, posicionFinal):
# distOptima : int = maxsize
def matrizAdyacencia(matriz):
......@@ -71,11 +89,8 @@ def encontrarCamino(matriz, 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()
#return matriz
solOptima = []
direcOptima = []
......@@ -83,17 +98,16 @@ def encontrarCamino(matriz, posicionInicial, posicionFinal):
direcciones = []
def caminoVueltraAtras(direccion, posicion):
global distOptima
#global distOptima
# print(direccion,posicion)
# print(sol)
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):
# print(direccion.name, bloque)
def moviAcept(direccion, bloque: int):
if (direccion == Direccion.Arriba):
# arriba = [2,4,5,8,9,10,11]
......@@ -125,37 +139,38 @@ def encontrarCamino(matriz, posicionInicial, posicionFinal):
return True
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)):
return True
return False
# pos = sol[-1]
#print(pos)
if (pos == posicionInicial):
# print(f"Descartado Inicio {direccion} : {pos}")
#print(f"Descartado Inicio {direccion} : {pos}")
return False
if (pos in sol):
# print(f"Descartado Usada {direccion} : {pos}")
#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}")
#print(f"Descartado Fuera de Rango {direccion} : {pos}")
return False
if (matriz[pos[0]][pos[1]] == 0):
# print(f"Descartado Edificio {direccion} : {pos}")
#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]}")
#print(f"Descartado Movimiento no aceptado {direccion} : {sol[-1]}")
return False
# if(len(sol)+1 >= distOptima):
if (len(solOptima) > 0):
if (len(sol) + 1 >= len(solOptima)):
# print(f"Descartado Muy Largo {direccion} : {pos}")
#print(f"Descartado Muy Largo {direccion} : {pos}")
return False
return True
......@@ -205,7 +220,8 @@ def encontrarCamino(matriz, posicionInicial, posicionFinal):
caminoVueltraAtras(Direccion.Izquierda, (posicionInicial[0], posicionInicial[1] - 1))
# Devuelve el recogido realizado tanto en posiciones como en direcciones
# return solOptima,direcOptima
#print(solOptima,direcOptima)
#return solOptima,direcOptima
def arriba(posicion):
return (posicion[0] - 1, posicion[1])
......@@ -219,17 +235,21 @@ def encontrarCamino(matriz, posicionInicial, posicionFinal):
def derecha(posicion):
return (posicion[0], posicion[1] + 1)
def crearMov(orient: Direccion, dir: Direccion):
def crearMov(orient: Direccion, dir):
if (orient is dir):
return MovGiro.Recto
if (orient is Direccion.Derecha):
return MovGiro(dir.value)
#return MovGiro(dir)
return dir
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):
return MovGiro((dir.value + 1) % 4)
#return MovGiro((dir + 1) % 4)
return (dir + 1) % 4
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'Casillas {solOptima}')
......@@ -258,11 +278,12 @@ def encontrarCamino(matriz, posicionInicial, posicionFinal):
if (direc is Direccion.Derecha):
listaCasillas.append(derecha(pos))
orientacion = Direccion.Derecha
# for d in dirs:
# print(d)
# print()
"""
for d in dirs:
print(d)
print()
solOptima.pop(0)
"""
# for d in solOptima:
# print(d)
......@@ -270,30 +291,8 @@ def encontrarCamino(matriz, posicionInicial, posicionFinal):
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)
mapa = "0202000105030705000200041109060110031000000200080101100110000106010701"
# pos = mAdy[6][0]
# mAdy[6][0] = 2
d,c = encontrarCamino(mapa,(0,0),(6,4))
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
import camino
from camino import MovGiro as mov
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
from pybricks.tools import wait, StopWatch, DataLog
from pybricks.robotics import DriveBase
from pybricks.media.ev3dev import SoundFile, ImageFile
import mensajeRobot as msg
#import mensajeRobot as msg
import camino
from camino import MovGiro
from camino import MovGiro as mov
class Robot:
green_rgb = (36, 70, 38)
black_rgb = (5, 5, 16)
green_rgb = (35, 55, 37)
black_rgb = (6, 3, 18)
tolerancia = 10
contadorNegros = 0
valorW = 30
w = 0
velocidad = 50
velocidad = 60
velocidadNegro = 50
negroFlag = False
radiusCurve = 20
......@@ -29,12 +29,13 @@ class Robot:
self.ev3 = EV3Brick()
left_motor = Motor(Port.A)
right_motor = Motor(Port.D)
self.pala = Motor(Port.B)
self.giroSensor = GyroSensor(Port.S1)
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.interfono = msg.MensajeRobot()
self.mapa = self.interfono.getMapa()
#self.interfono = msg.MensajeRobot()
#self.mapa = self.interfono.getMapa()
#Metodos gestion negros
......@@ -74,7 +75,7 @@ class Robot:
def calibraSensorColorReal(self):
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)
# Medir color verde
......@@ -82,7 +83,7 @@ class Robot:
self.ev3.screen.print("Color verde calibrado:", green_rgb_medido)
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)
# Medir color negro
......@@ -120,17 +121,19 @@ class Robot:
self.robot.turn(-1)
def giraDerecha(self):
self.robot.straight(60)
self.robot.straight(62) #60
self.giro(90)
self.sigueRecto()
def giraIzquierda(self):
self.robot.straight(60)
self.robot.straight(62) #60
self.giro(-90)
self.sigueRecto()
def mediaVuelta(self):
self.giro(180)
self.robot.straight(60)
self.giro(90)
self.giro(90)
def sigueRecto(self, color=green_rgb):
# Valores iniciales
......@@ -169,17 +172,23 @@ class Robot:
contNegroAnterior = self.contadorNegros
self.incrementaContNegros()
# Actualizacion de casilla
if self.queNegroEs(self.contadorNegros) == 1:
self.actualizaCasilla()
#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)
#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
......@@ -195,36 +204,25 @@ class Robot:
def recorreRuta(self,rutaDirecciones,rutaCoordenadas):
for dir,coor in zip(rutaDirecciones,rutaCoordenadas):
self.casillaSig = coor
if dir is MovGiro.Derecha:
if dir is 2:
self.giraDerecha()
elif dir is MovGiro.Izquierda:
elif dir is 0:
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.recogePaquete()
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.soltarPaquete()
self.mediaVuelta()
# def actualizaPosicionActual(self,movimiento:Direccion):
#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()
"""
\ No newline at end of file
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