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

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.

[caption id="attachment_660" align="aligncenter" width="300" caption="DMS sensor"][/caption]

La función "main" es así de sencilla:
[sourcecode language="c"]
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;
}
[/sourcecode]

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

[sourcecode language="c"]
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);
}
}
}
[/sourcecode]

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:

[sourcecode language="c"]
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;
}
[/sourcecode]

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