Driver de Player para el control 2.0 del robot RoMAA-II
Tabla de Contenidos
Objetivos
Desarrollar el nuevo driver del robot RoMAA para la versión 2.0 del sistema de control embebido compatible con Player versión 3.0.
Control embebido 2.0
El control embebido tiene tres modos de funcionamiento
- A lazo abierto mediante comandos de PWM a cada motor
- A lazo cerrado en velocidad independiente para cada motor
- Lazo cerrado cross-coupling utilizando comandos de velocidad lineal y angular
Para más detalles del control embebido ver "Hardware de Control de Plataforma Robótica Móvil con Arquitectura ARM y RTOS. Caracterización".
Player driver
Para la programación de un driver de Player se utilizan las librerías incluidas en player que permite generar un driver plugin. Los drivers plugin se carga en tiempo de ejecución del servidor. La instalación de Player incluye un ejemplo de programación de driver plugin que puede encontrarse, junto con el archivo CMakeList.txt para la compilación, en ${prefix}/share/player/example.
Estructura del driver plugin (player v3.0)
#include <libplayercore/playercore.h>
class Romaa : public ThreadedDriver
{
public:
// Constructor
Romaa(ConfigFile* cf, int section);
// This method will be invoked on each incoming message
virtual int ProcessMessage(QueuePointer &resp_queue,
player_msghdr * hdr,
void * data);
private:
// Main function for device thread.
virtual void Main();
virtual int MainSetup();
virtual void MainQuit();
};El driver plugin se programa como una clase de C++ heredara de la clase ThreadedDriver. Esta clase tiene los siguientes métodos virtuales
MainSetup():Realiza funciones de inicialización especificas del dispositivo por ej. abrir y configurar un puerto de comunicación serie; también ejecuta el thread del driver. El método MainSetup se llama cada vez que un cliente se subscribe al driver.
MainQuit(): Es el opuesto al método MainSetup. MainQuit asegura la terminación de dispositivos como por ej. cerrar un puerto de comunicación serie; también detiene el thread del driver.
Main(): Es la función principal del thread del dispositivo, donde se incluyen todas las interacciones con el dispositivo.
ProcessMessage():Este método se invoca con cada mensaje entrante al servidor y ofrece la posibilidad de enviar una respuesta publicando un mensaje utilizando la función miembro pública Publish.
El server player inicia el plugin driver llamando a player_driver_init (función en C), esta función llama a Romaa_Register, la cual agrega el driver a una tabla de drivers del servidor Player mediante método AddDriver que llama a Romaa_Init. Romaa_Init crea un nuevo objeto Romaa y devuelve un puntero al driver. Una vez creado el objeto Romaa se ejecuta el constructor. En el constructor se cargan los datos del archivo de configuración (.cfg) del servidor Player. Luego se ejecuta MainSetup y el método principal Main.
/* need the extern to avoid C++ name-mangling */
extern "C" {
int player_driver_init(DriverTable* table)
{
puts("romaa driver initializing");
Romaa_Register(table);
puts("romaa driver done");
return(0);
}
}
void Romaa_Register(DriverTable* table)
{
table->AddDriver("romaa", Romaa_Init);
}
Driver* Romaa_Init(ConfigFile* cf, int section)
{
// Create and return a new instance of this driver
return((Driver*)(new Romaa(cf, section)));
}El método Main es el núcleo del driver plugin y se ejecuta en un thread lo que significa que corre en paralelo con otros drivers. La mayor parte del método Main esta contenida en un loop infinito. El método Main tiene que llamar unas pocas funciones específicas:
pthread_testcancel:Verifica si el thread fue terminado (killed). La función saldrá del loop y ejecutara la el método MainQuit.
ProcessMessages: Le pasa el control a Player que llamará la función ProcessMessage() del plugin con cada mensaje esperando en la cola de mensajes.
usleep: Es un comando de sleep para liberar los recursos utilizados por el driver.
Romaa::Main()
{
// The main loop; interact with the device here
for(;;)
{
// test if we are supposed to cancel
pthread_testcancel();
// Process incoming messages. Romaa::ProcessMessage() is
// called on each message.
ProcessMessages();
// Interact with the device, and push out the resulting data, using
// Driver::Publish()
// Sleep (you might, for example, block on a read() instead)
usleep(100000);
}
}
Procesamiento de mensajes
Las diferentes interfaces en Player interactúan unas con otras enviando y recibiendo mensajes a través de Player. Algunos de los diferentes tipos de mensajes son PLAYER_MSGTYPE_DATA (datos), PLAYER_MSGTYPE_CMD (comando), PLAYER_MSGTYPE_REQ (petición), PLAYER_MSGTYPE_RESP_ACK (respuesta), etc.
Tipos de mensajes
Commands: Se utilizan para darle instrucciones al driver cuando no se requiere una respuesta
Requests: Son mensajes desde otros drivers para acceder a datos que no son publicados regularmente o enviar comandos que requieren algún tipo de respuesta
Data: Los mensajes de datos son publicados en cada iteración del loop Main del driver.
Además, cada interfaz soporta un subconjunto de los mensajes anteriores. Los mensajes se transfieren como un puntero a objetos Message. Un método importante de la clase Messages es MatchMessage que se utiliza dentro de ProcessMessage para determinar si el encabezado de un mensaje coincide con algunos de los tipos preestablecidos.
Archivo de configuración
En el archivo de configuración para el driver del robot RoMAA se fijan los parámetros de la comunicación como el puerto serie y la velocidad. Además de los parámetros de los controladores PID tanto para los motores como para el lazo cruzado de control de velocidad lineal y angular. Otro parámetro importante es wheel_control que permite (cuando vale 0) controlar el robot mediante velocidad lineal/angular, o bien (cuando vale 1) controlar el robot mediante velocidad lineal de cada rueda.
Para el RoMAA
Los comandos de la interfaz position2d implementados son: PLAYER_POSITION2D_CMD_VEL, PLAYER_POSITION2D_REQ_RESET_ODOM, PLAYER_POSITION2D_REQ_SET_ODOM, PLAYER_POSITION2D_REQ_GET_GEOM, PLAYER_POSITION2D_REQ_MOTOR_POWER, PLAYER_POSITION2D_DATA_STATE
El archivo de configuración para cargar el driver es el siguiente
driver ( name "romaa" plugin "libromaa" provides [ "position2d:0" ] port "/dev/ttyUSB0" baudrate 115200 motorpid [ 1300.0 5000.0 1.0 ] vwpid [ 0.0 25.0 0.0 ] wheel_control 0 todometry 25 tloop 20 )
los parámetros son los siguientes
motorpid: valores de las constantes del PID del lazo cerrado de velocidad de cada motor (Kp,Ki,Kd)
vwpid: valores de la constante del PID del cross-coupling, somamante integrador (Ki)
wheel_control: puesto a 1 permite controlar el RoMAA mediante velocidad en cada rueda
todometry: tiempo (en ms) en que el controlador embebido actualiza el valor de odometría
tloop: tiempo (en ms) en que el controlador embebido calcula el lazo cerrado
Para el joystick
La siguiente sección driver permite controlar al robot RoMAA con el driver del joystick del lado del servidor de Player. En este caso se fija como velocidad lineal máxima 1m/s y se controla con el eje 1, y de velocidad angular máxima de 30deg/s y se controla con el eje 2. Para el control mediante vw se tiene que fijar wheel_control 0 en el driver del RoMAA.
# 1 m/sec max linear velocity # 30 deg/sec max angular velocity # Axis 1 is X # Axis 2 is Yaw # Y is not used here driver ( name "linuxjoystick" provides [ "joystick:0" ] requires [ "position2d:0" ] max_speed [1 0 30] axes [1 -1 2] port "/dev/input/js0" alwayson 1 )
La siguiente sección driver permite controlar al robot RoMAA con el driver del joystick del lado del servidor de Player. En este caso se fija como velocidad lineal máxima de cada rueda a 0.5m/s y se controlan con los ejes 1 y 3. Para el control mediante vw se tiene que fijar wheel_control 1 en el driver del RoMAA.
# control wheel linear speed # Axis 1 is X # Axis 3 is Y # Yaw is not used here driver ( name "linuxjoystick" provides [ "joystick:0" ] requires [ "position2d:0" ] max_speed [0.5 0.5 0] axes [1 3 -1] port "/dev/input/js0" alwayson 1 )
