#!/usr/bin/env python
# coding: utf-8

# # QuickStart: Playing with a Poppy Ergo (or a PoppyErgoJr)

# **This notebook is still work in progress! [Feedbacks are welcomed](https://github.com/poppy-project/pypot/labels/Notebooks)!**

# In this tutorial we will show how to get started with your [PoppyErgo](https://www.poppy-project.org/project/mathematics-a-beautiful-elsewhere/) creature. You can use a [PoppyErgoJr](https://github.com/poppy-project/poppy-ergo-jr) 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](https://github.com/poppy-project/pypot) library
# * the [poppy-ergo](https://github.com/pierre-rouanet/poppy-ergo) library (or use the [poppy-ergo-jr](https://github.com/poppy-project/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:

# In[1]:


from pypot.creatures import PoppyErgo


# In[2]:


ergo = PoppyErgo()


# For a PoppyErgoJr:

# In[ ]:


from pypot.creatures import PoppyErgoJr

ergo = PoppyErgoJr()


# ## Get robot current status

# In[3]:


ergo


# In[4]:


ergo.m2


# In[5]:


ergo.m2.present_position


# In[6]:


ergo.m2.present_temperature


# In[7]:


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


# ## Turn on/off the compliancy of a motor

# In[8]:


ergo.m3.compliant


# In[9]:


ergo.m6.compliant = False


# ## Go to the zero position

# In[10]:


ergo.m6.goal_position = 0.


# In[11]:


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


# In[ ]:


# 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


# ## Make a simple dance movement

# On a single motor:

# In[13]:


import time

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


# On multiple motors:

# In[14]:


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


# Wrap it inside a function for convenience:

# In[15]:


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.)


# In[16]:


dance()


# In[17]:


for _ in range(4):
    dance()


# Using goto position instead:

# In[18]:


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)


# In[19]:


for _ in range(4):
    dance2()