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.

DMS sensor

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.

Series Navigation<< Programacion C con CM-510: leyendo y moviendo Dynamixel AX-12 (I)<< Programacion C con CM-510: leyendo y moviendo Dynamixel AX-12 (II)
%d bloggers like this: