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

# # Les commandes de bas niveau pour configurer un moteur xl-320

# Importer les modules necessaires.

# In[2]:


import pypot.dynamixel
import time


# ### Connecter les moteurs au niveau logiciel.

# Trouver le port sur lequel est branché le moteur 

# In[3]:


print(pypot.dynamixel.get_available_ports())


# Ouvrir une communication série avec le moteur :

# In[4]:


dxl_io = pypot.dynamixel.Dxl320IO('/dev/ttyAMA0')


# Cherche les moteurs connectés sur le bus (il en faut un seul avec l'ID 1 pour la première utilisation) :

# In[15]:


motors = (dxl_io.scan(range(60)))
print(motors)
print(len(motors))


# Modifier l'identifiant d'un moteur (ID) :

# In[14]:


#Remplace l'ID 1 par 2 sur le moteur 1.
#Attention, les moteurs doivents tous avoir une ID différente sur le même bus. 
dxl_io.change_id({1:2}) 


# Retourne la position cible pour le moteur 1 :

# In[16]:


dxl_io.get_goal_position({1})


# Donne un objectif de position pour le moteur 1 :

# In[ ]:


dxl_io.set_goal_position({12: 60})


# Ferme la connexion avec les moteurs :

# In[18]:


dxl_io.close()


# ### Contrôler plusieurs moteurs en continue

# In[19]:


from pypot.dynamixel import autodetect_robot
my_robot = autodetect_robot()


# Afficher les moteurs de my_robot :

# In[22]:


my_robot.motors


# Faire bouger le moteur 2 jusqu'à la position 0 degrés en 2 secondes :

# In[23]:


my_robot.compliant = False


# In[26]:


my_robot.motor_2.goto_position(0,2)


# Aller plus loin dans les commandes possibles : 
# 
# La documentation de Pypot : https://poppy-project.github.io/pypot/
# 
# Le projet Poppy : https://www.poppy-project.org/fr/