Skip to content

Vicente Castellano Gómez / robotAmbientales

Go to a project
Commit 335c2164 by almagosi
Options

Initial commit

parent c2a7ab8a
Inline Side-by-side
Showing with 0 additions and 332 deletions
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_interfaz.py deleted 100644 → 0
import mensajeInterfaz as msg
import time
msgInterfaz = msg.MensageInterfaz()
mapa = msgInterfaz.getMapa()
print(f"Mapa recibido {mapa}")
print("Comprobando solicitud")
while(not msgInterfaz.pedidoSolicitado):
time.sleep(1)
print("Comprobando solicitud")
posicion = msgInterfaz.getPosicion()
print(f"Posicion Recibida {posicion}")
pedido = ((4,5),(7,9))
msgInterfaz.sendPedido(pedido)
print("Pedido enviado")
\ No newline at end of file
main_map.py deleted 100644 → 0
import conexion as cn
import time
mapa = "0202000105030705000200041109060110031000000200080101100110000106010701"
while(True):
cn.send_mqtt_message("map",mapa)
print("Enviado")
time.sleep(60)
\ No newline at end of file
mensajeInterfaz.py deleted 100644 → 0
#from typing import Any
import conexion as cn
import time
import json
class MensageInterfaz:
def __init__(self) -> None:
def filtrarMensage(topic,mensaje):
if(topic == self.prefMsg['mapa']):
self.mapa = mensaje
while(not self.sinc):
time.sleep(1)
self.__enviarMapa()
self.conex.desubscribir(topic)
else:
if(topic == self.topicSubs[-1]):
if(mensaje != str(self.__class__)):
self.sinc = True
else:
if(mensaje.split(self.sepMsg)[0] != self.prefMsg['pedido']):
self.pedidoSolicitado = True
if(mensaje.split(self.sepMsg)[0] != self.prefMsg['posicion']):
self.posicion = list(mensaje.split(self.sepMsg)[1])
def on_message(client, userdata, message):
print(f'Recive Topic:{message.topic} Mensage:{str(message.payload.decode("utf-8"))}')
filtrarMensage(message.topic,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['topicSubsInterfaz']
#self.topicSend = ["A3-467/GrupoL/Interfaz"]
self.topicSend = data['topicSendInterfaz']
#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.mapa = ""
self.posicion : list
self.pedidoSolicitado = False
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 __enviarMapa(self):
self.conex.publicar(self.topicSend,self.prefMsg['mapa'] + self.sepMsg + self.mapa)
def getMapa(self):
while(len(self.mapa) == 0):
time.sleep(1)
return self.mapa
def sendPedido(self,pedido):
self.conex.publicar(self.topicSend,self.prefMsg['pedido'] + self.sepMsg + str(pedido))
self.pedidoSolicitado = False
def getPosicion(self):
while(self.posicion is None):
print("No hay posicion")
time.sleep(1)
return self.posicion
\ No newline at end of file
pedidos.py deleted 100644 → 0
from math import pow,sqrt
from sys import maxsize
class Posicion:
def __init__(self,x : int,y : int):
self.x = x
self.y = y
def __str__(self) -> str:
return f'({self.x},{self.y})'
class Pedido:
def __init__(self,x_ini : int,y_ini : int, x_fin : int,y_fin : int) :
self.inicio = Posicion(x_ini,y_ini)
self.final = Posicion(x_fin,y_fin)
def __str__(self) -> str:
return f'({self.inicio},{self.final})'
def ordenarPedidos(posicionActual : Posicion,lista : list):
def imprimir(lista : list):
for elem in lista:
print(elem)
def imprimirPosiciones(lP : list):
pos = ""
for p in lP:
pos = pos + str(p) + ":"
print(pos)
def distanciaPosicionPedido(pos: Posicion,ped : Pedido):
return sqrt(pow(pos.x - ped.inicio.x,2) + pow(pos.y - ped.inicio.y,2))
def distanciaPosiciones(pos1 : Posicion, pos2 : Posicion):
return sqrt(pow(pos1.x - pos2.x,2) + pow(pos1.y - pos2.y,2))
def transformarAPedidos(lista : list):
listaPedidos = []
for elem in lista:
listaPedidos.append(Pedido(elem[0][0],elem[0][1],elem[1][0],elem[1][1]))
return listaPedidos
pedidos = transformarAPedidos(lista=lista)
n_Posiciones = len(pedidos) * 2
listaPosiciones = []
matriz = []
for i in range(n_Posiciones):
m = []
for j in range(n_Posiciones):
m.append(maxsize)
matriz.append(m)
mindist = maxsize
minPos : int
for p in pedidos:
posIni = len(listaPosiciones)
listaPosiciones.append(p.inicio)
posFin = posIni + 1
listaPosiciones.append(p.final)
matriz[posIni][posFin] = 0
#matriz[posFin][posIni] = 0
dist = distanciaPosicionPedido(pos=posicionActual,ped=p)
#print("Posicion Actual : ",posicionActual," Pedido : ",p.inicio,p.final," Valor : ",dist)
if(dist < mindist):
mindist = dist
minPos = posIni
listaPedidos = [listaPosiciones[minPos]]
for i in range(n_Posiciones):
for j in range(n_Posiciones):
# a mod 2 --> 0 : inicio , 1 : final
if( matriz[i][j] != 0 and matriz[j][i] != 0 and i != j and i%2 == 1 and j%2 == 0):
matriz[i][j] = distanciaPosiciones(listaPosiciones[i],listaPosiciones[j])
#pos = ""
#for p in listaPosiciones:
# pos = pos + str(p) + ":"
#print(pos)
#for m in matriz:
# s = ""
# for mj in m:
# if(mj != maxsize):
# s = f'{s}, {mj}'
# else:
# s = f'{s}, inf'
# print(s)
while(len(listaPedidos) != n_Posiciones):
pos = minPos
mindist = maxsize
minPos = -1
for i in range(n_Posiciones):
if(matriz[pos][i] < mindist and not listaPosiciones[i] in listaPedidos):
mindist = matriz[pos][i]
minPos = i
listaPedidos.append(listaPosiciones[minPos])
#imprimirPosiciones(listaPedidos)
return listaPedidos
posicionInicio = Posicion(6,0)
listaPed = []
listaPed.append(((6,4),(0,3)))
listaPed.append(((0,1),(3,0)))
#listap = transformarAPedidos(posicionInicio,listaPed)
peds = ordenarPedidos(posicionActual=posicionInicio,lista=listaPed)
for p in peds:
print(p)
#print(transformarAPedidos(listaPed))
\ 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