tri avec un seul robot

import pypot
import time
%pylab inline
import cv2
import numpy as np 
import matplotlib.pyplot as plt
#initialisation de l'ergo junior
from poppy_ergo_jr import PoppyErgoJr
ergo = PoppyErgoJr()

approche={'m1':-90,'m2': -70}
prend1={'m1':0}
prend2={'m2':-5,'m3':21 ,'m4':0,'m5': 45,'m6':20}
leve={'m1':0,'m2': 0,'m3':0, 'm4':0,'m5':0,'m6':0}
vert={'m1':90,'m2': -20,'m3':70, 'm4':3,'m5':-70}
jaune={'m1':60,'m2': -20,'m3':70, 'm4':3,'m5':-70}
orange={'m1':-30,'m2': -20,'m3':70, 'm4':3,'m5':-70}
rouge={'m1':30,'m2': -20,'m3':70, 'm4':3,'m5':-70}
magenta={'m1':-60,'m2': -20,'m3':70, 'm4':3,'m5':-70}
cyan={'m1':-90,'m2': -20,'m3':70, 'm4':3,'m5':-70}
lache={'m6':20}
ergo.compliant = False
       
for i in range(1,15):
    #traitement d'image
    img=ergo.camera.frame
    time.sleep(0.5)
    print("cam"),
    roi_color = img[200:280, 180:360]
    cv2.imwrite("toto.jpg",roi_color)
    hsvt = cv2.cvtColor(roi_color, cv2.COLOR_BGR2HSV)
    h,s,v = cv2.split(hsvt)
    b=np.max(h)      
    if b>=0 and b<=360:
        if b<15 or b>=178:
            couleur="rouge"            
        if b>=15 and b<28:
            couleur="orange"            
        if b>=28 and b<38:
            couleur="jaune"            
        if b>=38 and b<60:
            couleur="vert"            
        if b>=60and b<62:
            couleur="vert"           
        if b>=63 and b<105:
            couleur="cyan"            
        if b>=105 and b<161:
            couleur="bleu"           
        if b>161 and b<178:
            couleur="magenta"  
    #prise de balle sur la rampe
    ergo.m1.goal_position=0
    time.sleep(0.5)
    ergo.goto_position(prend1,3,wait=True)
    ergo.goto_position(prend2,2,wait=True)
    
    ergo.m6.goal_position=-5
    time.sleep(3)
    ergo.goto_position(leve,3,wait=True)
    
    ergo.m3.goal_position=-40
    t=couleur
    print(couleur),
    print(b),
    #balle dans le verre en fonction de sa couleur    
    if(t=="vert"):        
        ergo.goto_position(vert,2,wait=True)
    if(t=="jaune"):        
        ergo.goto_position(jaune,2,wait=True)
    if(t=="orange"):        
        ergo.goto_position(orange,2,wait=True)
    if(t=="magenta"):        
        ergo.goto_position(magenta,2,wait=True)
    if(t=="rouge"):        
        ergo.goto_position(rouge,2,wait=True)
    if(t=="cyan"):        
        ergo.goto_position(cyan,2,wait=True)
    ergo.goto_position(lache,1,wait=True)
    time.sleep(3)
    ergo.goto_position(leve,1,wait=True)
    
    print(i)