==Brazo Bioloid==

 

 

 

Para la comunicación e intercambio de información entre [http://www.arduino.cc arduino] y [http://www.ros.org ROS] es necesario instalar [http://www.arduino.cc arduino] IDE y rosserial (''package'' de [http://www.ros.org ROS] que contiene el ''stack'' rosserial_arduino con las librerias para [http://www.arduino.cc arduino]). Comenzaremos instalando el [http://www.arduino.cc arduino] IDE, para lo que ejecutaremos los siguientes comandos en un terminal:

sudo apt-get install arduino arduino-core

sudo apt-get install arduino</syntaxhighlight>

Instalados [http://www.arduino.cc arduino] IDE y el ''package'' rosserial debemos copiar las librerial de ''stack'' rosserial_arduino a el sketchbook de [http://www.arduino.cc arduino], carpeta que se encuentra habitualmente en la carpeta personal, para lo que ejecutaremos en un terminal los siguientes comandos:

cp -r ros_lib home/”nombre_sesion”/sketchbook/libraries/ros_lib</syntaxhighlight>

===Creación de un package para nuestros programas===

Deberemos sustituir el contenido del fichero CmakeLists.txt, situado en la carpeta del ''package'' creado, por el siguiente:

===Primer programa (A toda potencia)===

Para su compilación y envio a la placa [http://www.arduino.cc arduino] es necesario añadir al archivo CmakeLists.txt las siguientes líneas:

set(${FIRMWARE_NAME}_PORT /dev/ttyACM0)            # Puerto serie de subida

==Arduino y servomotores Dinamixel==

===Segundo programa (Muévete)===

==Modelo cinemático del brazo==

===Cinemática inversa===

Siguiendo los diagramas planteados y considerando ''x'',''y'' y ''z'' como las coordenadas del punto destino, y que la pinza siempre se ha de mantener paralela al plano x-z, las ecuaciones obtenidas para la cinemática inversa son las siguientes:

===Cinemática directa===

===Espacio de trabajo y limitaciones===

==Programas de control==

===Tercer programa (Ven aquí)===

====Creación de mensajes personalizados en ROS====

Previamente [[Fernando-TFM-ROS02#Creando un package|crearemos un package]] con las dependencias necesarias para nuestros programas. Dentro de este ''package''crearemos una carpeta llamada "msg", donde colocaremos los siguientes archivos con la descripción de los mensajes (tipo de dato y nombre e campo).  

Vamos a crear un mensaje que contenga los datos que enviaremos a los servomotores (posición y par). Ahora el tipo de dato que vamos a manejar es el que hemos creado anteriormente. Creamos un archivo llamado "WriteServos.msg" con el siguiente contenido:

Para la recepción de datos de los servomotores crearemos un archivo llamado "ReadServos.mag" con el siguiente contenido:

Para la creación de los mensajes en el sistema [http://www.ros.org ROS] debemos editar el archivo "CMakeLists.txt" y eliminar el símbolo "#" para que deje de estar comentada la siguiente línea:

====Programa arduino====

<syntaxhighlight lang=cpp>

#include <std_msgs/Int16.h>

     Dynamixel.moveSpeed(1,move.base,abs(move.base - posicion[0])/5);   

     Dynamixel.moveSpeed(2,move.arti1,abs(move.arti1 - posicion[1])/5);   

     Dynamixel.moveSpeed(3,move.arti2,abs(move.arti2 - posicion[2])/5);   

     Dynamixel.moveSpeed(4,move.arti3,abs(move.arti3 - posicion[3])/5);   

     Dynamixel.moveSpeed(5,move.pinza,abs(move.pinza - posicion)/5);   

====Programa ROS====

Este es el programa que indica la posición de los servomotores para alcanzar un punto del espacio de trabajo. Está suscrito al ''topic'' "point", por el cual recibe el punto del espacio en el que tiene que posicionarse, evalúa la viabilidad y obra en consecuencia, tambien esta suscrito al ''topic'' "pose_arm", donde recibe la información de los servomotores que publica el programa [http://www.arduino.cc arduino], y publica el ''topic'' "move_arm", donde indica al programa [http://www.arduino.cc arduino] la posición y par de los servomotores. Dentro del ''pakage'' creado, en el directorio "src" crearemos un fichero llamado "control_v01.cpp" con el siguiente contenido:

<syntaxhighlight lang=cpp>

   brazo_fer::Servos move, p, e, c;

double alfa, beta, beta_a, beta_p, gamma, delta, delta_a, delta_p;

L_a = sqrt(pow(y,2)+pow(z_p-Lp,2));

beta_p = atan2(y,z_p-Lp);

         delta_p = acos((pow(L_a,2)+pow(Lp,2)-pow(L,2))/(2*L_a*Lp));

if (y<0) {

<syntaxhighlight lang=cmake>rosbuild_add_executable(contro_v01 src/control_v01.cpp)</syntaxhighlight>

====Ejecutando el programa====

<syntaxhighlight>rostopic pub point geometry_msgs/Point '{x: 50, y: 100, z: 150}' --once</syntaxhighlight>

===Cuarto programa (Agarra la botella)===

# Primera aproximación (sitúa el brazo a unos 60 mm del punto indicado).

# Posicionamiento en el punto indicado.

Dentro del package creado, en el directorio "src" crearemos un fichero llamado "control_v02.cpp" con el siguiente contenido:

   #include "ros/ros.h"  

   #include "math.h"

   #define L1 104 // longitud antebrazo (mm)

   #define L2 104 // longitud brazo (mm)

   #define Lp 60 //longitud a la pinza

  geometry_msgs::Point punto_0,punto_1;

  int cont = 0;

  int cont_1 = 0;

  int cont_2 = 0;

  brazo_fer::Servos pg;

  brazo_fer::Servos move;

  brazo_fer::Servos pin;

  brazo_fer::Servos on;

   brazo_fer::Servos off;

  brazo_fer::Servos inversa(geometry_msgs::Point destino)  

double alfa, beta, beta_a, beta_p, gamma, delta, delta_a, delta_p;

L_a = sqrt(pow(y,2)+pow(z_p-Lp,2));

beta_p = atan2(y,z_p-Lp);

         delta_p = acos((pow(L_a,2)+pow(Lp,2)-pow(L,2))/(2*L_a*Lp));

if (y<0) {

if (isnan(gamma)) // si no hay solución

brazo_fer::Servos brazo;

brazo.base = ((alfa+150)*1023)/300;

brazo.arti3 = ((delta-30)*1023)/300;

if (coordenadas_correctas == 1 && (205 <= brazo.base && brazo.base <= 818) && (120 <= brazo.arti1 && brazo.arti1 <= 920) && brazo.arti2 >= 50  && brazo.arti3 <= 828) {

return brazo;

return ::pg;

   void home()

   ros::Publisher move_pub_=n.advertise<brazo_fer::WriteServos>("move_arm", 1);   //Publicación del topic "move_arm"

if (::cont == 0) {

 

::punto_0.x = 0;

::punto_0.y = 75;

::punto_0.z = 50;

::on.base = 1;

::on.pinza = 1;

::off.base = 0;

::off.arti1 = 0;

::off.arti2 = 0;

::off.arti3 = 0;

::off.pinza = 0;  

::punto_1 = ::punto_0;

brazo_fer::Servos inicio = inversa(::punto_0);

brazo_fer::WriteServos ini;

ini.posicion = inicio;

ini.par = ::on;

move_pub_.publish(ini);

hand_pub_.publish(ini);

   brazo_fer::Servos acercar(geometry_msgs::Point cerca)

   cerca.z = cerca.z - 60*cos(atan2(x,z));

   cerca.x = cerca.x - 60*sin(atan2(x,z));

   brazo_fer::Servos move = inversa(cerca);

     brazo_fer::Servos levantar_acercar(geometry_msgs::Point la)

   brazo_fer::Servos move = inversa(la);

   void posicion_estado_corriente(const brazo_fer::ReadServos& pec)  //Función de llamda con cada dato recibido

   ros::Publisher move_pub_=n.advertise<brazo_fer::WriteServos>("move_arm", 1);   //Publicación del topic "move_arm"

   ros::Publisher hand_pub_=n.advertise<brazo_fer::WriteServos>("hand_arm", 1);  //Publicación del topic "hand_arm" 

        brazo_fer::Servos p = pec.posicion;   

::pg = pec.posicion;

brazo_fer::Servos c = pec.corriente;

home();

::cont = ::cont+1;

  ::move = acercar(::punto_0);

  brazo_fer::WriteServos uno;

  uno.posicion = ::move;

   if (((p.base-15) < ::move.base && ::move.base < (p.base+15)) && ((p.arti1-15) < ::move.arti1 && ::move.arti1 < (p.arti1+15)) && ((p.arti2-15) < ::move.arti2 && ::move.arti2 < (p.arti2+15)) && ((p.arti3-15) < ::move.arti3 && ::move.arti3 < (p.arti3+15))) {

::cont = 0;

home();

::cont = ::cont+1;

brazo_fer::WriteServos dos;  

dos.par = ::on;

move_pub_.publish(dos);

::cont_2 = ::cont_2+1;

  if (((p.base-15) < ::move.base && ::move.base < (p.base+15)) && ((p.arti1-15) < ::move.arti1 && ::move.arti1 < (p.arti1+15)) && ((p.arti2-15) < ::move.arti2 && ::move.arti2 < (p.arti2+15)) && ((p.arti3-15) < ::move.arti3 && ::move.arti3 < (p.arti3+15))) {

pinza = p.pinza;

if(c.pinza < 300) {

pinza = pinza + 20;

::pin.pinza = pinza;

brazo_fer::WriteServos tres;  

tres.posicion = ::pin;

tres.par = ::on;

hand_pub_.publish(tres);

::move = levantar_acercar(::punto_0);

move_pub_.publish(cuatro);

   void punto(const geometry_msgs::Point& point)  

   ros::Publisher move_pub_=n.advertise<brazo_fer::WriteServos>("move_arm", 1);   

   ros::Publisher hand_pub_=n.advertise<brazo_fer::WriteServos>("hand_arm", 1);   

ros::init(argc, argv, "control_brazo");

   ros::Subscriber pose_sub_= n.subscribe("pose_arm", 1, posicion_estado_corriente);

   ros::Subscriber point_sub_= n.subscribe("point", 1, punto);

ros::Publisher move_pub_=n.advertise<brazo_fer::WriteServos>("move_arm", 1);

ros::Publisher hand_pub_=n.advertise<brazo_fer::WriteServos>("hand_arm", 1);  

ros::spin();  

====Ejecutando el programa====

Para poder publicar las coordenadas del punto donde se encuentra la botella (ya que no disponemos de otros medios para calcular la posición se colocará la botella en una posición conocida), debemos publicar en el topic "point", al que esta suscrito el programa de control para recibir el punto. Ejecutaremos en un nuevo terminal el siguiente comando (en este caso la posición en la que se encuentra la botella):

<syntaxhighlight>rostopic pub point geometry_msgs/Point '{x: -150, y: 0, z: 150}' --once</syntaxhighlight>