Difference between revisions of "Teleoperación con controlador inalámbrico xbox360 (controlador xbox360+joy)"

From robotica.unileon.es
Jump to: navigation, search
(→‎Programa)
(→‎Programa)
Line 28: Line 28:
 
===Programa===
 
===Programa===
  
−
Se encargará de la asignación de acciones a los diferentes botones y ejes del controlador. El programa se subscribe al ''topic'' "joy" donde se publica el estado de los botones del controlador y publica los ''topics'' "cmd_vel", "mov_arm" y "hand_arm" para el control de [http://www.irobot.com/global/es/store/Roomba.aspx?gclid=CMfZy73s570CFTMetAodW3UA7A roomba] y el [[Control brazo MYRAbot (bioloid+arduino)|brazo]].
+
Se encargará de la asignación de acciones a los diferentes botones y ejes del controlador. El programa se subscribe al ''topic'' "joy" donde se publica el estado de los botones del controlador y publica los ''topics'' "cmd_vel", "move_arm" y "hand_arm" para el control de [http://www.irobot.com/global/es/store/Roomba.aspx?gclid=CMfZy73s570CFTMetAodW3UA7A roomba] y el [[Control brazo MYRAbot (bioloid+arduino)|brazo]]. Comenzaremos creando un nuevo ''package'' en nuestro espacio de trabajo. Nos situaremos en nuestro espacio de trabajo y ejecutaremos en un terminal el siguiente comando:
−
Comenzaremos creando un nuevo ''package'' en nuestro espacio de trabajo. Nos situaremos en nuestro espacio de trabajo y ejecutaremos en un terminal el siguiente comando:
 
  
 
<syntaxhighlight>roscreate-pkg myrabot_teleop brazo_fer geometry_msgs sensor_msgs std_msgs ros_cpp</syntaxhighlight>
 
<syntaxhighlight>roscreate-pkg myrabot_teleop brazo_fer geometry_msgs sensor_msgs std_msgs ros_cpp</syntaxhighlight>
  
−
crearemos un archivo llamado "teleoperador_xbox360.cpp" dentro del directorio "src" del ''package'' creado con el siguiente contenido:
+
Crearemos un archivo llamado "teleoperador_xbox360.cpp" dentro del directorio "src" del ''package'' creado con el siguiente contenido:
  
 +
<syntaxhighlight lang=cpp enclose=div>
  
 +
  #include "ros/ros.h" 
 +
  #include "brazo_fer/Servos.h"
 +
  #include "brazo_fer/WriteServos.h" 
 +
  #include "brazo_fer/ReadServos.h"
 +
  #include "brazo_fer/brazo_fer.h"
 +
  #include "geometry_msgs/Twist.h"
 +
  #include "geometry_msgs/Point.h"   
 +
  #include "sensor_msgs/Joy.h"
 +
  #include "math.h"
 +
 +
myrabot_arm_base_b::Servos p, e, c, pinza;
 +
geometry_msgs::Point punto;
 +
int pinza_incli = 0;
 +
int paso = 5;
 +
int cont = 0;
 +
 +
myrabot_arm_base_b::WriteServos teleop;
 +
myrabot_arm_base_b::WriteServos teleop_pinza;
 +
 +
float avance = 0.2, giro = 0.5;
 +
int velocidad = 50;
 +
int start = 0;
 +
 +
geometry_msgs::Twist base;
 +
 +
float pad_izquierda_x, pad_izquierda_y, gatillo_izquierda, pad_derecha_x, pad_derecha_y, gatillo_derecha;
 +
int boton_a, boton_b, boton_izquierda, boton_x, boton_y, boton_derecha, boton_back, boton_start, boton_pad_izquierda, boton_pad_derecha, cruz_izquierda, cruz_derecha, cruz_arriba, cruz_abajo;
 +
 +
  void posicion_estado_corriente(const myrabot_arm_base_b::ReadServos& pec) 
 +
  {
 +
 +
::p = pec.posicion;
 +
::e = pec.estado;
 +
::c = pec.corriente;
 +
 
 +
  }  
 +
 +
  void xbox(const sensor_msgs::Joy& xbox_joystick) 
 +
  {
 +
 +
::pad_izquierda_x = xbox_joystick.axes[0];
 +
::pad_izquierda_y = xbox_joystick.axes[1];
 +
::gatillo_izquierda = xbox_joystick.axes[2] -1;
 +
::pad_derecha_x = xbox_joystick.axes[3];
 +
::pad_derecha_y = xbox_joystick.axes[4];
 +
::gatillo_derecha = xbox_joystick.axes[5] -1;
 +
 +
::boton_a = xbox_joystick.buttons[0];
 +
::boton_b = xbox_joystick.buttons[1];
 +
::boton_x = xbox_joystick.buttons[2];
 +
::boton_y = xbox_joystick.buttons[3];
 +
::boton_izquierda = xbox_joystick.buttons[4];
 +
::boton_derecha = xbox_joystick.buttons[5];
 +
::boton_back = xbox_joystick.buttons[6];
 +
::boton_start = xbox_joystick.buttons[7];
 +
::boton_pad_izquierda = xbox_joystick.buttons[9];
 +
::boton_pad_derecha = xbox_joystick.buttons[10];
 +
::cruz_izquierda = xbox_joystick.buttons[11];
 +
::cruz_derecha = xbox_joystick.buttons[12];
 +
::cruz_arriba = xbox_joystick.buttons[13];
 +
::cruz_abajo = xbox_joystick.buttons[14];
 +
  }
 +
 +
  int main(int argc, char **argv)
 +
  {
 +
 +
ros::init(argc, argv, "::teleoperador_xbox360"); 
 +
 +
ros::NodeHandle n;
 +
 +
 
 +
  ros::Subscriber pose_sub_= n.subscribe("pose_arm", 1, posicion_estado_corriente); 
 +
 
 +
  ros::Subscriber joystick_sub_= n.subscribe("joy", 1, xbox);
 +
 
 +
 
 +
ros::Publisher move_pub_=n.advertise<myrabot_arm_base_b::WriteServos>("move_arm", 1); 
 +
 +
ros::Publisher hand_pub_=n.advertise<myrabot_arm_base_b::WriteServos>("hand_arm", 1);
 +
 +
ros::Publisher base_pub_=n.advertise<geometry_msgs::Twist>("cmd_vel", 1);
 +
 +
    ros::Rate loop_rate(5);
 +
 +
while (ros::ok())
 +
{
 +
 +
myrabot_arm_base_b::Servos pose = ::p;
 +
 +
if (::cont == 0 && (pose.base != 0 && pose.arti1 != 0 && pose.arti2 != 0 && pose.arti3 != 0))
 +
{
 +
::punto = home(::p, ::c);
 +
 +
::cont = 1;
 +
 +
::pinza.pinza = 511;
 +
 +
::teleop = inversa(::punto, ::pinza_incli, ::p, ::velocidad);
 +
 +
::teleop_pinza = control_pinza(::pinza, ::p, ::c);
 +
}
 +
 +
if (::pad_izquierda_y > 0.2 || ::pad_izquierda_y < -0.2 || ::pad_izquierda_x > 0.2 || ::pad_izquierda_x < -0.2 || ::pad_derecha_x > 0.2 || ::pad_derecha_x < -0.2 || pad_derecha_y > 0.2 || pad_derecha_y < -0.2 || (::gatillo_izquierda < -0.2 && ::gatillo_izquierda != -1) || (::gatillo_derecha < -0.2 && ::gatillo_derecha != -1) || ::cruz_izquierda !=  0 || ::cruz_derecha !=  0 || ::cruz_arriba != 0 || ::cruz_abajo != 0 || ::boton_back != 0 || ::boton_start != 0 || ::boton_x != 0 || ::boton_y != 0 || ::boton_izquierda != 0 || ::boton_derecha != 0)
 +
{
 +
 +
if (::boton_start != 0)
 +
{
 +
if (::start == 0)
 +
{
 +
::start = 1;
 +
}
 +
else
 +
{
 +
::start = 0;
 +
}
 +
}
 +
 +
if (::start == 1)
 +
{
 +
 +
if (::boton_back != 0)
 +
{
 +
::cont = 0;
 +
}
 +
 +
if (::cruz_izquierda != 0)
 +
{
 +
::velocidad = ::velocidad - 10;
 +
 +
if (::velocidad < 10)
 +
{
 +
::velocidad = 10;
 +
}
 +
}
 +
 +
if (::cruz_derecha != 0)
 +
{
 +
::velocidad = ::velocidad + 10;
 +
 +
if (::velocidad > 100)
 +
{
 +
::velocidad = 100;
 +
}
 +
}
 +
 +
if (::pad_derecha_x > 0.2 || ::pad_derecha_x < -0.2)
 +
{
 +
::punto.x = ::punto.x + (::pad_derecha_x * ::paso);
 +
::teleop = inversa(::punto, ::pinza_incli, ::p, ::velocidad);
 +
if (::teleop.posicion.base == pose.base && ::teleop.posicion.arti1 == pose.arti1 && ::teleop.posicion.arti2 == pose.arti2 && ::teleop.posicion.arti3 == pose.arti3)
 +
{::punto.x = ::punto.x - (::pad_derecha_x * ::paso);}
 +
}
 +
 +
if (pad_derecha_y > 0.2 || pad_derecha_y < -0.2)
 +
{
 +
::punto.y = ::punto.y + (::pad_derecha_y * ::paso);
 +
::teleop = inversa(::punto, ::pinza_incli, ::p, ::velocidad);
 +
if (::teleop.posicion.base == pose.base && ::teleop.posicion.arti1 == pose.arti1 && ::teleop.posicion.arti2 == pose.arti2 && ::teleop.posicion.arti3 == pose.arti3)
 +
{::punto.y = ::punto.y - (::pad_derecha_y * ::paso);}
 +
}
 +
 +
if (::gatillo_izquierda < -0.2 && ::gatillo_izquierda != -1)
 +
{
 +
::punto.z = ::punto.z + (::gatillo_izquierda  * (::paso/2));
 +
::teleop = inversa(::punto, ::pinza_incli, ::p, ::velocidad);
 +
if (::teleop.posicion.base == pose.base && ::teleop.posicion.arti1 == pose.arti1 && ::teleop.posicion.arti2 == pose.arti2 && ::teleop.posicion.arti3 == pose.arti3)
 +
{::punto.z = ::punto.z - (::gatillo_izquierda * (::paso/2));}
 +
}
 +
 +
if (::gatillo_derecha < -0.2 && ::gatillo_derecha != -1)
 +
{
 +
::punto.z = ::punto.z - (::gatillo_derecha  * (::paso/2));
 +
::teleop = inversa(::punto, ::pinza_incli, ::p, ::velocidad);
 +
if (::teleop.posicion.base == pose.base && ::teleop.posicion.arti1 == pose.arti1 && ::teleop.posicion.arti2 == pose.arti2 && ::teleop.posicion.arti3 == pose.arti3)
 +
{::punto.z = ::punto.z + (::gatillo_derecha * (::paso/2));}
 +
}
 +
 +
if (::cruz_arriba !=  0)
 +
{
 +
::pinza_incli = ::pinza_incli - (::paso * ::cruz_arriba);
 +
teleop = inversa(::punto, ::pinza_incli, ::p, ::velocidad);
 +
if (teleop.posicion.base == pose.base && teleop.posicion.arti1 == pose.arti1 && teleop.posicion.arti2 == pose.arti2 && teleop.posicion.arti3 == pose.arti3)
 +
{::pinza_incli = ::pinza_incli + (::paso * ::cruz_arriba);}
 +
}
 +
 +
if (::cruz_abajo !=  0)
 +
{
 +
::pinza_incli = ::pinza_incli + (::paso * ::cruz_abajo);
 +
teleop = inversa(::punto, ::pinza_incli, ::p, ::velocidad);
 +
if (teleop.posicion.base == pose.base && teleop.posicion.arti1 == pose.arti1 && teleop.posicion.arti2 == pose.arti2 && teleop.posicion.arti3 == pose.arti3)
 +
{::pinza_incli = ::pinza_incli - (::paso * ::cruz_abajo);}
 +
}
 +
 +
if (::boton_izquierda != 0)
 +
{   
 +
::pinza.pinza = ::pinza.pinza - (::paso * ::boton_izquierda);
 +
::teleop_pinza = control_pinza(::pinza, ::p, ::c);
 +
if (::teleop_pinza.posicion.pinza == pose.pinza)
 +
{::pinza.pinza = ::pinza.pinza + (::paso * ::boton_izquierda);}
 +
}
 +
 +
if (::boton_derecha != 0)
 +
{   
 +
::pinza.pinza = ::pinza.pinza + (::paso * ::boton_derecha);
 +
::teleop_pinza = control_pinza(::pinza, ::p, ::c);
 +
if (::teleop_pinza.posicion.pinza == pose.pinza)
 +
{::pinza.pinza = ::pinza.pinza - (::paso * ::boton_derecha);}
 +
}
 +
 +
if (::teleop.posicion.base != pose.base || ::teleop.posicion.arti1 != pose.arti1 || ::teleop.posicion.arti2 != pose.arti2 || ::teleop.posicion.arti3 != pose.arti3)
 +
{
 +
move_pub_.publish(::teleop);
 +
}
 +
if (::teleop_pinza.posicion.pinza != pose.pinza)
 +
{
 +
hand_pub_.publish(::teleop_pinza);
 +
}
 +
 +
if (::boton_a != 0)
 +
{
 +
::avance = ::avance - 0.05;
 +
 +
if (::avance < 0.05)
 +
{
 +
::avance = 0.05;
 +
}
 +
}
 +
 +
if (::boton_b != 0)
 +
{
 +
::avance = ::avance + 0.05;
 +
 +
if (::avance > 0.5)
 +
{
 +
::avance = 0.5;
 +
}
 +
}
 +
 +
if (::pad_izquierda_y > 0.2 || ::pad_izquierda_y < -0.2)
 +
{
 +
::base.linear.x = ::avance * pad_izquierda_y;
 +
}
 +
else
 +
{
 +
::base.linear.x = 0;
 +
}
 +
 +
if (::boton_x != 0)
 +
{
 +
::giro = ::giro - 0.05;
 +
 +
if (::giro < 0.1)
 +
{
 +
::giro = 0.1;
 +
}
 +
}
 +
 +
if (::boton_y != 0)
 +
{
 +
::giro = ::giro + 0.05;
 +
 +
if (::giro > 1.5)
 +
{
 +
::giro = 1.5;
 +
}
 +
}
 +
 +
if (::pad_izquierda_x > 0.2 || ::pad_izquierda_x < -0.2)
 +
{
 +
::base.angular.z = ::giro * pad_izquierda_x;
 +
}
 +
else
 +
{
 +
::base.angular.z = 0;
 +
}
 +
 +
base_pub_.publish(::base);
 +
}
 +
}
 +
 +
//std::cout<<::punto<<std::endl;
 +
 +
ros::spinOnce();
 +
 +
loop_rate.sleep();
 +
}  
 +
 +
        return 0;
 +
  }
 +
 +
</syntaxhighlight>
  
 
[[Mobile manipulation | < volver a principal]]
 
[[Mobile manipulation | < volver a principal]]

Revision as of 15:40, 17 April 2014

< volver a principal

Teleoperación de MYRAbot

Para la realización de determinadas tareas es necesario hacer uso de algún sistema de teleoperación para controlar el robot. Lo más habitual es haciendo uso de una conexión ssh con el PC del robot y usando el teclado de nuestro PC. Este método emplea una red wifi de la cual depende la conexión entre los dos equipos, la cual puede fallar o ser inexistente. Por eso la mejor opción es emplear un dispositivo conectado al PC del robot provisto de su propia red inalámbrica. El dispositivo escogido por robustez y versatilidad ha sido el controlador inalámbrico xbox360 de microsoft.

Controlador xbox360

Descripción de botones y ejes

Este controlador está equipado con 6 ejes de control y 14 botones, que se reparten de la siguiente manera:

  • Palanca izquierda: 2 ejes y 1 botón (pulsación).
  • Palanca derecha: 2 ejes y 1 botón (pulsación).
  • Gatillo izquierdo: 1 eje.
  • Gatillo derecho: 1 eje.
  • Cruz: 4 botones.
  • Botones lado derecho: 4 botones.
  • Botón izquierdo: 1 botón.
  • Botón derecho: 1 botón.
  • Botón "back": 1 botón.
  • Botón "start": 1 botón.

En la siguiente imagen se muestra la función asignada a cada botón y eje del controlador xbox360 para la teleoperación de MYRAbot:

Funcionalidad controlador xbox360 para MYRAbot.

Programa

Se encargará de la asignación de acciones a los diferentes botones y ejes del controlador. El programa se subscribe al topic "joy" donde se publica el estado de los botones del controlador y publica los topics "cmd_vel", "move_arm" y "hand_arm" para el control de roomba y el brazo. Comenzaremos creando un nuevo package en nuestro espacio de trabajo. Nos situaremos en nuestro espacio de trabajo y ejecutaremos en un terminal el siguiente comando: 

roscreate-pkg myrabot_teleop brazo_fer geometry_msgs sensor_msgs std_msgs ros_cpp

Crearemos un archivo llamado "teleoperador_xbox360.cpp" dentro del directorio "src" del package creado con el siguiente contenido: 

  #include "ros/ros.h"  
  #include "brazo_fer/Servos.h"
  #include "brazo_fer/WriteServos.h"  
  #include "brazo_fer/ReadServos.h"
  #include "brazo_fer/brazo_fer.h"
  #include "geometry_msgs/Twist.h"
  #include "geometry_msgs/Point.h"    
  #include "sensor_msgs/Joy.h"
  #include "math.h"

myrabot_arm_base_b::Servos p, e, c, pinza;
geometry_msgs::Point punto;
int pinza_incli = 0;
int paso = 5;
int cont = 0;
	
myrabot_arm_base_b::WriteServos teleop;
myrabot_arm_base_b::WriteServos teleop_pinza;

float avance = 0.2, giro = 0.5;
int velocidad = 50;
int start = 0;

geometry_msgs::Twist base;

float pad_izquierda_x, pad_izquierda_y, gatillo_izquierda, pad_derecha_x, pad_derecha_y, gatillo_derecha;
int boton_a, boton_b, boton_izquierda, boton_x, boton_y, boton_derecha, boton_back, boton_start, boton_pad_izquierda, boton_pad_derecha, cruz_izquierda, cruz_derecha, cruz_arriba, cruz_abajo;
 
  void posicion_estado_corriente(const myrabot_arm_base_b::ReadServos& pec)   
  {
	
	::p = pec.posicion;
	::e = pec.estado;
	::c = pec.corriente;
	  
  }	  	 
 
  void xbox(const sensor_msgs::Joy& xbox_joystick)   
  {	
	
	::pad_izquierda_x = xbox_joystick.axes[0];
	::pad_izquierda_y = xbox_joystick.axes[1];
	::gatillo_izquierda = xbox_joystick.axes[2] -1;
	::pad_derecha_x = xbox_joystick.axes[3];
	::pad_derecha_y = xbox_joystick.axes[4];
	::gatillo_derecha = xbox_joystick.axes[5] -1;
	
	::boton_a = xbox_joystick.buttons[0];
	::boton_b = xbox_joystick.buttons[1];
	::boton_x = xbox_joystick.buttons[2];
	::boton_y = xbox_joystick.buttons[3];
	::boton_izquierda = xbox_joystick.buttons[4];
	::boton_derecha = xbox_joystick.buttons[5];
	::boton_back = xbox_joystick.buttons[6];
	::boton_start = xbox_joystick.buttons[7];
	::boton_pad_izquierda = xbox_joystick.buttons[9];
	::boton_pad_derecha = xbox_joystick.buttons[10];	
	::cruz_izquierda = xbox_joystick.buttons[11];
	::cruz_derecha = xbox_joystick.buttons[12];
	::cruz_arriba = xbox_joystick.buttons[13];
	::cruz_abajo = xbox_joystick.buttons[14];	
  }
 
  int main(int argc, char **argv)
  {
 
	ros::init(argc, argv, "::teleoperador_xbox360");   
 
	ros::NodeHandle n;
	
  	
  	ros::Subscriber pose_sub_= n.subscribe("pose_arm", 1, posicion_estado_corriente);   
  	
  	ros::Subscriber joystick_sub_= n.subscribe("joy", 1, xbox); 
  	
  	
	ros::Publisher move_pub_=n.advertise<myrabot_arm_base_b::WriteServos>("move_arm", 1);   

	ros::Publisher hand_pub_=n.advertise<myrabot_arm_base_b::WriteServos>("hand_arm", 1);
	
	ros::Publisher base_pub_=n.advertise<geometry_msgs::Twist>("cmd_vel", 1); 
		
    ros::Rate loop_rate(5);	
	
	while (ros::ok())
	{
	
		myrabot_arm_base_b::Servos pose = ::p;	
		
		if (::cont == 0 && (pose.base != 0 && pose.arti1 != 0 && pose.arti2 != 0 && pose.arti3 != 0))
		{
			::punto = home(::p, ::c);	
		
			::cont = 1;
		
			::pinza.pinza = 511;
		
			::teleop = inversa(::punto, ::pinza_incli, ::p, ::velocidad);		
		
			::teleop_pinza = control_pinza(::pinza, ::p, ::c);
		}

		if (::pad_izquierda_y > 0.2 || ::pad_izquierda_y < -0.2 || ::pad_izquierda_x > 0.2 || ::pad_izquierda_x < -0.2 || ::pad_derecha_x > 0.2 || ::pad_derecha_x < -0.2 || pad_derecha_y > 0.2 || pad_derecha_y < -0.2 || (::gatillo_izquierda < -0.2 && ::gatillo_izquierda != -1) || (::gatillo_derecha < -0.2 && ::gatillo_derecha != -1) || ::cruz_izquierda !=  0 || ::cruz_derecha !=  0 || ::cruz_arriba != 0 || ::cruz_abajo != 0 || ::boton_back != 0 || ::boton_start != 0 || ::boton_x != 0 || ::boton_y != 0 || ::boton_izquierda != 0 || ::boton_derecha != 0)
		{
		
			if (::boton_start != 0)
			{
				if (::start == 0)
				{
					::start = 1;
				}
				else
				{
					::start = 0;
				}
			}
			
			if (::start == 1)
			{
			
				if (::boton_back != 0)
				{
					::cont = 0;
				}
				
				if (::cruz_izquierda != 0)
				{
					::velocidad = ::velocidad - 10;
					
					if (::velocidad < 10)
					{
						::velocidad = 10;
					}
				}
				
				if (::cruz_derecha != 0)
				{
					::velocidad = ::velocidad + 10;
					
					if (::velocidad > 100)
					{
						::velocidad = 100;
					}
				}

				if (::pad_derecha_x > 0.2 || ::pad_derecha_x < -0.2)
				{
				::punto.x = ::punto.x + (::pad_derecha_x * ::paso);
				::teleop = inversa(::punto, ::pinza_incli, ::p, ::velocidad);
				if (::teleop.posicion.base == pose.base && ::teleop.posicion.arti1 == pose.arti1 && ::teleop.posicion.arti2 == pose.arti2 && ::teleop.posicion.arti3 == pose.arti3) 
				{::punto.x = ::punto.x - (::pad_derecha_x * ::paso);}
				}
				
				if (pad_derecha_y > 0.2 || pad_derecha_y < -0.2)
				{
				::punto.y = ::punto.y + (::pad_derecha_y * ::paso);
				::teleop = inversa(::punto, ::pinza_incli, ::p, ::velocidad);
				if (::teleop.posicion.base == pose.base && ::teleop.posicion.arti1 == pose.arti1 && ::teleop.posicion.arti2 == pose.arti2 && ::teleop.posicion.arti3 == pose.arti3) 
				{::punto.y = ::punto.y - (::pad_derecha_y * ::paso);}
				}
				
				if (::gatillo_izquierda < -0.2 && ::gatillo_izquierda != -1)
				{
				::punto.z = ::punto.z + (::gatillo_izquierda  * (::paso/2));
				::teleop = inversa(::punto, ::pinza_incli, ::p, ::velocidad);
				if (::teleop.posicion.base == pose.base && ::teleop.posicion.arti1 == pose.arti1 && ::teleop.posicion.arti2 == pose.arti2 && ::teleop.posicion.arti3 == pose.arti3) 
				{::punto.z = ::punto.z - (::gatillo_izquierda * (::paso/2));}
				}
				
				if (::gatillo_derecha < -0.2 && ::gatillo_derecha != -1)
				{
				::punto.z = ::punto.z - (::gatillo_derecha  * (::paso/2));
				::teleop = inversa(::punto, ::pinza_incli, ::p, ::velocidad);
				if (::teleop.posicion.base == pose.base && ::teleop.posicion.arti1 == pose.arti1 && ::teleop.posicion.arti2 == pose.arti2 && ::teleop.posicion.arti3 == pose.arti3) 
				{::punto.z = ::punto.z + (::gatillo_derecha * (::paso/2));}
				}		
				
				if (::cruz_arriba !=  0)
				{
				::pinza_incli = ::pinza_incli - (::paso * ::cruz_arriba);
				teleop = inversa(::punto, ::pinza_incli, ::p, ::velocidad);
				if (teleop.posicion.base == pose.base && teleop.posicion.arti1 == pose.arti1 && teleop.posicion.arti2 == pose.arti2 && teleop.posicion.arti3 == pose.arti3) 
				{::pinza_incli = ::pinza_incli + (::paso * ::cruz_arriba);}
				}
				
				if (::cruz_abajo !=  0)
				{
				::pinza_incli = ::pinza_incli + (::paso * ::cruz_abajo);
				teleop = inversa(::punto, ::pinza_incli, ::p, ::velocidad);
				if (teleop.posicion.base == pose.base && teleop.posicion.arti1 == pose.arti1 && teleop.posicion.arti2 == pose.arti2 && teleop.posicion.arti3 == pose.arti3) 
				{::pinza_incli = ::pinza_incli - (::paso * ::cruz_abajo);}
				}
				
				if (::boton_izquierda != 0)
				{     
				::pinza.pinza = ::pinza.pinza - (::paso * ::boton_izquierda);
				::teleop_pinza = control_pinza(::pinza, ::p, ::c);
				if (::teleop_pinza.posicion.pinza == pose.pinza) 
				{::pinza.pinza = ::pinza.pinza + (::paso * ::boton_izquierda);}		
				}
				
				if (::boton_derecha != 0)
				{     
				::pinza.pinza = ::pinza.pinza + (::paso * ::boton_derecha);
				::teleop_pinza = control_pinza(::pinza, ::p, ::c);
				if (::teleop_pinza.posicion.pinza == pose.pinza) 
				{::pinza.pinza = ::pinza.pinza - (::paso * ::boton_derecha);}		
				}

				if (::teleop.posicion.base != pose.base || ::teleop.posicion.arti1 != pose.arti1 || ::teleop.posicion.arti2 != pose.arti2 || ::teleop.posicion.arti3 != pose.arti3)
				{
					move_pub_.publish(::teleop);			
				}
				if (::teleop_pinza.posicion.pinza != pose.pinza)
				{				
					hand_pub_.publish(::teleop_pinza);
				}
				
				if (::boton_a != 0)
				{
					::avance = ::avance - 0.05;
					
					if (::avance < 0.05)
					{
						::avance = 0.05;
					}
				}
				
				if (::boton_b != 0)
				{
					::avance = ::avance + 0.05;
					
					if (::avance > 0.5)
					{
						::avance = 0.5;
					}
				}
				
				if (::pad_izquierda_y > 0.2 || ::pad_izquierda_y < -0.2)
				{
					::base.linear.x = ::avance * pad_izquierda_y;
				}
				else
				{
					::base.linear.x = 0;
				}
				
				if (::boton_x != 0)
				{
					::giro = ::giro - 0.05;
					
					if (::giro < 0.1)
					{
						::giro = 0.1;
					}
				}
				
				if (::boton_y != 0)
				{
					::giro = ::giro + 0.05;
					
					if (::giro > 1.5)
					{
						::giro = 1.5;
					}
				}
				
				if (::pad_izquierda_x > 0.2 || ::pad_izquierda_x < -0.2)
				{
					::base.angular.z = ::giro * pad_izquierda_x;
				}
				else
				{
					::base.angular.z = 0;
				}
				
				base_pub_.publish(::base);		
			}
		}
		
		//std::cout<<::punto<<std::endl;
		
		ros::spinOnce();
	
		loop_rate.sleep();
	}		  
 
        return 0;
  }

< volver a principal

Retrieved from "https://robotica.unileon.es/index.php?title=Teleoperación_con_controlador_inalámbrico_xbox360_(controlador_xbox360%2bjoy)&oldid=3504"