diff --git a/src/drivers/PiBot/JdeRobotKids.yml b/src/drivers/PiBot/Kibotics.yml similarity index 94% rename from src/drivers/PiBot/JdeRobotKids.yml rename to src/drivers/PiBot/Kibotics.yml index f95886514..a6b87ae30 100644 --- a/src/drivers/PiBot/JdeRobotKids.yml +++ b/src/drivers/PiBot/Kibotics.yml @@ -1,9 +1,11 @@ -JdeRobotKids: +Kibotics: Robot: pibot #pibot, mbot or gazebo Real: Port: /dev/ttyUSB0 Camera: PiCam + Dist_ruedas: 1 + Radio_ruedas: 1 Sim: Motors: diff --git a/src/drivers/PiBot/PiBot.py b/src/drivers/PiBot/PiBot.py new file mode 100644 index 000000000..ab47e93a9 --- /dev/null +++ b/src/drivers/PiBot/PiBot.py @@ -0,0 +1,28 @@ +from jderobot_config import config + + +def dameRobot(): + cfg_file = "Kibotics.yml" + cfg = config.load(cfg_file) + bot = cfg.getProperty('Kibotics.Robot') + bot = bot.lower() + + if bot == "pibot": + # Import real Pibot wrapper + from real import PiBot as pireal + + robot = pireal(cfg) + + if bot == "mbot": + #port = cfg.getProperty('Kibotics.Real.Port') + #robot = MBot() + None + + elif bot == "gazebo": + # Import simulated Pibot wrapper + from gazebo import PiBot as pisim + + robot = pisim(cfg) + + return robot + diff --git a/src/drivers/PiBot/PiBotHAL.py b/src/drivers/PiBot/PiBotHAL.py deleted file mode 100644 index 916acae3a..000000000 --- a/src/drivers/PiBot/PiBotHAL.py +++ /dev/null @@ -1,28 +0,0 @@ -import config - - -def dameRobot(): - cfg_file = "JdeRobotKids.yml" - cfg = config.load(cfg_file) - bot = cfg.getProperty('JdeRobotKids.Robot') - bot = bot.lower() - - if bot == "pibot": - # Import real Pibot wrapper - from .real import PiBot as pireal - - cam = cfg.getProperty('JdeRobotKids.Real.Camera') - robot = pireal(cam) - - if bot == "mbot": - port = cfg.getProperty('JdeRobotKids.Real.Port') - #robot = MBot() - - elif bot == "gazebo": - # Import simulated Pibot wrapper - from .gazebo import PiBot as pisim - - robot = pisim(cfg_file) - - return robot - diff --git a/src/drivers/PiBot/__init__.py b/src/drivers/PiBot/__init__.py index 12440c994..48e08b508 100644 --- a/src/drivers/PiBot/__init__.py +++ b/src/drivers/PiBot/__init__.py @@ -1 +1 @@ -from .PiBotHAL import dameRobot +from .PiBot import dameRobot diff --git a/src/drivers/PiBot/gazebo/piBot.py b/src/drivers/PiBot/gazebo/piBot.py index 3a69e2911..04065bc92 100644 --- a/src/drivers/PiBot/gazebo/piBot.py +++ b/src/drivers/PiBot/gazebo/piBot.py @@ -1,5 +1,5 @@ # -*- coding: utf-8 -*- -from JdeRobotKids import JdeRobotKids +from jderobot_interfaces import Kibotics import numpy import threading import sys @@ -21,7 +21,7 @@ ORANGE_MAX = numpy.array([179, 255, 255],numpy.uint8)#numpy.array([67, 177, 192],numpy.uint8) -class PiBot: +class PiBot(Kibotics): ''' Controlador para el Robot PiBot de JdeRobot-Kids @@ -29,15 +29,16 @@ class PiBot: def __init__(self, cfg): print("En constructor") - cfg = config.load(cfg) + Kibotics.__init__(self) + #cfg = config.load(cfg) #starting comm - jdrc= comm.init(cfg, 'JdeRobotKids.Sim') - self.camera = jdrc.getCameraClient("JdeRobotKids.Sim.Camera") - self.motors = jdrc.getMotorsClient("JdeRobotKids.Sim.Motors") - self.irLeft = jdrc.getIRClient("JdeRobotKids.Sim.IRLeft") - self.irRight = jdrc.getIRClient("JdeRobotKids.Sim.IRRight") - self.us = jdrc.getSonarClient("JdeRobotKids.Sim.Sonar") + jdrc= comm.init(cfg, 'Kibotics.Sim') + self.camera = jdrc.getCameraClient("Kibotics.Sim.Camera") + self.motors = jdrc.getMotorsClient("Kibotics.Sim.Motors") + self.irLeft = jdrc.getIRClient("Kibotics.Sim.IRLeft") + self.irRight = jdrc.getIRClient("Kibotics.Sim.IRRight") + self.us = jdrc.getSonarClient("Kibotics.Sim.Sonar") def moverServo(self, *args): @@ -188,6 +189,12 @@ def dameObjeto(self, lower=ORANGE_MIN, upper=ORANGE_MAX, showImageFiltered=False return center, area + def mostrarImagen(self): + None + + def dameSonarVisual(self): + None + def quienSoy(self): print ("Yo soy un robot simulado PiBot") diff --git a/src/drivers/PiBot/real/piBot.py b/src/drivers/PiBot/real/piBot.py index d2a155c43..73bd84ac6 100644 --- a/src/drivers/PiBot/real/piBot.py +++ b/src/drivers/PiBot/real/piBot.py @@ -1,15 +1,15 @@ # -*- coding: utf-8 -*- -from jderobot_interfaces import JdeRobotKids +from jderobot_interfaces import Kibotics import numpy import threading import sys, time import jderobot_config -import progeo +#import progeo import cv2 import time import math import yaml -import RPi.GPIO as GPIO +#import RPi.GPIO as GPIO from imutils.video import VideoStream import imutils @@ -17,7 +17,7 @@ ORANGE_MIN = numpy.array([0, 123, 165],numpy.uint8)#numpy.array([48, 138, 138],numpy.uint8) ORANGE_MAX = numpy.array([179, 255, 255],numpy.uint8)#numpy.array([67, 177, 192],numpy.uint8) -class PiBot: +class PiBot(Kibotics): ''' Controlador para el Robot PiBot de JdeRobot-Kids @@ -25,24 +25,28 @@ class PiBot: - damePosicionDeObjetoDeColor - dameSonarVisual ''' - def __init__(self, camara): + def __init__(self, cfg): # Libreria RPi.GPIO print("real") import pigpio # Libreria para manejar los servos - JdeRobotKids.__init__(self) + Kibotics.__init__(self) + self._GPIO = GPIO self._tipo = "PiBot" self._dit = pigpio.pi() self._frame = None self._odometry_interval = 0.032 + cam = cfg.getProperty('Kibotics.Real.Camera') + d = cfg.getProperty('Kibotics.Real.Dist_ruedas') + r = cfg.getProperty('Kibotics.Real.Radio_ruedas') + if camara == "PiCam": self._videostream = VideoStream(usePiCamera=True).start() else: self._videostream = VideoStream(usePiCamera=False).start() time.sleep(2) - self.lock = threading.Lock() #Variables de la posicion relativa del PiBot self.posx = 0 #posicion en x @@ -53,9 +57,10 @@ def __init__(self, camara): self.kill_event = threading.Event() #Creo un nuevo hilo que ejecutara el metodo 'readOdometry' - self.hilo = threading.Thread(target=self.readOdometry, args=(self.kill_event,)) + self.hilo = threading.Thread(target=self.readOdometry, args=(self.kill_event,d,r)) self.hilo.start() + def __del__(self): self.fin() @@ -65,16 +70,20 @@ def __init__(self): actual = 0 dif = 0 - def readOdometry(self, kill_event): + + def readOdometry(self, kill_event,d,r): ''' Esta leyendo los angulos de ambos motores llamando a la funcion 'leerangulorueda' y va modificando las variables posx, posy, postheta ''' - config = yaml.load(open('config.yml')) + #config = yaml.load(open('config.yml')) #Constantes: FullCircle = 2 * math.pi #radianes - DistRuedas = float(config['Dist_Ruedas']) #metros - Rrueda = float(config['Radio_Rueda']) #Radio de la rueda + #DistRuedas = float(config['Dist_Ruedas']) #metros + #Rrueda = float(config['Radio_Rueda']) #Radio de la rueda + + DistRuedas = d + Rrueda = r #Pines de los encoders: EncoderL = 6 @@ -99,6 +108,7 @@ def readOdometry(self, kill_event): (angleizq.prev, angledcho.prev) = (angleizq.actual, angledcho.actual) #los angulos actuales son los previos para la siguiente vuelta time.sleep(sample_time) + def getOdometry(self): self.lock.acquire() aux1 = self.posx @@ -107,9 +117,11 @@ def getOdometry(self): self.lock.release() return (aux1, aux2, aux3) + def stopOdometry(self): self.kill_event.set() + def moverServo(self, *args): ''' Función que hace girar al servo motor a un angulo dado como parámetro. @@ -130,6 +142,7 @@ def moverServo(self, *args): p.ChangeDutyCycle(angulo) time.sleep(0.5) + def avanzar(self, vel): ''' Función que hace avanzar al robot en línea recta a una velocidad dada como parámetro. @@ -138,6 +151,7 @@ def avanzar(self, vel): ''' self.move(vel, 0) + def retroceder(self, vel): ''' Función que hace retroceder al robot en línea recta a una velocidad dada como parámetro. @@ -146,6 +160,7 @@ def retroceder(self, vel): ''' self.move(-vel, 0) + def parar(self): ''' Función que hace detenerse al robot. @@ -155,7 +170,9 @@ def parar(self): puertoR = 18 self._dit.set_servo_pulsewidth(puertoL, 1520) #parado 1525 self._dit.set_servo_pulsewidth(puertoR, 1520) #parado 1510 - def girarIzquierda(self): + + + def girarIzquierda(self, vel=None): ''' Función que hace rotar al robot sobre sí mismo hacia la izquierda a una velocidad dada como parámetro. @type vel: entero @@ -163,7 +180,8 @@ def girarIzquierda(self): ''' self.move(0, 1) - def girarDerecha(self): + + def girarDerecha(self, vel=None): ''' Función que hace rotar al robot sobre sí mismo hacia la derecha a una velocidad dada como parámetro. @type vel: entero @@ -171,6 +189,7 @@ def girarDerecha(self): ''' self.move(0, -1) + def moverHasta(self, pos): ''' Avanza o retrocede a una posicion dada @@ -209,12 +228,13 @@ def timeout_excedido(elapsed): #self.fin() self.parar() time.sleep(0.12) + + def girarHasta(self, angle): ''' Gira el robot un angulo determinado en sentido horario o antihorario dependiendo del signo de 'angle' ''' - ErrorRange = 0.2 Timeout = 10 @@ -278,6 +298,7 @@ def alcanzarposicion(angle_aux): self.parar() time.sleep(0.12) + def arcoHasta(self, x, y, theta): ''' Mueve el robot a la posicion relativa (x, y, theta) @@ -355,6 +376,7 @@ def fin(self): self.parar() self.stopOdometry() + def move(self, velV, velW): ''' Función que hace avanzar y girar al robot al mismo tiempo, según las velocidades V,W dadas como parámetro. @@ -395,7 +417,6 @@ def move(self, velV, velW): self._movermotordcho(-velmotorgiro) - def leerIRSigueLineas(self): ''' devuelve el estado de los sensores IR @@ -424,6 +445,7 @@ def leerIRSigueLineas(self): return state + def leerUltrasonido(self): ''' devuelve la distancia a un objeto en metros @@ -451,6 +473,7 @@ def leerUltrasonido(self): return (elapsed * 343) / 2 + def dameImagen (self): ''' devuelve una matriz que se corresponde con una imagen @@ -510,6 +533,7 @@ def dameObjeto(self, lower=ORANGE_MIN, upper=ORANGE_MAX, showImageFiltered=False return center, area + def dameSonarVisual (): ''' Función que devuelve el array de puntos [X,Y] (Z=0) correspondiente al obstáculo detectado @@ -580,17 +604,57 @@ def tipo(self): def tipo(self, valor): self._tipo = valor - #AQUI EMPIEZAN LOS METODOS PRIVADOS - def _esnegativo(self, n): return n < 0 + def _espositivo(self, n): return n > 0 + def _escero(self, n): return n == 0 + + def _pulse_in(self, inp, bit): + ''' + inp: pin digital de entrada + bit: Nivel que quiero testear. 1(nivel alto) o 0(nivel bajo) + + Devuelve el ancho de pulso (en microsegundos) del estado especificado de la señal (alto o bajo) + ''' + + def readuntil(inp, bit): + rec = self._GPIO.input(inp) + if(rec == bit): + #esperar hasta terminar de leer unos + while(rec == bit): + rec = self._GPIO.input(inp) + #leer ceros hasta que me llegue el primer uno + if(bit == 1): + while(rec == 0): + rec = self._GPIO.input(inp) + #ahora me acaba de llegar el primer uno despues de los ceros + else: + while(rec == 1): + rec = self._GPIO.input(inp) + #ahora me acaba de llegar el primer cero despues de los unos + + if(bit != 0 and bit != 1): + return 0 + else: + readuntil(inp, bit) #leo hasta que me llega ese bit + start = time.time() #guardo la hora actual + if(bit == 1): + readuntil(inp, 0) #leo hasta que me llega un bit contrario al anterior + else: + readuntil(inp, 1) + finish = time.time() + elapsed = (finish - start) * 1000000 #tiempo en microsegundos + + return elapsed + + def _movermotorizq(self, vel): ''' Mueve el motor izquierdo a la velicidad dada como parametro @@ -628,6 +692,7 @@ def _movermotorizq(self, vel): else: #velocidad muy rapida self._dit.set_servo_pulsewidth(puertoL, 500) + def _movermotordcho(self, vel): ''' Mueve el motor derecho a la velicidad dada como parametro @@ -665,6 +730,7 @@ def _movermotordcho(self, vel): else: #velocidad muy rapida self._dit.set_servo_pulsewidth(puertoR, 1700) + def _leerangulorueda(self, Encoder): ''' Encoder: encoder del motor cuyo angulo se quiere conocer @@ -739,6 +805,7 @@ def initangle(): return angle + def _leerangulos(self, l, r): ''' l: Encoder del motor izquierdo @@ -748,6 +815,7 @@ def _leerangulos(self, l, r): ''' return (self._leerangulorueda(l), self._leerangulorueda(r)) + def _diferenciaangulos(self, izq, dcho, fullcircle): ''' Recibe dos objetos de la clase 'Angles' y devuelve la diferencia de angulos @@ -769,6 +837,7 @@ def actualesmenor(p, a): return (diferencia(izq.prev, izq.actual), diferencia(dcho.prev, dcho.actual)) + def _calcularposicion(self, difizq, difdcho, rrueda, distruedas): ''' Recibe la diferencia de angulo (en radianes) de cada motor (difizq y difdcho) y @@ -806,6 +875,15 @@ def diferencialtheta(difizq, difdcho): y = d * math.sin(dtheta) guardarposicion(x, y, dtheta) + @property + def tipo(self): + return self._tipo + + + @tipo.setter + def tipo(self, valor): + self._tipo = valor + def _pulse_in(self, inp, bit): ''' inp: pin digital de entrada diff --git a/src/drivers/PiBot/test.py b/src/drivers/PiBot/test.py index 3c1394786..76ebd6d9e 100644 --- a/src/drivers/PiBot/test.py +++ b/src/drivers/PiBot/test.py @@ -1,7 +1,7 @@ #!/bin/python2 import sys, time -import PiBot +import PiBot if __name__ == "__main__": diff --git a/src/interfaces/python/Kibotics.py b/src/interfaces/python/Kibotics.py new file mode 100644 index 000000000..063616994 --- /dev/null +++ b/src/interfaces/python/Kibotics.py @@ -0,0 +1,74 @@ +#(c) JdeRobot-kids 2018 + +# -*- coding: utf-8 -*- +import abc +from abc import ABCMeta + + +class Kibotics(object): + __metaclass__ = ABCMeta + + @abc.abstractmethod + def quienSoy(self): + print ("Soy un tipo de robot abstracto") + + @abc.abstractmethod + def avanzar(self, vel): + print ("Avanzar abstracto") + + @abc.abstractmethod + def retroceder(self, vel): + print ("Retroceder abstracto") + + @abc.abstractmethod + def parar(self): + print ("Parar abstracto") + + @abc.abstractmethod + def girarIzquierda(self, vel): + print ("Girar izquierda abstracto") + + @abc.abstractmethod + def girarDerecha(self, vel): + print ("Girar derecha abstracto") + + @abc.abstractmethod + def move(self, velV, velW): + print ("Move abstracto") + + @abc.abstractmethod + def leerUltrasonido(self): + print ("leerUltrasonido abstracto") + + @abc.abstractmethod + def leerIRSigueLineas(self): + print ("leerIRSigueLineas abstracto") + + @abc.abstractmethod + def moverServo(self, *args): + print ("moverServo abstracto") + + def get_tipo(self): + pass + + def set_tipo(self, valor): + pass + + tipo = abc.abstractproperty(get_tipo, set_tipo) + + @abc.abstractmethod + def dameImagen(self): + print ("dameImagen abstracto") + + @abc.abstractmethod + def mostrarImagen(self): + print ("mostrarImagen abstracto") + + @abc.abstractmethod + def dameSonarVisual (self, image, cameraModel): + print ("dameSonarVisual abstracto") + + @abc.abstractmethod + def dameObjeto(self, lower, upper, showImageFiltered): + print ("dameObjeto abstracto") +