Merge remote-tracking branch 'origin/robot' into Finalizacion

.gitignore

@@ -2,4 +2,5 @@ __pycache__/
 *.pyc
 .venv/
 .vscode/
+.idea/
 

camino.py

+class Bloque:
+    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
+
+    #value
+
+    def __init__(self,cod):
+        self.value = cod
+
+
+class Direccion:
+    Arriba = 0
+    Derecha = 1
+    Abajo = 2
+    Izquierda = 3
+
+    #value
+    def __init__(self,direccion):
+        self.value = direccion
+
+
+class MovGiro:
+    Izquierda = 0
+    Recto = 1
+    Derecha = 2
+
+    #value
+
+    def __init__(self,MovGiro):
+        self.value = MovGiro
+
+class Casilla:
+
+    def __init__(self, x: int, y: int, codigo_bloque: str):
+        self.x = x
+        self.y = y
+        self.bloque = Bloque(int(codigo_bloque))
+
+
+def encontrarCamino(stringMapa, posicionInicial, posicionFinal):
+    def mapaMatrix(mapa):
+        segmentos = [mapa[i:i + 2] for i in range(0, len(mapa), 2)]
+        matriz = []
+        for i in range(7):
+            fila = segmentos[i * 5:(i + 1) * 5]
+            matriz.append(fila)
+
+        return matriz
+
+    matriz = mapaMatrix(stringMapa)
+
+    # distOptima : int = maxsize
+    def matrizAdyacencia(matriz):
+
+        def convertirMatrizCasillas(matriz):
+            matCas = []
+            for i in range(len(matriz)):
+                fila = []
+                for j in range(len(matriz[0])):
+                    fila.append(Casilla(i, j, matriz[i][j]))
+                matCas.append(fila)
+
+            return matCas
+
+        def dentroTablero(coord):
+            if (coord[0] < len(matriz) and coord[0] >= 0 and coord[1] < len(matriz[0]) and coord[1] >= 0):
+                return True
+            return False
+
+        matrizCasillas = convertirMatrizCasillas(matriz=matriz)
+
+        matrizAdy = []
+        for i in range(len(matriz)):
+            fila = []
+            for j in range(len(matriz[0])):
+                fila.append(matrizCasillas[i][j].bloque.value)
+            matrizAdy.append(fila)
+
+        return matrizAdy
+
+    matriz = matrizAdyacencia(matriz)
+
+    #return matriz
+
+    solOptima = []
+    direcOptima = []
+    sol = [posicionInicial]
+    direcciones = []
+
+    def caminoVueltraAtras(direccion, posicion):
+        #global distOptima
+
+        # print(direccion,posicion)
+        # print(sol)
+
+        def factible(direc, pos):
+
+            def movimientoAceptado(direccion, bloque: int, bloqueSiguiente: int):
+
+                def moviAcept(direccion, bloque: int):
+                    if (direccion == Direccion.Arriba):
+
+                        # arriba = [2,4,5,8,9,10,11]
+                        arriba = [2, 3, 6, 7, 8, 10, 11]
+                        if (bloque in arriba):
+                            return True
+                        return False
+                    if (direccion == Direccion.Abajo):
+                        # abajo = [2,3,6,7,8,10,11]
+                        abajo = [2, 4, 5, 8, 9, 10, 11]
+                        if (bloque in abajo):
+                            return True
+                        return False
+                        # if(bloquePosterio.value == 2 or bloquePosterio.value == 3
+                        #   or bloquePosterio.value == 6 or bloquePosterio.value == 7
+                        #   or bloquePosterio.value == 10 or bloquePosterio.value == 11):
+                        #    return True
+                        # return False
+                    if (direccion == Direccion.Izquierda):
+                        # izquierda = [1,3,4,7,8,9,11]
+                        izquierda = [1, 5, 6, 7, 9, 10, 11]
+                        if (bloque in izquierda):
+                            return True
+                        return False
+                    if (direccion == Direccion.Derecha):
+                        # derecha = [1,6,7,9,10,11]
+                        derecha = [1, 3, 4, 7, 8, 9, 11]
+                        if (bloque in derecha):
+                            return True
+                        return False
+
+                #direcSiguiente = Direccion((direccion + 2) % 4)
+                direcSiguiente = (direccion + 2) % 4
+                if (moviAcept(direccion, bloque) and moviAcept(direcSiguiente, bloqueSiguiente)):
+                    return True
+                return False
+
+            # pos = sol[-1]
+            #print(pos)
+            if (pos == posicionInicial):
+                #print(f"Descartado Inicio {direccion} : {pos}")
+                return False
+
+            if (pos in sol):
+                #print(f"Descartado Usada {direccion} : {pos}")
+                return False
+
+            if (pos[0] < 0 or pos[0] >= len(matriz) or pos[1] < 0 or pos[1] >= len(matriz[0])):
+                #print(f"Descartado Fuera de Rango {direccion} : {pos}")
+                return False
+
+            if (matriz[pos[0]][pos[1]] == 0):
+                #print(f"Descartado Edificio {direccion} : {pos}")
+                return False
+
+            if (not movimientoAceptado(direc, matriz[sol[-1][0]][sol[-1][1]], matriz[pos[0]][pos[1]])):
+                #print(f"Descartado Movimiento no aceptado {direccion} : {sol[-1]}")
+                return False
+
+            # if(len(sol)+1 >= distOptima):
+            if (len(solOptima) > 0):
+                if (len(sol) + 1 >= len(solOptima)):
+                    #print(f"Descartado Muy Largo {direccion} : {pos}")
+                    return False
+
+            return True
+
+        if (factible(direccion, posicion)):
+
+            # print("Factible")
+            sol.append(posicion)
+            direcciones.append(direccion)
+
+            pos = sol[-1]
+            # bq = mAdy[pos[0]][pos[1]]
+            # mAdy[pos[0]][pos[1]] = 0
+
+            if (pos != posicionFinal):
+                caminoVueltraAtras(Direccion.Arriba, (pos[0] - 1, pos[1]))
+                caminoVueltraAtras(Direccion.Abajo, (pos[0] + 1, pos[1]))
+                caminoVueltraAtras(Direccion.Derecha, (pos[0], pos[1] + 1))
+                caminoVueltraAtras(Direccion.Izquierda, (pos[0], pos[1] - 1))
+            else:
+                if (len(solOptima) > 0):
+                    if (len(sol) < len(solOptima)):
+                        # distOptima = len(sol)
+                        solOptima.clear()
+                        direcOptima.clear()
+                        for i in range(len(direcciones)):
+                            solOptima.append(sol[i])
+                            direcOptima.append(direcciones[i])
+                        solOptima.append(sol[-1])
+                else:
+                    # distOptima = len(sol)
+                    solOptima.clear()
+                    direcOptima.clear()
+                    for i in range(len(direcciones)):
+                        solOptima.append(sol[i])
+                        direcOptima.append(direcciones[i])
+                    solOptima.append(sol[-1])
+
+            # mAdy[pos[0]][pos[1]] = bq
+
+            sol.pop()
+            direcciones.pop()
+
+    caminoVueltraAtras(Direccion.Arriba, (posicionInicial[0] - 1, posicionInicial[1]))
+    caminoVueltraAtras(Direccion.Abajo, (posicionInicial[0] + 1, posicionInicial[1]))
+    caminoVueltraAtras(Direccion.Derecha, (posicionInicial[0], posicionInicial[1] + 1))
+    caminoVueltraAtras(Direccion.Izquierda, (posicionInicial[0], posicionInicial[1] - 1))
+
+    # Devuelve el recogido realizado tanto en posiciones como en direcciones
+    #print(solOptima,direcOptima)
+    #return solOptima,direcOptima
+
+    def arriba(posicion):
+        return (posicion[0] - 1, posicion[1])
+
+    def abajo(posicion):
+        return (posicion[0] + 1, posicion[1])
+
+    def izquierda(posicion):
+        return (posicion[0], posicion[1] - 1)
+
+    def derecha(posicion):
+        return (posicion[0], posicion[1] + 1)
+
+    def crearMov(orient: Direccion, dir):
+        if (orient is dir):
+            return MovGiro.Recto
+        if (orient is Direccion.Derecha):
+            #return MovGiro(dir)
+            return dir
+        if (orient is Direccion.Izquierda):
+            #return MovGiro((dir + 2) % 4)
+            return (dir + 2) % 4
+        if (orient is Direccion.Arriba):
+            #return MovGiro((dir + 1) % 4)
+            return (dir + 1) % 4
+        if (orient is Direccion.Abajo):
+            #return MovGiro((dir - 1) % 4)
+            return (dir - 1) % 4
+
+    # print(f'Direcciones {len(direcOptima)}')
+    # print(f'Casillas {solOptima}')
+
+    pos = posicionInicial
+    listaCasillas = []
+    print("directOptima",direcOptima)
+    orientacion = direcOptima[0]
+    dirs = []
+
+    for direc in direcOptima:
+
+        dirs.append(crearMov(orientacion, direc))
+
+        if (direc is Direccion.Arriba):
+            listaCasillas.append(arriba(pos))
+            orientacion = Direccion.Arriba
+
+        if (direc is Direccion.Abajo):
+            listaCasillas.append(abajo(pos))
+            orientacion = Direccion.Abajo
+
+        if (direc is Direccion.Izquierda):
+            listaCasillas.append(izquierda(pos))
+            orientacion = Direccion.Izquierda
+
+        if (direc is Direccion.Derecha):
+            listaCasillas.append(derecha(pos))
+            orientacion = Direccion.Derecha
+    """
+    for d in dirs:
+        print(d)
+    print()
+    solOptima.pop(0)
+    """
+
+    # for d in solOptima:
+    #    print(d)
+    # print()
+    return dirs, solOptima

conexion2.py

-import paho.mqtt.client as mqtt
-import json
+#import paho.mqtt.client as mqtt
+import ujson
+from umqtt.robust import MQTTClient
 
 class Conexion:   
-    
-    def on_message(client,userdata,message):
-        print(f'MENSAGE PERDIDO - Recive Topic:{message.topic} Mensage:{str(message.payload.decode("utf-8"))}')
 
-    def __init__(self, topics:list,on_msg:list):
+
+    def __init__(self, topics:list,on_msg):
 
         
-        with open(file="conexionConfig.json",mode='r') as f:
-            data = json.load(f)
+        with open("conexionConfig.json",'r') as f:
+            data = ujson.load(f)
             self.__broker_address = data['ipbroker']
             self.__port = int(data['portbroker'])
 
@@ -33,27 +32,31 @@ class Conexion:
         #broker_address="192.168.0.19"
         #broker_address="iot.eclipse.org"
         print("creating new instance")
-        self.client = mqtt.Client(mqtt.CallbackAPIVersion.VERSION2)        
+        #self.client = mqtt.Client(mqtt.CallbackAPIVersion.VERSION2)
+        self.client = MQTTClient("RobotPACO",self.__broker_address)
+        self.client.set_callback(on_msg)
+        self.client.connect()
         #client.on_message = on_message
         #client.on_message=on_message #attach function to callback
         #client.on_message = on_msg
 
+        """
         for tp,df in zip(topics,on_msg):
             self.client.message_callback_add(tp,df)
             print(f" Funcion {df} adjunta a {tp}")
         
         print("connecting to broker")
         self.client.connect(self.__broker_address,self.__port) #connect to broker
-
+        """
         for tp in topics:
             self.client.subscribe(tp)
-            print(f" Subscrito a {tp}")
+            print("Subscrito a ",tp)
+            #print(f" Subscrito a {tp}")
 
         #self.__client = self.__connect_mqtt()
         #self.inicializar()
         #self.__client.loop_start()
-        self.client.on_message = self.on_message
-        self.client.loop_start() #start the loop
+        #self.client.loop_start() #start the loop
 
 
         #self.subscribe(cliente=client,topic=self.__topic)
@@ -68,15 +71,11 @@ class Conexion:
         
         #client = mqtt.Client(mqtt.CallbackAPIVersion.VERSION2)        
         #client.connect(self.__broker_address, self.__port)
-        result = self.client.publish(topic,msg)
+        print("Enviar  a topic",topic," mensage ",msg)
+        self.client.publish(topic,msg)
         #client.disconnect()
         #result = self.__client.publish(self.__topic[0],msg)
         # result: [0, 1]
-        status = result[0]
-        if status == 0:
-            print(f"Send `{msg}` to topic `{topic}`")
-        else:
-            print(f"Failed to send message to topic {topic}")   
 
     def desconectar(self):
         self.client.loop_stop()
@@ -84,14 +83,26 @@ class Conexion:
         print("Desconectado")
 
     def desubscribir(self,topic):
-        self.client.unsubscribe(topic)
-        print(f"Desubscrito topic : {topic}")
+        #self.client.unsubscribe(topic)
+        print("Desubscrito topic : ",topic)
+
+    def checkMensages(self):
+        self.client.check_msg()
 
 
 # Función para enviar mensajes MQTT al broker
+
+def publicarOut( topic, msg):
+    # client = mqtt.Client(mqtt.CallbackAPIVersion.VERSION2)
+    # client.connect(self.__broker_address, self.__port)
+    print("Enviar  a topic", topic, " mensage ", msg)
+    client = MQTTClient("PACO", "192.168.48.245")
+    client.connect()
+    client.publish(topic, msg)
+"""
 def send_mqtt_message( topic,message):
-    client = mqtt.Client(mqtt.CallbackAPIVersion.VERSION2)
-    client.connect("192.168.0.134" , 1883)
+    client = MQTTClient("PACO",)
+    client.connect("192.168.0.19" , 1883)
     client.publish(topic, message)
     client.disconnect()
 
@@ -124,3 +135,6 @@ def send_mqtt_message( topic,message):
     #    client = connect_mqtt()
     #    subscribe(client)
     #    client.loop_forever()
+    
+"""
+

main.py

+#!/usr/bin/env pybricks-micropython
+from robot import Robot
+
+robot = Robot()
+#robot.calibraSensorColor()
+robot.realizaPedido()
+
+
+#robot.recorreRuta(mov,cas)
+#robot.recogePaquete()
+
+
+
+
+
+
+
+"""
+def sub_cp(topic, msg):
+    print("Recibido topic", topic.decode(), " mensage ", msg.decode())
+subs = ["A3-467/GrupoL/Robot","A3-467/GrupoL/SincRobot"]
+send = ["A3-467/GrupoL/Interfaz","A3-467/GrupoL/SincInterfaz"]
+
+import mensajeRobot as m
+
+mR = m.MensajeRobot()
+print("Creado")
+print("creada conexion")
+mapa = mR.getMapa()
+print(mapa)
+
+pos = (4,4)
+mR.sendPosicion(pos)
+
+"""
+
+

mensajeRobot.py

+#from typing import Any
+import conexion as cn
+import time
+import ujson
+
+
+
+
+class MensajeRobot:
+    def __init__(self) -> None:
+
+        print("Inicizlizando")
+
+        def sub_cp(topic, msg):
+            print("Recibido topic", topic.decode(), " mensage ", msg.decode())
+            if(topic.decode() == self.topicSubs[-1]):
+                self.sinc = True
+            else:
+                self.mensaje = str(msg.decode())
+
+
+        def on_message_default(client,userdata,message):
+            #print(f'Recive Topic:{message.topic} Mensage:{str(message.payload.decode("utf-8"))}')
+            self.mensaje = str(message.payload.decode("utf-8"))
+            if(self.mensaje.split(self.sepMsg)[0] != self.prefMsg['mapa']):
+                print("EL MAPA EL MAPA EL MAPA")
+
+        def on_message_sinc(client,userdata,message):
+            #print(f'Recive Topic:{message.topic} Mensage:{str(message.payload.decode("utf-8"))}')
+            self.sinc = True            
+
+        #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")))
+            #if(message.topic == self.topicSubs[-1]):
+            #        if(str(message.payload.decode("utf-8")) != str(self.__class__)):
+            #            self.sinc = True
+            #else:
+            #    self.mensaje = str(message.payload.decode("utf-8"))
+
+        with open("conexionConfig.json",'r') as f:
+            data = ujson.load(f)
+
+            #self.topicSubs = ["A3-467/GrupoL/Robot"]
+            self.topicSubs = data['topicSubsRobot']
+            #print(f'Topics a subscribir {self.topicSubs}')
+            #self.topicSend = ["A3-467/GrupoL/Interfaz"]
+            self.topicSend = data['topicSendRobot']
+            #print(f'Topics a enviar {self.topicSend}')
+
+            #self.prefMsg = {"mapa":'map',
+            #                "pedido":'ped',
+            #                "posicion":'pos'}    
+            
+            self.prefMsg = data['prefijoMensajes']
+            self.sepMsg = data['separadorMensaje']
+        
+        defs = [on_message_default,on_message_sinc]
+        self.conex = cn.Conexion(self.topicSubs,on_msg=sub_cp)
+        self.mensaje = ""
+        print("sincronizando")
+        self.sinc = False
+        self.__sincronizacion()
+        #self.sinc = True
+
+
+    def __sincronizacion(self):
+        while(not self.sinc):
+            self.conex.checkMensages()
+            self.conex.publicar(self.topicSend[-1],self.prefMsg['sinc'])
+            time.sleep(2)  
+        print("SINCRONIZADO")
+        self.conex.desubscribir(self.topicSubs[-1])      
+        self.conex.publicar(self.topicSend[-1],self.prefMsg['sinc'])
+
+
+    def getMapa(self):
+        while(self.mensaje.split(self.sepMsg)[0] != self.prefMsg['mapa']):
+            self.conex.checkMensages()
+            time.sleep(2)
+
+        return self.mensaje.split(self.sepMsg)[1]
+    
+    def getPedido(self):
+        
+        self.conex.publicar(self.topicSend[0],self.prefMsg['pedido'] + self.sepMsg)
+
+        while(self.mensaje.split(self.sepMsg)[0] != self.prefMsg['pedido']):
+            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 = []
+        i = 0
+        for c in self.mensaje.split(self.sepMsg)[1]:
+            pos.append(int(c))
+            if( i%2 == 1):
+                ped.append(pos)
+                pos = []
+            i = i+1
+
+        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))
+
+
+
+        
+    
+
+    
+
+
+
+        
\ No newline at end of file

robot.py

+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
+import mensajeRobot as msg
+import camino
+from camino import MovGiro as mov
+
+class Robot:
+    green_rgb = (39, 74, 36)
+    black_rgb = (6, 6, 13)
+    tolerancia = 10
+    contadorNegros = 0
+    valorW = 30
+    w = 0
+    velocidad = 60
+    velocidadNegro = 50
+    negroFlag = False
+    radiusCurve = 20
+    interfono = None
+    mapa = None
+    casillaActual = (6,4)
+    pedido = None
+    casillaSig = None
+
+    def __init__(self):
+        self.ev3 = EV3Brick()
+        left_motor = Motor(Port.A)
+        right_motor = Motor(Port.D)
+        self.pala = Motor(Port.B)
+        self.giroSensor = GyroSensor(Port.S1)
+        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.sendPosicion(self.casillaActual)
+
+
+    #Metodos gestion negros
+    def queNegroEs(self,negro):
+        if negro == 0:
+            return 0
+        if negro % 2 == 0:
+            return 0
+        return 1
+
+    def resetContadorNegros(self):
+        self.contadorNegros = 0
+
+    def incrementaContNegros(self):
+        self.contadorNegros = self.contadorNegros + 1
+
+    #metodos deteccion de colores
+    def is_green(self,rgb):
+        if (rgb[0] >= (Robot.green_rgb[0]-Robot.tolerancia) and rgb[0] <= (Robot.green_rgb[0]+Robot.tolerancia)) and (rgb[1] >= (Robot.green_rgb[1]-Robot.tolerancia) and rgb[1] <= (Robot.green_rgb[1]+Robot.tolerancia)) and (rgb[2] >= (Robot.green_rgb[2]-Robot.tolerancia) and rgb[2] <= (Robot.green_rgb[2]+Robot.tolerancia)):
+            return True
+        return False
+
+    def is_black(self,rgb):
+        if (rgb[0] >= (Robot.black_rgb[0]-Robot.tolerancia) and rgb[0] <= (Robot.black_rgb[0]+Robot.tolerancia)) and (rgb[1] >= (Robot.black_rgb[1]-Robot.tolerancia) and rgb[1] <= (Robot.black_rgb[1]+Robot.tolerancia)) and (rgb[2] >= (Robot.black_rgb[2]-Robot.tolerancia) and rgb[2] <= (Robot.black_rgb[2]+Robot.tolerancia)):
+            return True
+        return False
+
+    def calibraSensorColor(self):
+        while True:
+            print(self.colorSensor.rgb())
+            wait(20)
+
+    #Metodos para el movimiento
+    def giro(self,grados):
+        self.giroSensor.reset_angle(0)
+        self.robot.turn(grados)
+        while (self.giroSensor.angle() != grados):
+            diferencia = self.giroSensor.angle() - grados
+            ajuste = diferencia * -1
+            for i in range(abs(ajuste)):
+                if (ajuste > 0):
+                    self.robot.turn(1)
+                elif (ajuste < 0):
+                    self.robot.turn(-1)
+
+    def giraDerecha(self):
+        self.robot.straight(62) #60
+        self.giro(90)
+        self.sigueRecto()
+
+    def giraIzquierda(self):
+        self.robot.straight(62) #60
+        self.giro(-90)
+        self.sigueRecto()
+
+    def mediaVuelta(self):
+        self.robot.straight(70)
+        self.giro(90)
+        self.giro(90)
+        self.giro(-10)
+
+    def sigueRecto(self, color=green_rgb):
+        # Valores iniciales
+        rgb = self.colorSensor.rgb()
+        negroAnteriorFlag = self.negroFlag
+        contNegroAnterior = self.contadorNegros
+        if self.is_black(rgb):
+            self.negroFlag = True
+        while (((self.queNegroEs(contNegroAnterior) == 1) and (self.queNegroEs(self.contadorNegros) == 0)) == False) or self.contadorNegros == 0:
+
+            #Fase de lectura
+            rgb = self.colorSensor.rgb()
+
+            #Fase de control
+            if self.is_green(rgb):
+                self.w = self.valorW
+                self.velocidad = Robot.velocidad
+                negroAnteriorFlag= self.negroFlag
+                self.negroFlag = False
+
+            elif self.is_black(rgb):
+                negroAnteriorFlag = self.negroFlag
+                self.negroFlag = True
+                self.w = 0
+                self.velocidad = self.velocidadNegro
+
+            else:
+                negroAnteriorFlag= self.negroFlag
+                self.negroFlag = False
+                self.w = -self.valorW
+                self.velocidad = Robot.velocidad
+
+            #Fase de ejecución
+            #Incremento negro al entrar
+            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)
+
+    def recogePaquete(self):
+        self.robot.stop()
+        self.pala.run_target(70,-60)
+
+
+    def soltarPaquete(self):
+        self.robot.stop()
+        self.pala.run_target(70,0)
+
+
+    #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.casillaSig)
+
+    def recorreRuta(self,rutaDirecciones,rutaCoordenadas):
+        for dir,coor in zip(rutaDirecciones,rutaCoordenadas):
+            self.casillaSig = coor
+            self.actualizaCasilla()
+            if dir is 2:
+                self.giraDerecha()
+            elif dir is 0:
+                self.giraIzquierda()
+            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()
+            self.casillaSig = rutaCoordenadas[-1]
+            self.actualizaCasilla()
+
+            #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()
+            self.casillaSig = rutaCoordenadas[-1]
+            self.actualizaCasilla()
+            self.robot.stop()