Skip to content

Vicente Castellano Gómez / robotAmbientales

Go to a project
Commit 5a8b03b3 by Vicente Castellano Gómez
Options

- Funca menos segundo pedido

parent b267f2cb
Inline Side-by-side
Showing with 34 additions and 25 deletions
camino.py
...@@ -256,6 +256,7 @@ def encontrarCamino(stringMapa, posicionInicial, posicionFinal): ...@@ -256,6 +256,7 @@ def encontrarCamino(stringMapa, posicionInicial, posicionFinal):
pos = posicionInicial pos = posicionInicial
listaCasillas = [] listaCasillas = []
print("directOptima",direcOptima)
orientacion = direcOptima[0] orientacion = direcOptima[0]
dirs = [] dirs = []
...@@ -291,8 +292,3 @@ def encontrarCamino(stringMapa, posicionInicial, posicionFinal): ...@@ -291,8 +292,3 @@ def encontrarCamino(stringMapa, posicionInicial, posicionFinal):
return dirs, solOptima return dirs, solOptima
mapa = "0202000105030705000200041109060110031000000200080101100110000106010701"
d,c = encontrarCamino(mapa,(0,0),(6,4))
conexionConfig.json
{ {
"ipbroker" :"192.168.48.245", "ipbroker" :"192.168.0.134",
"portbroker" : "1883", "portbroker" : "1883",
"topicSubsRobot" : ["A3-467/GrupoL/Robot","A3-467/GrupoL/SincRobot"], "topicSubsRobot" : ["A3-467/GrupoL/Robot","A3-467/GrupoL/SincRobot"],
"topicSubsInterfaz" : ["A3-467/GrupoL/Interfaz","map","A3-467/GrupoL/SincInterfaz"], "topicSubsInterfaz" : ["A3-467/GrupoL/Interfaz","map","A3-467/GrupoL/SincInterfaz"],
......
main.py
...@@ -3,9 +3,21 @@ from robot import Robot ...@@ -3,9 +3,21 @@ from robot import Robot
import camino import camino
from camino import MovGiro as mov from camino import MovGiro as mov
from umqtt.robust import MQTTClient from umqtt.robust import MQTTClient
import conexion as cn import conexion as cn
robot = Robot()
#robot.calibraSensorColor()
robot.realizaPedido()
#mov,cas = camino.encontrarCamino(mapa,(0,0),(3,0))
#robot.recorreRuta(mov,cas)
#robot.recogePaquete()
""" """
def sub_cp(topic, msg): def sub_cp(topic, msg):
print("Recibido topic", topic.decode(), " mensage ", msg.decode()) print("Recibido topic", topic.decode(), " mensage ", msg.decode())
...@@ -24,13 +36,5 @@ pos = (4,4) ...@@ -24,13 +36,5 @@ pos = (4,4)
mR.sendPosicion(pos) mR.sendPosicion(pos)
""" """
robot = Robot()
mapa = "0202000105030705000200041109060110031000000200080101100110000106010701"
#robot.calibraSensorColor()
mov,cas = camino.encontrarCamino(mapa,(0,0),(3,0))
robot.recorreRuta(mov,cas)
robot.recogePaquete()
mensajeRobot.py
...@@ -92,6 +92,7 @@ class MensajeRobot: ...@@ -92,6 +92,7 @@ class MensajeRobot:
self.conex.checkMensages() self.conex.checkMensages()
#print(f"Esperando Pedido - {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) time.sleep(1)
print("Pedido recibido en getPedido : ",self.mensaje.split(self.sepMsg)[1])
ped = [] ped = []
pos = [] pos = []
...@@ -103,7 +104,11 @@ class MensajeRobot: ...@@ -103,7 +104,11 @@ class MensajeRobot:
pos = [] pos = []
i = i+1 i = i+1
return ped pedi = ((ped[0][0],ped[0][1]),(ped[1][0],ped[1][1]))
self.mensaje = ""
return pedi
def sendPosicion(self,posicion): def sendPosicion(self,posicion):
self.conex.publicar(self.topicSend[0],self.prefMsg['posicion'] + self.sepMsg + str(posicion)) self.conex.publicar(self.topicSend[0],self.prefMsg['posicion'] + self.sepMsg + str(posicion))
......
robot.py
...@@ -4,7 +4,7 @@ from pybricks.parameters import Port, Stop, Direction, Button, Color ...@@ -4,7 +4,7 @@ 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 as mov from camino import MovGiro as mov
...@@ -34,8 +34,9 @@ class Robot: ...@@ -34,8 +34,9 @@ class Robot:
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()
self.interfono.sendPosicion(self.casillaActual)
#Metodos gestion negros #Metodos gestion negros
...@@ -73,6 +74,7 @@ class Robot: ...@@ -73,6 +74,7 @@ class Robot:
print(self.colorSensor.rgb()) print(self.colorSensor.rgb())
wait(20) wait(20)
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.CENTER in self.ev3.buttons.pressed(): while not Button.CENTER in self.ev3.buttons.pressed():
...@@ -94,7 +96,6 @@ class Robot: ...@@ -94,7 +96,6 @@ class Robot:
self.black_rgb = black_rgb_medido self.black_rgb = black_rgb_medido
self.ev3.screen.print("Calibración completa.") self.ev3.screen.print("Calibración completa.")
#Metodos para el movimiento #Metodos para el movimiento
def giro(self,grados): def giro(self,grados):
self.giroSensor.reset_angle(0) self.giroSensor.reset_angle(0)
...@@ -172,8 +173,8 @@ class Robot: ...@@ -172,8 +173,8 @@ 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)
...@@ -211,18 +212,22 @@ class Robot: ...@@ -211,18 +212,22 @@ class Robot:
else: else:
self.sigueRecto() self.sigueRecto()
"""
def realizaPedido(self): def realizaPedido(self):
while self.recogePedido(): while self.recogePedido():
#posicion actual ---> A #posicion actual ---> A
print("Recogida Principio",self.casillaActual," Final ",self.pedido[0])
rutaDirecciones,rutaCoordenadas = camino.encontrarCamino(self.mapa,self.casillaActual,self.pedido[0]) rutaDirecciones,rutaCoordenadas = camino.encontrarCamino(self.mapa,self.casillaActual,self.pedido[0])
print("Pedido Recogida:",rutaDirecciones,rutaCoordenadas)
self.recorreRuta(rutaDirecciones,rutaCoordenadas) self.recorreRuta(rutaDirecciones,rutaCoordenadas)
self.recogePaquete() self.recogePaquete()
self.mediaVuelta() self.mediaVuelta()
#Ir desde punto A ---> B #Ir desde punto A ---> B
print("Entrega Principio",self.pedido[0]," Final ",self.pedido[1])
rutaDirecciones,rutaCoordenadas = camino.encontrarCamino(self.mapa,self.pedido[0],self.pedido[1]) rutaDirecciones,rutaCoordenadas = camino.encontrarCamino(self.mapa,self.pedido[0],self.pedido[1])
print("Pedido Soltar:",rutaDirecciones,rutaCoordenadas)
self.recorreRuta(rutaDirecciones, rutaCoordenadas) self.recorreRuta(rutaDirecciones, rutaCoordenadas)
self.soltarPaquete() self.soltarPaquete()
self.mediaVuelta() self.mediaVuelta()
""" self.robot.stop()
\ 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