from poppy.creatures import PoppyErgo

ergo = PoppyErgo()

"""
Explanation: QuickStart: Playing with a Poppy Ergo (or a PoppyErgoJr)
This notebook is still work in progress! Feedbacks are welcomed!
In this tutorial we will show how to get started with your PoppyErgo creature. You can use a PoppyErgoJr instead.
<img src="https://raw.githubusercontent.com/poppy-project/poppy-ergo-jr/master/doc/img/poppy-ergo-jr.jpg" alt="Poppy Ergo Jr" style="height: 500px;"/>
To run the code in this notebook, you will need:
* a poppy ergo creature (or a Jr)
* the pypot library
* the poppy-ergo library (or use the poppy-ergo-jr library instead)
You can install those libraries with the pip tool (see here if you don't know how to run this):
```bash
pip install pypot poppy-ergo
```
Connect to your robot
For a PoppyErgo:
End of explanation
"""


from poppy.creatures import PoppyErgoJr

ergo = PoppyErgoJr()

"""
Explanation: For a PoppyErgoJr:
End of explanation
"""


ergo

ergo.m2

ergo.m2.present_position

ergo.m2.present_temperature

for m in ergo.motors:
    print 'Motor "{}" current position = {}'.format(m.name, m.present_position)

"""
Explanation: Get robot current status
End of explanation
"""


ergo.m3.compliant

ergo.m6.compliant = False

"""
Explanation: Turn on/off the compliancy of a motor
End of explanation
"""


ergo.m6.goal_position = 0.

for m in ergo.motors:
    m.compliant = False
    
    # Goes to the position 0 in 2s
    m.goto_position(0, 2)

# You can also change the maximum speed of the motors
# Warning! Goto position also change the maximum speed.

for m in ergo.motors:
    m.moving_speed = 50

"""
Explanation: Go to the zero position
End of explanation
"""


import time

ergo.m4.goal_position = 30
time.sleep(1.)
ergo.m4.goal_position = -30

"""
Explanation: Make a simple dance movement
On a single motor:
End of explanation
"""


ergo.m4.goal_position = 30
ergo.m5.goal_position = 20
ergo.m6.goal_position = -20

time.sleep(1.)

ergo.m4.goal_position = -30
ergo.m5.goal_position = -20
ergo.m6.goal_position = 20

"""
Explanation: On multiple motors:
End of explanation
"""


def dance():
    ergo.m4.goal_position = 30
    ergo.m5.goal_position = 20
    ergo.m6.goal_position = -20

    time.sleep(1.)

    ergo.m4.goal_position = -30
    ergo.m5.goal_position = -20
    ergo.m6.goal_position = 20
    
    time.sleep(1.)

dance()

for _ in range(4):
    dance()

"""
Explanation: Wrap it inside a function for convenience:
End of explanation
"""


def dance2():
    ergo.goto_position({'m4': 30, 'm5': 20, 'm6': -20}, 1., wait=True)
    ergo.goto_position({'m4': -30, 'm5': -20, 'm6': 20}, 1., wait=True)

for _ in range(4):
    dance2()

"""
Explanation: Using goto position instead:
End of explanation
"""


