IMU with IIC
Dependencies: FastAnalogIn mbed
main.cpp
- Committer:
- efrain95
- Date:
- 2017-03-21
- Revision:
- 0:51c8d005e429
File content as of revision 0:51c8d005e429:
#include "mbed.h" #include "FastAnalogIn.h" Serial pc(USBTX, USBRX); //Configuración de los pines deñ serial DigitalInOut SDA(PTB3); //Pin para el SDA DigitalOut SCL(PTB2);//Pin para el SCL typedef signed short i16; const unsigned char PI =3.14159265; const unsigned char ADDRESS_READ = 0b11010001; //address of GY-87 for read const unsigned char ADDRESS_WRITE = 0b11010000; //addres of GY-87 for write const unsigned char ACCEL_CONFIGURATION = 0x00;// // Full scale range = +/-2g const unsigned char accel_reg_dir = 0x1C; //Address acelerometer register const unsigned char accel_data = 0x3B; double lsb=16384.0; //sensibilidad del acelerometro double angleX,angleY,angleZ,forceX,forceY,forceZ,x; //Variables para el ángulo y fuerza i16 ACCELX; i16 ACCELY; i16 ACCELZ; unsigned char data_accel_xyz[6]; void IIC_init (void) { SCL=0; SDA=0; SDA.output();//conf SDA salida asm("nop"); asm("nop"); asm("nop"); asm("nop"); } void IIC_start (void) { SDA=1; SCL=1; asm("nop"); asm("nop"); SDA=0; asm("nop"); //esperar 1u con nop's SCL=0; asm("nop"); asm("nop"); } void IIC_stop (void) { SCL=1; //esperar TSU.STO STOP Condition Setup Time con nop's 600 ns min asm("nop"); SDA=1; //esperar tBUF, Bus Free Time Between STOP and START Condition 1.3us min asm("nop"); asm("nop"); SCL=0; SDA=0; } unsigned char IIC_read_byte (void) { unsigned char temp; unsigned char cont; cont=8; SDA.input();//configurar SDA como entrada do { temp<<=1; SCL=1; if (SDA==1) temp |= 0x01; asm("nop"); asm("nop"); SCL=0; asm("nop"); asm("nop"); }while (--cont); SDA.output();//configurar SDA como salida return temp; } void IIC_ACK_output (unsigned char ACK_output) { if (ACK_output==0) SDA=0; else SDA=1; asm("nop"); SCL=1; asm("nop"); asm("nop"); asm("nop"); SCL=0; SDA=0; } void error() { while(true){} } void IIC_send_byte (unsigned char byte) { unsigned char data; unsigned char i=8; data=byte; do{ if (data&0x80) { SDA=1; } else { SDA=0; } SCL=1; asm("nop"); asm("nop"); asm("nop"); SCL=0; asm("nop"); asm("nop"); asm("nop"); data<<=1; }while (--i); SDA=0; } unsigned char IIC_ACK_inp (void) { unsigned char temp; SDA.input();//SDA entrada SCL=1; if (SDA==0) temp=0; else temp=1; SCL=0; SDA.output();//SDA salida return temp; } void IIC_byte_write(const unsigned char direccion, unsigned char dato) { IIC_start(); IIC_send_byte(ADDRESS_WRITE); if (IIC_ACK_inp()==1) error(); IIC_send_byte((char)direccion); if (IIC_ACK_inp()==1) error(); IIC_send_byte(dato); if (IIC_ACK_inp()==1) error(); IIC_stop(); } void IIC_sequential_read(const unsigned int direccion, unsigned char num_bytes) { unsigned char restador=num_bytes; unsigned char iterador = 0; IIC_start(); IIC_send_byte(ADDRESS_WRITE); if (IIC_ACK_inp()==1) error(); IIC_send_byte((char)direccion); if (IIC_ACK_inp()==1) error(); IIC_start(); IIC_send_byte(ADDRESS_READ); if (IIC_ACK_inp()==1) error(); do{ data_accel_xyz[iterador++]=IIC_read_byte(); IIC_ACK_output(0); }while(--restador); IIC_ACK_output(1); IIC_stop(); } void Data_converter() //Función para pasar los datos de 8 bits a las variables de 16 bits { ACCELX = data_accel_xyz[0]<<8 | data_accel_xyz[1]; ACCELY = data_accel_xyz[2]<<8; ACCELY|= data_accel_xyz[3]; ACCELZ = data_accel_xyz[4]<<8; ACCELZ|= data_accel_xyz[5]; } void ANGULOS(){ //Función para sacar los ángulos correspondientes de cada eje angleX=atan((double)ACCELX/(sqrt((double)(ACCELZ*ACCELZ+ACCELY*ACCELY))))*180/PI; angleY=atan((double)ACCELY/(sqrt((double)(ACCELZ*ACCELZ+ACCELX*ACCELX))))*180/PI; angleZ=atan((double)ACCELZ/(sqrt((double)(ACCELX*ACCELX+ACCELY*ACCELY))))*180/PI; } void FORCE(){ //Función para sacar la fuerza respecto a cada eje x=(double)ACCELX; forceX=x/lsb; forceY=(double)ACCELY/(16384.0); forceZ=(double)ACCELZ/(16384.0); } int main() { IIC_init(); //configuracion del registro acelerometro IIC_byte_write(0x6B,0b00000000); IIC_init(); IIC_byte_write(accel_reg_dir,ACCEL_CONFIGURATION ); while(true) { IIC_init(); IIC_sequential_read(accel_data,6); Data_converter(); ANGULOS(); FORCE(); pc.printf("Angle: X:%f Y:%f Z:%f \n",angleX,angleY,angleZ); pc.printf("\r"); pc.printf("Ac(g): X:%f Y:%f Z:%f \n",forceX,forceY,forceZ); pc.printf("\r"); } }