3615

import random
import time
from pypot.dynamixel.conversion import XL320LEDColors

#------------------------------------------------------
# CREATION DU ROBOT
#------------------------------------------------------

from pypot.creatures import PoppyErgoJr
poppy = PoppyErgoJr()

#------------------------------------------------------
# INITIALISATION DU ROBOT
#------------------------------------------------------

for m in poppy.motors :  # Boucle sur les six moteurs
    m.compliant = False
    m.led = 'green'
position_1 = {'m1': 0, 'm2': -110, 'm3': 66, 'm4': 0, 'm5': 34, 'm6': 20}  # safe_power_up
poppy.goto_position(position_1, 2, wait=True)  # 2 secondes pour prendre la position

#------------------------------------------------------
# FONCTION animation
#------------------------------------------------------

def animation(moteur, angleMin, angleMax, forceMax) :
    moteur.goal_position = random.randint(angleMin,angleMax)
    if abs(moteur.present_load) > forceMax :
        print (moteur.present_load)
        for m in poppy.motors :
            m.compliant = True
            m.led = 'off'
        for i in range(5) :
            moteur.led = 'red'
            time.sleep(0.5)
            moteur.led = 'off'
            time.sleep(0.5)
        time.sleep(1)

#------------------------------------------------------
# BOUCLE
#------------------------------------------------------

while 1 :
    for m in poppy.motors :
        m.moving_speed = random.randint(5,20)  # 5,20 à 5,30
        m.compliant = False
        m.led = XL320LEDColors(random.randint(2,8)).name
    animation(poppy.m1,-90,90,50)
    animation(poppy.m2,-110,0,30)
    animation(poppy.m3,0,60,30)
    animation(poppy.m4,-90,90,30)
    animation(poppy.m5,-40,40,20)
    animation(poppy.m6,-60,60,10)
    if random.randrange(4) == 1 :
        time.sleep(random.randrange(5))