Integracion completa a falta de prueba de la ruta con los movimientos

main.py

 #!/usr/bin/env pybricks-micropython
 from robot import Robot
-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
+
 
 
 robot = Robot()
 
-robot.sigueRecto()
-robot.sigueRecto()
-#robot.sigueLineaHastaNegro()
-#robot.sigueLineaHastaNegro()
-robot.giraDerecha()
-robot.sigueRecto()
-robot.giraIzquierda()
+robot.calibraSensorColor()
 
-#robot.calibraSensorColor()
+robot.realizaPedido()

main_robot.py

-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

@@ -6,7 +6,7 @@ import json
 
 
 
-class MensageRobot:    
+class MensajeRobot:
 
 
     def __init__(self) -> None:

robot.py

@@ -4,6 +4,9 @@ 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 camino
+from camino import MovGiro
 
 class Robot:
     green_rgb = (36, 70, 38)
@@ -16,6 +19,11 @@ class Robot:
     velocidadNegro = 50
     negroFlag = False
     radiusCurve = 20
+    interfono = None
+    mapa = None
+    casillaActual = (6,0)
+    pedido = None
+    casillaSig = None
 
     def __init__(self):
         self.ev3 = EV3Brick()
@@ -25,7 +33,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.colaMovimientos = []
+        self.interfono = msg.MensajeRobot()
+        self.mapa = self.interfono.getMapa()
+
 
     #Metodos gestion negros
     def queNegroEs(self,negro):
@@ -112,10 +122,15 @@ class Robot:
     def giraDerecha(self):
         self.robot.straight(60)
         self.giro(90)
+        self.sigueRecto()
 
     def giraIzquierda(self):
         self.robot.straight(60)
         self.giro(-90)
+        self.sigueRecto()
+
+    def mediaVuelta(self):
+        self.giro(180)
 
     def sigueRecto(self, color=green_rgb):
         # Valores iniciales
@@ -141,6 +156,7 @@ class Robot:
                 self.negroFlag = True
                 self.w = 0
                 self.velocidad = self.velocidadNegro
+
             else:
                 negroAnteriorFlag= self.negroFlag
                 self.negroFlag = False
@@ -152,26 +168,57 @@ class Robot:
             if not negroAnteriorFlag and self.negroFlag:
                 contNegroAnterior = self.contadorNegros
                 self.incrementaContNegros()
+                # Actualizacion de casilla
+                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)
 
-    #Deficnición y realización de la ruta
-
-    # Recogemos la lista de movimientos del algoritmo
-    #def recogeListaMovimientosPedido(self, lMovimientos):
-    #    for movimiento in lMovimientos:
-    #        self.colaMovimientos.put(movimiento)
+    #def recogePaquete(self):
 
+    #def soltarPaquete(self):
 
 
-    def rutaPedido(self):
-        while self.colaMovimientos.not_empty():
-            movimientoAct = self.colaMovimientos.get()
-            #Aqui faltan los if correspondientes a cada tipo de movimiento
-
+    #Deficnición y realización de la ruta
+    # Recogemos la lista de movimientos del algoritmo
+    def recogePedido(self):
+        self.pedido = self.interfono.getPedido()
+        return True
 
+    def actualizaCasilla(self):
+        self.casillaActual = self.casillaSig
+        self.interfono.sendPosicion(self.casillaActual)
+
+    def recorreRuta(self,rutaDirecciones,rutaCoordenadas):
+        for dir,coor in zip(rutaDirecciones,rutaCoordenadas):
+            self.casillaSig = coor
+            if dir is MovGiro.Derecha:
+                self.giraDerecha()
+            elif dir is MovGiro.Izquierda:
+                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.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.mediaVuelta()
+
+   # def actualizaPosicionActual(self,movimiento:Direccion):
 
     #Extras totalmente innecesarios
     def tono(self,cancion):