Siguiendo con este cacharrito les comento que ya tengo lectura del ángulo usando el acelerometro en conjunto del gyro los sensores LSM303DLHC y L3GD20 respectivamente, realize una libreria para hacer las lecturas del eje que en mi caso como acomode la pcb es el eje Y les dejo la libreria por si alguien se anima a usarla y si se puede mejorarla pues sera mejor, el codigo es para CCS yo estoy usando el PIC16f1939 pero se puede ajustar a cualquiera. Las resistencias pull-up para el bus de i2c las tengo de 4.7Kohms.
Para usarla unicamente se llama a la función leer_angulo(); dando como argumento el tiempo de muestreo en ms
void main(){
float angulo;
int ts=20;
gyro_init(); //inicialización de gyro
accel_init(); //inicialización de acel
while(1){
angulo=leer_angulo(ts);
delay_ms(ts);
}
}
Aquí la librería IMU.h:
/*********************************************
Libreria para leer IMU
Acelerometro y Magnetometro LSM303DHLHC
Giroscopio L3GD20
Ing. Gabriel Casarrubias Guerrero
*********************************************/
#use i2c(Master, sda=PIN_C4, scl=PIN_C3)
#include <MATH.h>
float olda=0; //angulo en el instante anterior
//****************** Registros de configuración de acelerometro
#define escriacce 0x32 //dirección de escritura para acelerometro
#define lectacce 0x33 //dirección de lectura para acelerometro
#define REG1_A 0x20 //registro de configuración 1
#define REG2_A 0x21 //registro de configuración 2
#define REG3_A 0X22 //registro de configuración 3
#define REG4_A 0X23 //registro de configuración 4
#define REG5_A 0X24 //registro de configuración 5
#define REG6_A 0X25 //registro de configuración 6
//***************** Registros de configuración de magnetometro
#define magneton 0x3c //dirección de magnetometro
#define LSM303_MR_REG_M 0x02
//******************* Registros de configuración de gyro
#define escrigiro 0xD6 //Dirección de escritura para giro
#define lectgiro 0xD7 //Dirección de lectura para giro
#define REG1_G 0x20 //registro de configuración 1
#define REG2_G 0x21 //registro de configuración 2
#define REG3_G 0X22 //registro de configuración 3
#define REG4_G 0X23 //registro de configuración 4
#define REG5_G 0X24 //registro de configuración 5
//******************* Registros de lectura para IMU acelerometro y gyro
#define OUT_X_L 0x28
#define OUT_X_H 0x29
#define OUT_Y_L 0x2A
#define OUT_Y_H 0x2B
#define OUT_Z_L 0x2C
#define OUT_Z_H 0x2D
//******************************************************************************
//**********Función para escribir un dato a los sensores************************
// dirección=dirección del dispositivo
// reg= registro al cual se escribe
// valor= valor que se escribira
void escribir_dato(int direccion,int reg, int valor){
i2c_start();
i2c_write(direccion);
i2c_write(reg);
i2c_write(valor);
i2c_stop();
}
//******************** funciones manejo de acelerometro*************************
//funcion que inicializa el accelerometro;
void accel_init(){
escribir_dato(escriacce,REG1_A,0b01100111); //modo normal a 200Hz
escribir_dato(escriacce,REG2_A,0);
escribir_dato(escriacce,REG3_A,0);
escribir_dato(escriacce,REG4_A,0b00001001);//actualización continua, justificado a la izquierda, rango de +-2g,high resolution
escribir_dato(escriacce,REG5_A,0);
escribir_dato(escriacce,REG6_A,0);
}
//funcion para leer el dato del accelerometro
// reg= registro bajo que se leera
int16 leer_accel(int8 reg){
int16 dato;
int Al,Ah;
i2c_start();
i2c_write(escriacce);
i2c_write(reg);
i2c_start();
i2c_write(lectacce);
Al=i2c_read(0);
i2c_stop();
//lectura registro alto
i2c_start();
i2c_write(escriacce);
i2c_write(reg+1);
i2c_start();
i2c_write(lectacce);
Ah=i2c_read(0);
i2c_stop();
dato=Ah;
dato=dato<<8; // Ah:Al
dato=dato+Al; //dato de la forma AD:C0 donde el valor es AD:C y los primeeros 4 bits no cuentan
//entonces se recorre el dato a la derecha 4 bits.
dato=dato>>4;
return(dato); //dato leido del acelerometro
}
//********funcion para convertir el dato del accelerometro a g
// dato= dato que se convertira
float convertirg(int16 dato){
float salida;
salida=(float)dato;
salida=salida*.001; //resolucíon en +-2g=1mg/LSB
if(salida>=3.0) //condición para detectar los negativos
salida=-(4.1-salida);
return(salida);
}
// funcion para convertir de g a grados
// dato= dato a convertir
float caccelgrados(float dato){
float grados;
grados=asin(dato)*180/3.14158; //dato=g*sen(Theta) radianes para grados se multiplica por 180/pi
return(grados);
}
//funcion para convertir de g a radianes
// dato=dato a convertir
float caccelradianes(float dato){
float grados;
grados=asin(dato);
return(grados);
}
//******************* funciones manejo de gyroscopio***************************
// función para inicializar el gyro
void gyro_init(){
escribir_dato(escrigiro,REG1_G,0b10101111);
//escribir_dato(escrigiro,REG2_G,0b00000000);
//escribir_dato(escrigiro,REG3_G,0b00000000);
escribir_dato(escrigiro,REG4_G,0b00000001);
escribir_dato(escrigiro,REG5_G,0);
}
//función para leer el dato del giro
// reg= registro parte baja que se leera
int16 leer_gyro(int8 reg){
int16 dato;
int Al,Ah;
i2c_start();
i2c_write(escrigiro);
i2c_write(reg);
i2c_start();
i2c_write(lectgiro);
Al=i2c_read(0);
i2c_stop();
//lectura de registro parte alta
i2c_start();
i2c_write(escrigiro);
i2c_write(reg+1);
i2c_start();
i2c_write(lectgiro);
Ah=i2c_read(0);
i2c_stop();
dato=Ah;
dato=dato<<8;
dato=dato+Al;
dato=dato>>4;
//printf(lcd_putc,"\f Ah: %d+%d",Ah,Al);
return(dato);
}
//funcion para convertir a grados por segundo el dato del gyro
// dato=dato a convertir
float convertirdps(int16 dato){
float salida;
salida=(float)dato;
salida=salida*0.00875; //resolucion en 250dps=8.75mdps/LSB
return(salida);
}
//función que conjunta las mediciones para obtener una lectura corregida
// dato = dato en dps del gyro
// old= ángulo en un instante anterior
// tm= tiempo de muestreo
// Aacc= angulo del acelerometro
float cgyrogrados(float dato,float old,int tm,float Aacc){
float angulo;
float a=0.8;
angulo=((old+(dato*tm/1000))*a+(Aacc*(1-a)));// ángulo=(old+dato*ts)*a+Aacc*(1-a);
//donde a es el grado de confiabilidad del gyro con respecto al acelerometro dependiendo lo observado.
return(angulo);
}
//funcion que tenemos que llamar para leer el angulo en el eje Y para leer otro cambiar OUT_Y_L por OUT_X_L u OUT_Z_L
//tm= tiempo de muestreo para hacer la correción
float leer_angulo(int tm){
float Ya,Yg,Aya,Ayg;
int16 Yda,Ydg;
Yda=leer_accel(OUT_Y_L); //lee el dato del acelerometro
Ydg=leer_gyro(OUT_Y_L); //lee el dato del gyro
Ya=convertirg(Yda); //convierte a g el dato del acelerometro
Yg=convertirdps(Ydg); //convierte a º/s el dato del gyro
Aya=caccelgrados(Ya); //convierte a º las g
Ayg=cgyrogrados(Yg,olda,tm,Aya); //convierte a º las dps corrigiendo con el angulo del accelerometro
olda=Ayg; //actualiza el instante anterior
return(Ayg);
}
