Skip to content

Vicente Castellano Gómez / robotAmbientales

Go to a project
Commit e9b6f98b by almagosi
Options

Initial commit

parent c2a7ab8a
Inline Side-by-side
Showing with 0 additions and 395 deletions
camino.py deleted 100644 → 0
from enum import Enum
class Bloque(Enum):
Edificio = 0
Calle_Izquierda_Derecha = 1
Calle_Arriba_Abajo = 2
Calle_Arriba_Derecha = 3
Calle_Derecha_Abajo = 4
Calle_Abajo_Izquierda = 5
Calle_Izquierda_Arriba = 6
Calle_Izquierda_Arriba_Derecha = 7
Calle_Arriba_Derecha_Abajo = 8
Calle_Derecha_Abajo_Izquierda = 9
Calle_Abajo_Izquierda_Arriba = 10
Calle_Arriba_Derecha_Abajo_Izquierda = 11
class Direccion(Enum):
Arriba = 0
Derecha = 1
Abajo = 2
Izquierda = 3
class Casilla:
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(matriz,posicionInicial, posicionFinal):
#distOptima : int = maxsize
def matrizAdyacencia(matriz):
def convertirMatrizCasillas(matriz):
matCas = []
for i in range(len(matriz)):
fila = []
for j in range(len(matriz[0])):
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):
return True
return False
matrizCasillas = convertirMatrizCasillas(matriz=matriz)
matrizAdy = []
for i in range(len(matriz)):
fila = []
for j in range(len(matriz[0])):
fila.append(matrizCasillas[i][j].bloque.value)
matrizAdy.append(fila)
return matrizAdy
matriz = matrizAdyacencia(matriz)
solOptima = []
direcOptima = []
sol = [posicionInicial]
direcciones = []
def caminoVueltraAtras(direccion,posicion):
global distOptima
#print(sol)
def factible(direc,pos):
def movimientoAceptado(direccion : Direccion, bloque : int, bloqueSiguiente : int):
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):
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):
return True
return False
#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 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):
return True
return False
direcSiguiente = Direccion((direccion.value + 2) % 4)
if(moviAcept(direccion,bloque) and moviAcept(direcSiguiente,bloqueSiguiente)):
return True
return False
#pos = sol[-1]
if(pos == posicionInicial):
#print(f"Descartado Inicio {direccion} : {pos}")
return False
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}")
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]}")
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
return True
if(factible(direccion,posicion)):
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))
else:
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])
solOptima.append(sol[-1])
else:
#distOptima = len(sol)
solOptima.clear()
direcOptima.clear()
for i in range(len(direcciones)):
solOptima.append(sol[i])
direcOptima.append(direcciones[i])
solOptima.append(sol[-1])
#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))
# 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"]]
#mAdy = matrizAdyacencia(matriz)
#pos = mAdy[6][0]
#mAdy[6][0] = 2
sol = encontrarCamino(matriz,(5,2),(6,0))
#mAdy[6][0] = pos
for s in sol:
print(s)
\ No newline at end of file
casilla.py deleted 100644 → 0
from enum import Enum
class Bloque(Enum):
Edificio = 0
Calle_Izquierda_Derecha = 1
Calle_Arriba_Abajo = 2
Calle_Arriba_Derecha = 3
Calle_Derecha_Abajo = 4
Calle_Abajo_Izquierda = 5
Calle_Izquierda_Arriba = 6
Calle_Izquierda_Arriba_Derecha = 7
Calle_Arriba_Derecha_Abajo = 8
Calle_Derecha_Abajo_Izquierda = 9
Calle_Abajo_Izquierda_Arriba = 10
Calle_Arriba_Derecha_Abajo_Izquierda = 11
class Casilla:
def __init__(self,x,y,codigo_bloque):
self.x = x
self.y = y
self.bloque = Bloque(codigo_bloque)
def __str__(self):
return f'x : {self.x} | y : {self.y} | {self.bloque}'
\ No newline at end of file
main.py deleted 100644 → 0
#!/usr/bin/env pybricks-micropython
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
# Create your objects here.
ev3 = EV3Brick()
colorSensor = ColorSensor(Port.S4)
left_motor = Motor(Port.A)
right_motor = Motor(Port.D)
giroSensor = GyroSensor(Port.S1)
robot = DriveBase(left_motor, right_motor, wheel_diameter=55.5, axle_track=124)
#Metodo para el giro
def giro(grados):
robot.reset()
giroSensor.reset_angle(0)
robot.turn(grados)
while(giroSensor.angle() != grados):
diferencia = giroSensor.angle() - grados
ajuste = diferencia * -1
for i in range(abs(ajuste)):
if(ajuste > 0):
robot.turn(1)
else:
if (ajuste < 0):
robot.turn(-1)
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 deleted 100644 → 0
#from typing import Any
import conexion as cn
import time
import json
class MensageRobot:
def __init__(self) -> None:
def on_message(client, userdata, message):
print(f'Recive Topic:{message.topic} Mensage:{str(message.payload.decode("utf-8"))}')
if(message.topic == self.topicSubs[-1]):
if(str(message.payload.decode("utf-8")) != str(self.__class__)):
self.sinc = True
else:
self.mensaje = str(message.payload.decode("utf-8"))
with open(file="conexionConfig.json",mode='r') as f:
data = json.load(f)
#self.topicSubs = ["A3-467/GrupoL/Robot"]
self.topicSubs = data['topicSubsRobot']
#self.topicSend = ["A3-467/GrupoL/Interfaz"]
self.topicSend = data['topicSendRobot']
#self.prefMsg = {"mapa":'map',
# "pedido":'ped',
# "posicion":'pos'}
self.prefMsg = data['prefijoMensajes']
self.sepMsg = data['separadorMensaje']
self.conex = cn.Conexion(self.topicSubs,on_msg=on_message)
self.mensaje = ""
self.sinc = False
self.__sincronizacion()
def __sincronizacion(self):
while(not self.sinc):
self.conex.publicar(self.topicSubs[-1],str(self.__class__))
time.sleep(5)
self.conex.desubscribir(self.topicSubs[-1])
self.conex.publicar(self.topicSubs[-1],str(self.__class__))
def getMapa(self):
while(self.mensaje.split(self.sepMsg)[0] != self.prefMsg['mapa']):
time.sleep(1)
return self.mensaje.split(self.sepMsg)[1]
def getPedido(self):
self.conex.publicar(self.topicSend,self.prefMsg['pedido'] + self.sepMsg)
while(self.mensaje.split(self.sepMsg)[0] != self.prefMsg['pedido']):
print(f"Esperando Pedido - {self.mensaje.split(self.sepMsg)[0],self.prefMsg['pedido']}")
time.sleep(1)
return self.mensaje.split(self.sepMsg)[1]
def sendPosicion(self,posicion):
self.conex.publicar(self.topicSend,self.prefMsg['posicion'] + self.sepMsg + str(posicion))
\ 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