Créer ses propres primitives python et les ajouter au moniteur

Comment créer et manipuler les primitives de pypot pour les robots poppy ?

demo.py (28.9 KB)

Exemple appliquer : le mode demo du Poppy ErgoJr

pré-requis:

connaitre l’utiilsation de:
my_robot.motors
my_robot.mX.compliant
my_robot.mX.goto_position
my_robot.mX.led
my_robot.mX.present_position
my_robot.posture_is
avoir quelque notion des class et objet en python

les primitives:

Tinsel(Primitive):
ici on veut que l’ergo glignote, qu’on puisse lui spécifier les couleurs et la
vitesse (en Hz)

BaseDemo(Primitive):
je définit ma class BaseDemo, fille de la class Primitive, et futur mère :wink:

Tetu(BaseDemo, Primitive):
Ici, je veux créer un comportemant “de type tétu” autrement dit, quelqu’un qui
revient toujours sur ses potitions. Le comportement est définit comme suit:

  • si je suis dans ma position de départ, je me met en vert et attend
  • si je ne suis plus dans ma position de départ, je devient rouge et reprend
    ma position
    Quelque subtilité, sont ajouter ensuite pour affiner le comportement

Poule(BaseDemo, Primitive):
couple les 3 moteurs inférieur aux 3 moteurs supérieur tel une poule fixant l’horizon :wink:

ProgByDemo(BaseDemo, Primitive):
séquence alternant enregistrement et replay

Demos(BaseDemo, Primitive):
ordonnanceur permettant de sélectionner une primitive parmi les demo dispos (en fonction de la couleur affiché)

Tournesol(BaseDemo, Primitive):
quand il fait sombre, Ergo se recroqueville … quand il y a beaucoup de lumière, il se dresse vers le haut!

code source :

demo.py (28.9 KB)

# coding: utf-8
'''
# Comment créer et manipuler les primitives de pypot pour les robots poppy ?

## Exemple appliquer : le mode demo du Poppy ErgoJr

pré-requis:
connaitre l'utiilsation de:
my_robot.motors
my_robot.mX.compliant
my_robot.mX.goto_position
my_robot.mX.led
my_robot.mX.present_position
my_robot.posture_is

+ avoir quelque notion des class et objet en python

'''
'''
Tout d'abord quelque import
'''

import time
import random
import cv2
import numpy as np

from pypot.primitive import Primitive

'''
On apprend dans le fichier ~/poppy_src/pypot/primitive/primitive.py que:
- les primitives sont des 'objet'
- qu'elle hérite leurs attribue, méthode et propriéte de la class Primitives
- qu'il suiffit de compléter la fonction run()
- que la commande pour lancer la primitive est: name_of_my_primitive.start()
- que la commande pour lancer la primitive est: name_of_my_primitive.stop()
- mais qu'il faut d'abord "l'attacher au robot" via la commande: my_robot.attach_primitive(MyPrimitive(self.robot), 'name_of_my_primitive')

## Exemple la primitives Tinsel (girlande):
'''


class Tinsel(Primitive): #on crée un nouvelle class Tinsel qui hérite de la class Primitive
    '''
    ici on veut que l'ergo glignote, qu'on puisse lui spécifier les couleurs et la vitesse (en Hz)
    
    - Toute les fonctions qui ne sont pas rédéfinit ici sont hérité de la class Primitive
    - La fonction __init__() est exécute des que la class est instancier
    - Ici nous souhaitons ajouter deux variables non définit dans la class Primitive: les couleurs et la vitesse
    '''
    def __init__(self,robot,colors=['green','red','yellow','blue','pink','cyan','white','off'],freq=2): 
        #on redéfinit la fonction __init__() hérité de Primitive, et lui ajoute deux parametre: colors et freq
        Primitive.__init__(self,robot) 
        #on appel la fonction __init__() de la class Primitive (pour que les autres variables et parametre de celle-ci s'initie correctement)        
        self.colors=colors
        self.freq=freq
        #on définit l'attribut self.colors et self.freq (qui peuvent être vue comme des variables) qui contiennent respectivement les couleurs spécifier dans le parametre colors et la vitesse spécifier dans le parametre freq passer lors l'instanciation de la primitive
        
    def run(self):
        #on définit la fonction run()
        while not self.should_stop():
            #tant que l'on ne demande pas d'arréter la primitive
            for color in self.colors:
                #pour chacune des couleurs
                if not self.should_stop():
                    #si l'on ne demande pas d'arréter la primitives
                    if self.should_pause(): self.wait_to_resume()
                    #si on me demande pause, j'attend jusqu'a reprise
                    for m in self.robot.motors: m.led = color 
                    #pour chaque moteurs, mettre la led en la couleur
                    for i in range(5):
                        if not self.should_stop():
                            if self.should_pause(): self.wait_to_resume()                                               
                            time.sleep((1./self.freq)/5)
                    #je vérife dix foi par période si l'on ne demande pas d'arréter la primitives

'''
###Testons la primitives:
'''
'''
from poppy.creatures import PoppyErgoJr
poppy= PoppyErgoJr()
poppy.attach_primitive(Tinsel(poppy,['yellow','white'],2), 'girlande')
girlande.start()
time.sleep(5)
girlande.stop()
'''

'''
## Maintenant je souhaite créer plusieurs primitives qui ont des points commun.
Je vais créer une class mère (héritant de primitive) qui contiendras ces points commun.

dans le fichier ~/poppy_src/pypot/primitive/primitive.py , j'ai également appris que la class primitive exploiter la lib "thread". On apprend que chacune des primitives à un setup() (phase de lancement) un teardown() (phase d'extinction) et un run()

'''


class BaseDemo(Primitive):
    #je définit ma class BaseDemo, fille de la class Primitive, et futur mère ;)
    def __init__(self, robot):
        #je définit ma fonction __init__() 
        Primitive.__init__(self, robot)
        #j'apel la fonction __init__ de ma class mère
        
        self.defaut_posture = [ 0, -45, 15, 0, 55, -5 ] #[ 0, -50, 35, 0, 35, 30 ]
        #je définit un nouveau attribue
        
    def posture_is(self, posture, error=2, motors='all'):
        if motors == 'all' : motors= self.robot.motors
        for i in range(len(motors)):
            if abs( motors[i].present_position - posture[i] ) > error : return False
        return True
    
    def distance_with(self, posture, motors='all'):
        #retourne une liste contenant la difference en degres (pour chaque moteur) entre la position actuelle du robot et une position donnee
        if motors == 'all': motors= self.robot.motors
        output= [ abs( motors[i].present_position - posture[i] ) for i in range(len(motors)) ]
        '''
        autre ecriture:
        output=[]
        for i in range(len(motors)):
            output.append( abs( motors[i].present_position - posture[i] )            
        '''
        return output    
    
    def sleep_except_should_stop(self, time_seconde):
        for i in range (int(time_seconde*10)):
            if not self.should_stop(): time.sleep(time_seconde/(time_seconde*10))
            #si l'on ne demande pas d'arréter la primitive, on attend
            
    def countdown(self, time_seconde):
        for i in range(int(time_seconde+time_seconde*0.2)):
            
            if not self.should_stop():
                #si l'on ne demande pas d'arréter la primitive
                if self.should_pause(): self.wait_to_resume()
                #si on me demande pause, j'attend jusqu'a reprise
                
                for m in self.robot.motors: m.led="off"
                time.sleep( 0.5- (0.3/(time_seconde+time_seconde*0.2) *i ))
                for m in self.robot.motors: m.led="green"
                time.sleep(0.5)
        #je créer une sorte de compte a reboug lumineux
            
    def setup(self):
        #je définit la fonction setup()
        duration= int( max( self.distance_with( self.defaut_posture ) ) ) /100.
        #temps en fonction de la distance maximal actuel ; soit une distance max (thèorique) de 180° exécuter en un temps de 1,8 seconde ; connaissance ma position_défaut la distance max est 150° (avec m2) donnant un temps max de 1,5 secondes pour reprendre sa positon. on à donc le temps qui varie en fonction de la distance, c'est donc la vitesse qui est fixe. V=D/T ; 150/1,5=100 ; le moteur le plus éloigner de sa postion defaut se déplace à 100°/sec ; les autre moteurs ajuste leur vitesse (ralentissent) pour arriver à leur position defaut en même temps que le moteur le plus éloigné.
        for m in self.robot.motors: 
            m.compliant=False
            m.goto_position( self.defaut_posture[m.id-1] , duration )
        time.sleep( duration + 0.5 )
        #déplacer les moteurs vers leur position defaut (définit dans self.__init__)
        for m in self.robot.motors:
            m.compliant=True
            m.led='off'
        #mettre tout les moteurs en "souple" et éteindre les led
    
    #je ne définit pas de fonction run() ; elle sera définit dans mes class filles
               
    def teardown(self):
        #je définit la fonction teardown()
        BaseDemo.setup(self)
        #j'appel BaseDemo.setup() : le robot éffectue la même action qu'au démarage
    

'''
### Maintenant je vais créer une nouvelle primitive qui héritera de la class BaseDemo et de la class Primitive.
'''

                    
class Tetu(BaseDemo, Primitive):
    '''
    Ici, je veux créer un comportemant "de type tétu" autrement dit, quelqu'un qui revient toujours sur ses potitions. Le comportement est définit comme suit:
    - si je suis dans ma position de départ, je me met en vert et attend
    - si je ne suis plus dans ma position de départ, je devient rouge et reprend ma position
    
    Quelque subtilité, sont ajouter ensuite pour affiner le comportement
    '''
    
    #je n'est pas besoin de définir d'autre chose durant initialisation, je ne définit pas la fonction __init__()
    #la fonction BaseDemo.__init__() sera la fonction exécuté par défaut
    
    def setup(self):
        #je définit la fonction setup()
        BaseDemo.setup(self)
        #j'apel la fonction setup() de ma class mère
        
        sleep=5
        for i in range(int(sleep+(sleep/5))):            
            for m in self.robot.motors: m.led="off"
            time.sleep(0.5-((0.5/sleep)*i))
            for m in self.robot.motors: m.led="green"
            time.sleep(0.5)
        #je créer une sorte de compte a reboug lumineux
            
        self.start_pos= [round(m.present_position) for m in self.robot.motors]
        #je sauvegarde la position de départ choisie.

    def run(self):
        #on définit la fonction run
        while not self.should_stop():
            #si l'on ne demande pas d'arréter la primitive
            if self.should_pause(): self.wait_to_resume()
            #si on me demande pause, j'attend jusqu'a reprise
            
            if not (self.posture_is(self.start_pos,6)) :
                #si ma position n'est pas ma position de départ
                
                for m in self.robot.motors: m.led = 'yellow'
                #je deviens jaune
                
                #__________________________________________________________________________
                
                for t in range(10):
                        if not self.should_stop():
                            if self.should_pause(): self.wait_to_resume()
                            time.sleep(0.1)
                #j'attend 1 secondes sauf si l'on ne demande pas d'arréter la primitive
                    
                t=0
                while self.posture_is(self.start_pos,45) and t<200 and not(self.should_stop()):
                    if self.should_pause(): self.wait_to_resume()
                    time.sleep(0.01)
                    t+=1
                #tant que ma position n'est pas trop éloigner de ma position de départ, et qu'il s'est écoulé moins de 2 secondes, et que l'on ne demande pas d'arréter la primitive (ou de mettre en pause) ; j'attend
                
                if not self.should_stop():
                    #si l'on ne demande pas d'arréter la primitive
                    if self.should_pause(): self.wait_to_resume()
                    #si on me demande pause, j'attend jusqu'a reprise
                    
                    if not (self.posture_is(self.start_pos,6)):
                        #si on ne ma pas remis dans la bonne position alors...

                        for m in self.robot.motors: 
                            m.led = 'red'
                            m.compliant=False
                            m.goto_position(self.start_pos[m.id-1],0.5)
                        #...je reprend ma postion de départ en mallumant en rouge et...

                        #________________________________________________________________________________
                        for t in range(15):
                            if not self.should_stop():
                                if self.should_pause(): self.wait_to_resume()
                                time.sleep(0.1)
                        #... j'attend 1,5 secondes sauf si l'on ne demande pas d'arréter la primitive, puis...
                    
                    for m in self.robot.motors: 
                        m.led = 'green'  
                        m.compliant=True
                    #...je redeviens vert et manipulable.
                    
            time.sleep(0.01)
            #je vérifie tout les 0.01 seconde si j'ai bouger

'''
### Maintenant je vais créer une nouvelle primitive qui héritera de la class BaseDemo et de la class Primitive.
'''

class Poule(BaseDemo, Primitive):
    
    def setup(self):
        BaseDemo.setup(self)
               
        self.latence=0.5
        self.local_time=0
        for m in self.robot.tip: 
            m.led='red'
            m.compliant=False
        for m in self.robot.base:
            m.led='green'
            m.compliant=True

    def run(self):
        while not self.should_stop():
            if self.should_pause(): self.wait_to_resume()

            #self.local_time=time.time()
            #for i in range(3): self.robot.motors[i+3].goto_position(-(self.robot.motors[i].present_position),self.latence)
            for i in range(3): self.robot.motors[i+3].goto_position(-(self.robot.motors[i].present_position),0.1)
            #self.latence=  ( ( (time.time()-self.local_time) +0.001 ) + self.latence )/2.
            #self.latence=   ( (time.time()-self.local_time) + self.latence )/2.
            time.sleep(0.001)            

'''
d'autre exemples
'''

class PouleInv(BaseDemo, Primitive):
    
    def setup(self):
        BaseDemo.setup(self)

        self.latence=0.5
        self.local_time=0
        for m in self.robot.base:
            m.led='red'
            m.compliant=False            
        for m in self.robot.tip:
            m.led='green'
            m.compliant=True            

    def run(self):
        while not self.should_stop():
            if self.should_pause(): self.wait_to_resume()

            self.local_time=time.time()
            for i in range(3): self.robot.motors[i].goto_position(-(self.robot.motors[i+3].present_position),self.latence)
            #self.latence= ( ( (time.time()-self.local_time) +0.001 ) + self.latence )/2.
            self.latence=  ( (time.time()-self.local_time) + self.latence )/2.
            time.sleep(0.001)            

'''
### Maintenant nous parler du "primitive_manager"
lorsque plusieurs primitives s'éxécute en même temps, il combine les différent input pour produire un unique output vers le robot.
un certaine ordre héracchique est produit lorsque que l'on appelle une primitive depuis une primitive:
ici, nous allons utilise les primitives MoveRecorder et MovePlayer. Lors de leur utilisation il faudras déangager notre primitive du "primitive_manager" au profit des primitives MoveRecorder et MovePlayer.
'''
from pypot.primitive.move import MoveRecorder
from pypot.primitive.move import MovePlayer

class ProgByDemo(BaseDemo, Primitive):
    
    def __init__(self, robot, record_time=10):
        BaseDemo.__init__(self, robot)
        self.robot.attach_primitive(Tinsel(self.robot), 'tinsel')
        self.record_time=record_time # en seconde
       
        
    def run(self):
        while not self.should_stop():
            if self.should_pause(): self.wait_to_resume()

            
            while not(self.posture_is(self.defaut_posture)): time.sleep(0.1)
            for m in self.robot.motors: 
                m.led="green"
                m.compliant=True
            
            my_record = MoveRecorder(self.robot, 50, self.robot.motors)
            my_record.start()
                       
            #_________________________________________________________________________________________
            sleep = int(self.record_time+(self.record_time/5.))
            for i in range(sleep):
                if not self.should_stop():
                    if self.should_pause(): self.wait_to_resume()
                    for m in self.robot.motors: m.led="green"
                    time.sleep(0.5)
                    for m in self.robot.motors: m.led="off"
                    time.sleep(0.5-((0.5/sleep)*i))

            my_record.stop()
            #my_record.wait_to_stop()
            
            if not self.should_stop():
                if self.should_pause(): self.wait_to_resume()
                
                self.robot.tinsel.colors=['red']
                self.robot.tinsel.freq=1
                self.robot.tinsel.start()
                
                self.robot._primitive_manager.remove(self) 
                # patch pour passer des fake moteurs de la primitive ProgByDemo aux moteurs reel de l ergo
                
                my_play = MovePlayer(self.robot, my_record.move)
                my_play.start()
                
                for t in range(self.record_time*2):
                    if not self.should_stop():
                        if self.should_pause(): self.wait_to_resume()
                        time.sleep(0.5)
                        
                if my_play.is_alive(): my_play.stop()
                    
                self.robot.tinsel.stop()
                
                self.robot._primitive_manager.add(self) # on redonne le control des moteurs a la primitive ProgByDemo
                
                duration= int( max( self.distance_with( self.defaut_posture ) ) )/100.
                for m in self.robot.motors: 
                    m.compliant=False
                    m.goto_position(self.defaut_posture[m.id-1],duration)
                time.sleep(duration)

'''
### Maintenant parlons jeux
Nous allons créer des méthode suplémentaire pour facilité l'interaction
Ici, nous fabriquon une sorte de Puzzel
'''
            
class Puzzel(BaseDemo, Primitive):
    
    methods = ['start', 'stop', 'pause', 'resume',
               'level_veryeasy', 'level_easy','level_noramle','level_hard','level_veryhard','level_growing',
               'precision_easy','precision_normal','precision_hard']
    
    def __init__(self, robot, level=0, precision=20):
        BaseDemo.__init__(self, robot)
        self.level=level
        self.precision=precision
        self.set_manual=False
        self.fig=[]
        
    
    def level_growing(self):
        self.set_manual=False
        self.level=0
    def level_veryeasy(self):
        self.set_manual=True
        self.level=1
    def level_easy(self):
        self.set_manual=True
        self.level=2    
    def level_normal(self):
        self.set_manual=True
        self.level=3    
    def level_hard(self):
        self.set_manual=True
        self.level=4
    def level_veryhard(self):
        self.set_manual=True
        self.level=5
     
    def precision_easy(self): self.precision=20    
    def precision_normal(self): self.precision=10
    def precision_hard(self): self.precision=5
        
    def new_fig(self,level=0):       
        var=[-75, -45, 0, 0, 45, 75]        
        m1=m4=m6=0       
        fig=[
            [m1, 0, -90, m4, 45, m6],
            [m1, 0, 90, m4, -45, m6],
            [m1, 0, 0, m4, -90, m6],
            [m1, 0, 0, m4, 90, m6],
            [m1, 45, -90, m4, 0, m6],
            [m1, 45, -90, m4, 90, m6],
            [m1, -45, 90, m4, 0, m6],
            [m1, -45, 90, m4, -90, m6]]  
        
        if level==0:
            fig=fig[0]
        elif level==1:
            fig=fig[0]
            m = random.sample(var,1)[0]
            fig[0]=m
            fig[3]=m
            fig[5]=m            
        elif level==2: 
            fig=random.sample(fig,1)[0]
            m = random.sample(var,1)[0]
            fig[0]=m
            fig[3]=m
            fig[5]=m            
        elif level==3:
            fig=random.sample(fig,1)[0]
            m = random.sample(var,2)
            fig[0]=m[0]
            fig[3]=m[1]
            fig[5]=m[0]
        elif level==4:
            fig=random.sample(fig,1)[0]
            m = random.sample(var,3)
            fig[0]=m[0]
            fig[3]=m[1]
            fig[5]=m[2]
        else:
            fig=random.sample(fig,1)[0]
            m = [ random.randrange(-75,75) for i in range(3) ]
            fig[0]=m[0]
            fig[3]=m[1]
            fig[5]=m[2]     
        return fig      

    def run(self):
        while not self.should_stop():
            if self.should_pause(): self.wait_to_resume()
            
            if self.set_manual==True: self.fig.append(self.new_fig(self.level))
            else: self.fig.append(self.new_fig(len(self.fig)))
            
            duration= int( max( self.distance_with( self.fig[-1] ) ) )/100.
            for m in self.robot.motors:
                m.led='red'
                m.compliant=False
                m.goto_position(self.fig[-1][m.id-1],duration)
            
            #____________________________________________________
            for i in range(10): #time to show 5sec
                if not self.should_stop():
                    if self.should_pause(): self.wait_to_resume()
                    time.sleep(0.5)
            
            for m in self.robot.motors: m.goto_position(self.defaut_posture[m.id-1],1)
            while not(self.posture_is(self.defaut_posture)): time.sleep(0.1)
            
            for m in self.robot.motors: 
                m.compliant=True
                m.led='green'
            
                        
            time_search=10
            sleep = int(time_search+(time_search/5.))
            for i in range(sleep):
                if not self.should_stop():
                    if self.should_pause(): self.wait_to_resume()
                    for m in self.robot.motors: m.led="green"
                    time.sleep(0.5)
                    for m in self.robot.motors: m.led="off"
                    time.sleep(0.5-((0.5/sleep)*i))
            
            if not self.should_stop():
                if self.should_pause(): self.wait_to_resume()
                    
                if self.posture_is(self.fig[-1],self.precision):
                    for m in self.robot.motors: m.led='green'
                    time.sleep(0.5)
                    for m in self.robot.motors: m.led='off'
                    for i in range(len(self.fig)):
                        self.robot.motors[i].led='green'
                        time.sleep(0.25)
                else:
                    for m in self.robot.motors: 
                        m.led='red'
                        m.compliant=False
                        m.goto_position(self.fig[-1][m.id-1],1)
                    for i in range(4): #time to show 2sec
                        if not self.should_stop():
                            if self.should_pause(): self.wait_to_resume()
                            time.sleep(0.5)
                    self.fig=[]

                if len(self.fig)==6:                    
                    for m in self.robot.motors: m.led='off'
                    for i in range(len(self.fig)):
                        if not self.should_stop():
                            if self.should_pause(): self.wait_to_resume()
                            duration= int( max( self.distance_with( self.fig[i] ) ) )/100.
                            for m in self.robot.motors:
                                m.compliant=False
                                m.goto_position(self.fig[i][m.id-1],duration)
                            for l in range(i): self.robot.motors[i].led='green'
                            time.sleep(duration)
                                
                    self.robot._primitive_manager.remove(self) 
                    # patch pour passer des fake moteurs de la primitive ProgByDemo aux moteurs reel de l ergo
                    self.robot.dance.start()
                    #_________________________________________________________________________________________
                    for i in range(10): #time to dance 5sec
                        if not self.should_stop():
                            if self.should_pause(): self.wait_to_resume()
                            time.sleep(0.5)
                    self.robot.dance.stop()
                    self.robot.dance.wait_to_stop()
                    
                    self.robot._primitive_manager.add(self) 
                    # patch pour passer des fake moteurs de la primitive ProgByDemo aux moteurs reel de l ergo
                    self.fig=[]

                for m in self.robot.motors: 
                    m.compliant=False
                    m.led='off'
                    m.goto_position(self.defaut_posture[m.id-1],1)
                while not(self.posture_is(self.defaut_posture)): time.sleep(0.1)

'''
Enfin, regroupons notre travail
'''               
                    
class Demos(BaseDemo, Primitive):
    methods = ['start', 'stop', 'pause', 'resume', 'press_button']
    def __init__(self, robot):
        BaseDemo.__init__(self, robot)
        
        self.robot.attach_primitive(Tetu(self.robot), 'tetu')
        self.robot.attach_primitive(Poule(self.robot), 'poule')
        self.robot.attach_primitive(PouleInv(self.robot), 'poule_inv')
        self.robot.attach_primitive(ProgByDemo(self.robot), 'prog_by_demo')
        self.robot.attach_primitive(Puzzel(self.robot), 'puzzel')
        
        self.robot.attach_primitive(Tournesol(self.robot), 'tournesol')
               
        self.match={'white':self.robot.tetu,
                    'blue':self.robot.poule,
                    'cyan':self.robot.poule_inv, # a remplacer par tracking octave
                    #'yellow':self.robot.puzzel, # a remplacer par tournesol
                    'yellow':self.robot.tournesol,
                   'pink':self.robot.prog_by_demo 
                   }        
        """
        prefer use:
        red color for not_compliant_motors
        green color for compliant_motors
        
        """       
        self.robot.attach_primitive(Tinsel(self.robot), 'tinsel')        
        self.robot.tinsel.colors= [key for key in self.match.keys()]
        random.shuffle(self.robot.tinsel.colors)        
        self.robot.tinsel.freq= 1
                
        self.button=0
    
    def press_button(self):        
        self.button+=1
        return 'have press '+str(self.button)+'x'
    
    def reset_button(self):
        self.button=0
        return 'reset button'         

    def run(self):
        while not self.should_stop():
            if self.should_pause(): self.wait_to_resume()
                
            old_b_val=self.button
            self.robot.tinsel.colors= [key for key in self.match.keys()]
            random.shuffle(self.robot.tinsel.colors)
            self.robot.tinsel.freq= 1
               
            self.robot.tinsel.start()          
            while self.button==old_b_val and not self.should_stop():
                if self.should_pause(): self.wait_to_resume()
                time.sleep(0.01)
               
            color=self.robot.motors[0].led
            self.robot.tinsel.stop()
            
            old_b_val=self.button
            if not self.should_stop():
                
                self.robot._primitive_manager.remove(self)
               
                self.match[color].start()
                while self.button==old_b_val and not self.should_stop():
                    if self.should_pause(): self.wait_to_resume()
                    time.sleep(0.01)
                self.match[color].stop()
                
                self.robot._primitive_manager.add(self)
                
                
'''
ADD demo tournesol
'''
class Tournesol(BaseDemo, Primitive):
    '''
    Ici, je veux créer un comportemant "de type ...
    '''    
    #je n'est pas besoin de définir d'autre chose durant initialisation, je ne définit pas la fonction __init__()
    #la fonction BaseDemo.__init__() sera la fonction exécuté par défaut
    
    def setup(self):
        #je définit la fonction setup()
        BaseDemo.setup(self)
        #j'apel la fonction setup() de ma class mère
        for m in self.robot.motors: 
            m.led='white'
            m.compliant=False
            self.robot.m1.goto_position(0,1)
            self.robot.m4.goto_position(0,1)

    def run(self):
        #on définit la fonction run
        while not self.should_stop():
            #si l'on ne demande pas d'arréter la primitive
            if self.should_pause(): self.wait_to_resume()
            #si on me demande pause, j'attend jusqu'a reprise
    
            for i in range(5):
                alpha=[]
                img = self.robot.camera.frame
                gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
                hist = cv2.calcHist([gray],[0],None,[256],[0,256])
                #lum.append(((sum(hist[90:150])/10000)))
                #plt.hist(gray.ravel(),256,[0,256])
                #plt.title('Histogram for gray scale picture')
                #plt.show()
                alpha.append((sum([np.percentile(hist,i)/1000 for i in range(90,100)])-30)/55)
                time.sleep(0.1)
                
            alpha= np.mean(alpha)
            self.robot.m2.goto_position(-5-alpha*10,0.5)
            self.robot.m3.goto_position(-45+alpha*20,0.5)
            self.robot.m5.goto_position(0+alpha*90,0.5)
            self.robot.m6.goto_position(-45+alpha*80,0.5)

            alpha=int(alpha*4)
            if alpha == 4: 
                for m in self.robot.motors: m.led='white'
            else: 
                for m in self.robot.motors[:3]: m.led='green'
            if alpha == 3: 
                for m in self.robot.motors[3:]: m.led='green'
            elif alpha == 2: 
                for m in self.robot.motors[3:]: m.led='yellow'
            elif alpha == 1: 
                for m in self.robot.motors[4:]: m.led='pink'
                self.robot.m4.led='yellow'
            elif alpha == 0: 
                self.robot.m4.led='yellow'
                self.robot.m5.led='pink'
                self.robot.m6.led='red'