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

main.py

 #!/usr/bin/env pybricks-micropython
 #!/usr/bin/env pybricks-micropython
 from robot import Robot
 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 = 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:
     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.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 camino
+from camino import MovGiro
 
 
 class Robot:
 class Robot:
     green_rgb = (36, 70, 38)
     green_rgb = (36, 70, 38)
@@ -16,6 +19,11 @@ class Robot:
     velocidadNegro = 50
     velocidadNegro = 50
     negroFlag = False
     negroFlag = False
     radiusCurve = 20
     radiusCurve = 20
+    interfono = None
+    mapa = None
+    casillaActual = (6,0)
+    pedido = None
+    casillaSig = None
 
 
     def __init__(self):
     def __init__(self):
         self.ev3 = EV3Brick()
         self.ev3 = EV3Brick()
@@ -25,7 +33,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.colaMovimientos = []
+        self.interfono = msg.MensajeRobot()
+        self.mapa = self.interfono.getMapa()
+
 
 
     #Metodos gestion negros
     #Metodos gestion negros
     def queNegroEs(self,negro):
     def queNegroEs(self,negro):
@@ -112,10 +122,15 @@ class Robot:
     def giraDerecha(self):
     def giraDerecha(self):
         self.robot.straight(60)
         self.robot.straight(60)
         self.giro(90)
         self.giro(90)
+        self.sigueRecto()
 
 
     def giraIzquierda(self):
     def giraIzquierda(self):
         self.robot.straight(60)
         self.robot.straight(60)
         self.giro(-90)
         self.giro(-90)
+        self.sigueRecto()
+
+    def mediaVuelta(self):
+        self.giro(180)
 
 
     def sigueRecto(self, color=green_rgb):
     def sigueRecto(self, color=green_rgb):
         # Valores iniciales
         # Valores iniciales
@@ -141,6 +156,7 @@ class Robot:
                 self.negroFlag = True
                 self.negroFlag = True
                 self.w = 0
                 self.w = 0
                 self.velocidad = self.velocidadNegro
                 self.velocidad = self.velocidadNegro
+
             else:
             else:
                 negroAnteriorFlag= self.negroFlag
                 negroAnteriorFlag= self.negroFlag
                 self.negroFlag = False
                 self.negroFlag = False
@@ -152,26 +168,57 @@ class Robot:
             if not negroAnteriorFlag and self.negroFlag:
             if not negroAnteriorFlag and self.negroFlag:
                 contNegroAnterior = self.contadorNegros
                 contNegroAnterior = self.contadorNegros
                 self.incrementaContNegros()
                 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)
             self.robot.drive(self.velocidad,self.w)
 
 
             print(self.contadorNegros,contNegroAnterior)
             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 rutaPedido(self):
-        while self.colaMovimientos.not_empty():
-            movimientoAct = self.colaMovimientos.get()
-            #Aqui faltan los if correspondientes a cada tipo de movimiento
+    #def soltarPaquete(self):
 
 
 
 
+    #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
     #Extras totalmente innecesarios
     def tono(self,cancion):
     def tono(self,cancion):