import time
for m in poppy.motors: m.led="off"
asendid=[m.present_position for m in poppy.motors]
poppy.rest_posture.start()
time.sleep(5)
for m in poppy.motors:
    m.compliant=True
    
    
import time
d=10
for nr in range(80):
 praegu=[m.present_position for m in poppy.motors]
 for i in range(len(praegu)):
    m=poppy.motors[i]
    if abs(praegu[i]-asendid[i])<d: m.led="green"
    elif praegu[i]<asendid[i]: m.led="yellow"
    else: m.led="blue"
 time.sleep(0.5)
 
 
 
 import time
d=10
for nr in range(80):
 praegu=[m.present_position for m in poppy.motors]
 mitu=0
 for i in range(len(praegu)):
    m=poppy.motors[i]
    if abs(praegu[i]-asendid[i])<d: 
        m.led="green"
        mitu=mitu+1
    elif praegu[i]<asendid[i]: m.led="yellow"
    else: m.led="blue"
 time.sleep(0.5)
 if mitu==6: break

if mitu==6:
  for m in poppy.motors: m.led="white" 
  time.sleep(1)
  for m in poppy.motors: m.led="green" 
else: 
  for m in poppy.motors: m.led="red"