Programacion C con CM-510: leyendo y moviendo Dynamixel AX-12 (III y último)

Finalizaremos esta primera serie de programación del CM-510 en C moviendo un Dynamixel AX12 en función de los valores leídos con el sensor DMS.

DMS sensor

La función “main” es así de sencilla:

int main(void)
{  	
  int idAX12=4;
  int puertoDelSensorDMS=3;

  init();

// los dos ejemplos anterior refactorizados en una función

  // ordenesDesdeTerminal(); 
  // datosDesdeOtroAX12();
  
  puts("Situaremos el AX12 en la posicion que nos indique el sensor DMS");
  ordenesDesdeSensoresPorPasos(idAX12, puertoDelSensorDMS);
  
  // una prueba cruda
  //ordenesDesdeSensoresCrudo();

  puts("The End");

  return 0;
}

Siendo “void ordenesDesdeSensoresPorPasos(int idAX12, int puertoDelSensor)” la función principal del ejemplo:

void ordenesDesdeSensoresPorPasos(int idAX12, int puertoDelSensor)
{       	
/*
    Para evitar las habituales fluctuaciones que sufren los sensores
	sólo tendremos en cuenta las diferencias superiores al siguiente valor 
	entre la anterior lectura y la actual
*/
	int minimaDiferencia=25; 
	int diferencia=0; // la diferencia entre el valor anterior y el actual

	int anteriorValor=0; // el anterior valor obtenido del sensor
	int valor=0; // el de la lectura actual	

	while (true)
	{
	  anteriorValor=valor; // guardamos el anterior valor leído del sensor

	  valor=leerSensor(puertoDelSensor); // leemos el actual

	  diferencia=anteriorValor-valor;
	  //printf("n%i", valor); // por si queremos ir viendo los valores que envía el sensor

	  _delay_ms(100); // una brevisima pausa

	  // diferencias menores que esta las consideramos fluctuaciones del sensor
	  if (abs(diferencia)>minimaDiferencia)
	  {
	    // utilizamos el valor leído del sensor como posición a situar el AX12
		dxl_write_word( idAX12, P_GOAL_POSITION_L, valor);
	  }
	}
}

Utiliza la función leerSensor que tiene cierta dificultad, ya que utiliza la conversión analógico-digital de los microcontroladores ATMega. De momento no entraremos en los detalles, creo que será mejor explicarlo en una próxima serie que trate cuestiones más avanzadas. De todas formas, por si os despierta la curiosidad, os la incluyo a continuación; está basada en los ejemplos de Robotis:

int leerSensor(unsigned char idPuerto)
{
	ADCSRA = (1 << ADEN) | (1 << ADPS2) | (1 << ADPS1);	// ADC Enable, Clock 1/64div.

	// printf( "nnIR example for CM-510nn" );

	asignarPuerto(idPuerto);
	//PORTA &= ~0x80;
	//PORTA &= ~0x20;

	//_delay_us(12);				// Short Delay for rising sensor signal
	_delay_us(24);
	ADCSRA |= (1 << ADIF);		// AD-Conversion Interrupt Flag Clear
	ADCSRA |= (1 << ADSC);		// AD-Conversion Start
		
	while( !(ADCSRA & (1 << ADIF)) );	// Wait until AD-Conversion complete
		
	PORTA = 0xFC;				// IR-LED Off
		
	//printf( "%drn", ADC); // Print Value on USART
					
	//_delay_ms(50);
	_delay_ms(esperaLeerSensorMS);

	return ADC;
}

Y aquí os podéis descargar el ejemplo completo.

Programación C con CM-510: leyendo y moviendo Dynamixel AX-12 (I)

Como segunda entrega de este taller de programación de Bioloid vamos a realizar un programa que nos pedirá el identificador (ID) del AX-12+ a mover y la posición en la que lo situaremos.

Las explicaciones las he ido incluyendo en el código como comentarios, espero que los comentarios sean suficientes como para poderlo seguir, ya me diréis si es así.

El bucle principal es así de sencillo:

int main(void)
{
init();

while(true) // repetiremos eternamente este bucle
{
int id=obtenerId(); // obtenemos el ID del AX-12 a utilizar
int posicion=obtenerPosicion(); // obtenemos la posición en la que situar el AX-12 seleccionado
dxl_write_word( id, P_GOAL_POSITION_L, posicion); // enviamos la orden al Dynamixel
}

return 0;
}

Unas breves explicaciones sobre printf: La función printf es mucho más potente de lo que parece a primera vista, admitiendo bastantes parámetros que nos permiten mostrar una gran cantidad de tipos de datos y formatos diferentes. En el siguiente ejemplo %iindica que en dicha posición del mensaje se incluirá un entero que le pasaremos como parámetro a continuación de la cadena “ID del AX12:”. El carácter de control “n” indica que incluya un salto de línea.

Cada carácter de la cadena se almacena en un posición de memoria [I][D][ ][d][e][l]… siendo las cadenas un caso especial de vector o, en inglés, “array”.

Descárgate aquí una presentación en PDF con explicaciones muy detalladas sobre cadenas en C, de la Universidad del País Vasco.

Las funciones obtenerId() y obtenerPosicion() son también bastante sencillas, ¿verdad?

/*
La siguiente función solicita la entrada de un valor para seleccionar el ID del Dynamixel al que
enviar las órdenes, Comprobando que el que esté entre 1 y 18
*/

int obtenerId()
{
/*
Creamos una variable de un tamaño amplio, 256 bytes (posiciones de un carácter), aunque con 4
valdría, pero si se introdujeran más carácteres de los definidos provocaría un error.
*/
char cadena[256];

/*
Y otra variable de tipo entero, es recomendable asignar siempre un valor, en este caso
la inicializamos con el valor mínimo, 1
*/
int ax12Id=1;

// puts es muy similar a printf, muestra la cadena que recibe como parámetro por la pantalla
puts ("nnVamos a mover un Dynamixel a voluntad. V02");

do
{    // inicio del bucle
puts ("Introduce el ID del servo a mover, entre 1 y 18, ");
ax12Id=leerEnteroComoCadena(cadena); // llamamos a esta función  para entrar por pantalla el valor
//// la admiración (!) es el operador lógico NOT. Repetiremos el bucle mientras el rango NO se valido
}while(!enRangoValido(ax12Id, 1, 18));

// Mostramos el valor introducido
printf("ID del AX12: %in", ax12Id);

return ax12Id;
}

// Repetiremos prácticamente el mismo código que la función anterior, ¿sería fácil crear una función reutilizable para ambos, verdad?
int obtenerPosicion()
{
char cadena[256];
int posicion=0;

do
{
puts ("Introduce un valor entre 0 y 1023 para situarlo en esa posicion");
posicion=leerEnteroComoCadena(cadena);
}while(!enRangoValido(posicion, 0, 1023));

printf("nPosicion: %in", posicion);

return posicion;
}

Las funciones utilizadas para leer el texto de la consola son bastante interesantes,
ya que nos muestran la utilización de una cadena. También nos muestra como utilizar
los fichero .h (declaraciones) y .c (definiciones, el contenido de las funciones):

// Cuerpo (contenido) de las funciones de entrada de datos

/*
Recibimos una variable que tiene reservado espacio en memoria para almacenar
la cadena introducida
*/

void leerCadena(char parametroCadena[])
{
int i=0; // Utilizaremos esta variable como índice para ir almacenando los caracteres introducidos
do
{
parametroCadena[i]=getchar();  // almacenamos el carácter obtenido en la posición i de parametroCadena
putchar(parametroCadena[i]);   // lo mostramos por pantalla
if (parametroCadena[i]=='b')  // si se ha pulsado la tecla <Borrar> (justo encima de <Intro>)
i--;                           //    nos situamos en la posición anterior para escribir de nuevo sobre ella
else                           // si no
i++;                           //    escribiremos en la siguiente posición
}while(parametroCadena[i-1]!='n'); // mientras el último valor guardado NO sea INTRO. El símbolo ! representa el operador NOT
parametroCadena[i]=0;           // en la última posición de una cadena ha de estar el valor cero (NULO, null en Inglés)
}

/*
Leemos una cadena, la convertimos a entero y devolvemos el valor obtenido
*/
int leerEnteroComoCadena(char parametroCadena[])
{
leerCadena(parametroCadena);
return atoi(parametroCadena);
}

El código fuente entero os lo podéis descargar desde aquí.

El lenguaje C, también C++, tiene algunas características muy interesantes, como la inclusión de código dependiendo de determinadas condiciones. Esto permite utilizar el mismo código fuente para distintos procesadores, simplemente cambiando uno o varios parámetros.

A modo de ejemplo este ZIP contiene un proyecto preparado para Dev-CPP con una versión de los ficheros fuente válida para PC y para CM-510; simplemente comentando o descomentando una línea del fichero “myCM510.h” (para utilizarlo con AVR Studio se ha de crear el correspondiente proyecto e incluir estos fuentes).

En lugar de mover el servo AX-12 muestra en pantalla el texto “dxl_write_word” con los parámetros recibidos. Podemos practicar C y realizar distintas pruebas en el PC sin conectar ningún servo.

// Si no está comentada la siguiente línea podremos compilar el programa para PC
// Si está comentada la siguiente línea podremos compilar el programa para CM-510
#define __PC_TEST_