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.
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.