- Funca menos segundo pedido

camino.py

@@ -256,6 +256,7 @@ def encontrarCamino(stringMapa, posicionInicial, posicionFinal):
 
     pos = posicionInicial
     listaCasillas = []
+    print("directOptima",direcOptima)
     orientacion = direcOptima[0]
     dirs = []
 
@@ -291,8 +292,3 @@ def encontrarCamino(stringMapa, posicionInicial, posicionFinal):
     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",
     "topicSubsRobot" : ["A3-467/GrupoL/Robot","A3-467/GrupoL/SincRobot"],
     "topicSubsInterfaz" : ["A3-467/GrupoL/Interfaz","map","A3-467/GrupoL/SincInterfaz"],

main.py

@@ -3,9 +3,21 @@ from robot import Robot
 import camino
 from camino import MovGiro as mov
 from umqtt.robust import MQTTClient
-
 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):
     print("Recibido topic", topic.decode(), " mensage ", msg.decode())
@@ -24,13 +36,5 @@ pos = (4,4)
 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:
             self.conex.checkMensages()
             #print(f"Esperando Pedido - {self.mensaje.split(self.sepMsg)[0],self.prefMsg['pedido']}")
             time.sleep(1)
+        print("Pedido recibido en getPedido : ",self.mensaje.split(self.sepMsg)[1])
 
         ped = []
         pos = []
@@ -103,7 +104,11 @@ class MensajeRobot:
                 pos = []
             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):
         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
 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 as mov
 
@@ -34,8 +34,9 @@ class Robot:
         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()
+        self.interfono.sendPosicion(self.casillaActual)
 
 
     #Metodos gestion negros
@@ -73,6 +74,7 @@ class Robot:
             print(self.colorSensor.rgb())
             wait(20)
 
+
     def calibraSensorColorReal(self):
         self.ev3.screen.print("Presiona el botón para calibrar el color verde")
         while not Button.CENTER in self.ev3.buttons.pressed():
@@ -94,7 +96,6 @@ class Robot:
         self.black_rgb = black_rgb_medido
         self.ev3.screen.print("Calibración completa.")
 
-
     #Metodos para el movimiento
     def giro(self,grados):
         self.giroSensor.reset_angle(0)
@@ -172,8 +173,8 @@ 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)
@@ -211,18 +212,22 @@ class Robot:
             else:
                 self.sigueRecto()
 
-"""
+
     def realizaPedido(self):
         while self.recogePedido():
             #posicion actual ---> A
+            print("Recogida Principio",self.casillaActual," Final ",self.pedido[0])
             rutaDirecciones,rutaCoordenadas = camino.encontrarCamino(self.mapa,self.casillaActual,self.pedido[0])
+            print("Pedido Recogida:",rutaDirecciones,rutaCoordenadas)
             self.recorreRuta(rutaDirecciones,rutaCoordenadas)
             self.recogePaquete()
             self.mediaVuelta()
 
             #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])
+            print("Pedido Soltar:",rutaDirecciones,rutaCoordenadas)
             self.recorreRuta(rutaDirecciones, rutaCoordenadas)
             self.soltarPaquete()
             self.mediaVuelta()
-"""
\ No newline at end of file
+            self.robot.stop()