Efrain Duarte / Mbed 2 deprecated IMU

IMU with IIC

Dependencies:   FastAnalogIn mbed

main.cpp@0:51c8d005e429, 2017-03-21 (annotated)

Committer:
efrain95
Date:
Tue Mar 21 06:22:38 2017 +0000
Revision:
0:51c8d005e429
IMU-IIC

Who changed what in which revision?

UserRevisionLine numberNew contents of line
efrain95 0:51c8d005e429 1 #include "mbed.h"
efrain95 0:51c8d005e429 2 #include "FastAnalogIn.h"
efrain95 0:51c8d005e429 3
efrain95 0:51c8d005e429 4 Serial pc(USBTX, USBRX); //Configuración de los pines deñ serial
efrain95 0:51c8d005e429 5 DigitalInOut SDA(PTB3); //Pin para el SDA
efrain95 0:51c8d005e429 6 DigitalOut SCL(PTB2);//Pin para el SCL
efrain95 0:51c8d005e429 7
efrain95 0:51c8d005e429 8 typedef signed short i16;
efrain95 0:51c8d005e429 9 const unsigned char PI =3.14159265;
efrain95 0:51c8d005e429 10
efrain95 0:51c8d005e429 11 const unsigned char ADDRESS_READ = 0b11010001; //address of GY-87 for read
efrain95 0:51c8d005e429 12 const unsigned char ADDRESS_WRITE = 0b11010000; //addres of GY-87 for write
efrain95 0:51c8d005e429 13 const unsigned char ACCEL_CONFIGURATION = 0x00;// // Full scale range = +/-2g
efrain95 0:51c8d005e429 14
efrain95 0:51c8d005e429 15 const unsigned char accel_reg_dir = 0x1C; //Address acelerometer register
efrain95 0:51c8d005e429 16 const unsigned char accel_data = 0x3B;
efrain95 0:51c8d005e429 17 double lsb=16384.0; //sensibilidad del acelerometro
efrain95 0:51c8d005e429 18
efrain95 0:51c8d005e429 19 double angleX,angleY,angleZ,forceX,forceY,forceZ,x; //Variables para el ángulo y fuerza
efrain95 0:51c8d005e429 20 i16 ACCELX;
efrain95 0:51c8d005e429 21 i16 ACCELY;
efrain95 0:51c8d005e429 22 i16 ACCELZ;
efrain95 0:51c8d005e429 23
efrain95 0:51c8d005e429 24 unsigned char data_accel_xyz[6];
efrain95 0:51c8d005e429 25
efrain95 0:51c8d005e429 26 void IIC_init (void)
efrain95 0:51c8d005e429 27 {
efrain95 0:51c8d005e429 28 SCL=0;
efrain95 0:51c8d005e429 29 SDA=0;
efrain95 0:51c8d005e429 30 SDA.output();//conf SDA salida
efrain95 0:51c8d005e429 31 asm("nop");
efrain95 0:51c8d005e429 32 asm("nop");
efrain95 0:51c8d005e429 33 asm("nop");
efrain95 0:51c8d005e429 34 asm("nop");
efrain95 0:51c8d005e429 35 }
efrain95 0:51c8d005e429 36
efrain95 0:51c8d005e429 37 void IIC_start (void)
efrain95 0:51c8d005e429 38 {
efrain95 0:51c8d005e429 39 SDA=1;
efrain95 0:51c8d005e429 40 SCL=1;
efrain95 0:51c8d005e429 41 asm("nop");
efrain95 0:51c8d005e429 42 asm("nop");
efrain95 0:51c8d005e429 43 SDA=0;
efrain95 0:51c8d005e429 44 asm("nop"); //esperar 1u con nop's
efrain95 0:51c8d005e429 45 SCL=0;
efrain95 0:51c8d005e429 46 asm("nop");
efrain95 0:51c8d005e429 47 asm("nop");
efrain95 0:51c8d005e429 48 }
efrain95 0:51c8d005e429 49
efrain95 0:51c8d005e429 50 void IIC_stop (void)
efrain95 0:51c8d005e429 51 {
efrain95 0:51c8d005e429 52 SCL=1;
efrain95 0:51c8d005e429 53 //esperar TSU.STO STOP Condition Setup Time con nop's 600 ns min
efrain95 0:51c8d005e429 54 asm("nop");
efrain95 0:51c8d005e429 55 SDA=1;
efrain95 0:51c8d005e429 56 //esperar tBUF, Bus Free Time Between STOP and START Condition 1.3us min
efrain95 0:51c8d005e429 57 asm("nop");
efrain95 0:51c8d005e429 58 asm("nop");
efrain95 0:51c8d005e429 59 SCL=0;
efrain95 0:51c8d005e429 60 SDA=0;
efrain95 0:51c8d005e429 61 }
efrain95 0:51c8d005e429 62
efrain95 0:51c8d005e429 63 unsigned char IIC_read_byte (void)
efrain95 0:51c8d005e429 64 {
efrain95 0:51c8d005e429 65 unsigned char temp;
efrain95 0:51c8d005e429 66 unsigned char cont;
efrain95 0:51c8d005e429 67 cont=8;
efrain95 0:51c8d005e429 68 SDA.input();//configurar SDA como entrada
efrain95 0:51c8d005e429 69 do
efrain95 0:51c8d005e429 70 {
efrain95 0:51c8d005e429 71 temp<<=1;
efrain95 0:51c8d005e429 72 SCL=1;
efrain95 0:51c8d005e429 73 if (SDA==1) temp |= 0x01;
efrain95 0:51c8d005e429 74 asm("nop");
efrain95 0:51c8d005e429 75 asm("nop");
efrain95 0:51c8d005e429 76 SCL=0;
efrain95 0:51c8d005e429 77 asm("nop");
efrain95 0:51c8d005e429 78 asm("nop");
efrain95 0:51c8d005e429 79 }while (--cont);
efrain95 0:51c8d005e429 80 SDA.output();//configurar SDA como salida
efrain95 0:51c8d005e429 81 return temp;
efrain95 0:51c8d005e429 82 }
efrain95 0:51c8d005e429 83
efrain95 0:51c8d005e429 84 void IIC_ACK_output (unsigned char ACK_output)
efrain95 0:51c8d005e429 85 {
efrain95 0:51c8d005e429 86 if (ACK_output==0) SDA=0;
efrain95 0:51c8d005e429 87 else SDA=1;
efrain95 0:51c8d005e429 88 asm("nop");
efrain95 0:51c8d005e429 89 SCL=1;
efrain95 0:51c8d005e429 90 asm("nop");
efrain95 0:51c8d005e429 91 asm("nop");
efrain95 0:51c8d005e429 92 asm("nop");
efrain95 0:51c8d005e429 93 SCL=0;
efrain95 0:51c8d005e429 94 SDA=0;
efrain95 0:51c8d005e429 95 }
efrain95 0:51c8d005e429 96
efrain95 0:51c8d005e429 97 void error()
efrain95 0:51c8d005e429 98 {
efrain95 0:51c8d005e429 99 while(true){}
efrain95 0:51c8d005e429 100 }
efrain95 0:51c8d005e429 101
efrain95 0:51c8d005e429 102 void IIC_send_byte (unsigned char byte)
efrain95 0:51c8d005e429 103 {
efrain95 0:51c8d005e429 104 unsigned char data;
efrain95 0:51c8d005e429 105 unsigned char i=8;
efrain95 0:51c8d005e429 106 data=byte;
efrain95 0:51c8d005e429 107 do{
efrain95 0:51c8d005e429 108 if (data&0x80)
efrain95 0:51c8d005e429 109 {
efrain95 0:51c8d005e429 110 SDA=1;
efrain95 0:51c8d005e429 111 }
efrain95 0:51c8d005e429 112 else
efrain95 0:51c8d005e429 113 {
efrain95 0:51c8d005e429 114 SDA=0;
efrain95 0:51c8d005e429 115 }
efrain95 0:51c8d005e429 116 SCL=1;
efrain95 0:51c8d005e429 117 asm("nop");
efrain95 0:51c8d005e429 118 asm("nop");
efrain95 0:51c8d005e429 119 asm("nop");
efrain95 0:51c8d005e429 120 SCL=0;
efrain95 0:51c8d005e429 121 asm("nop");
efrain95 0:51c8d005e429 122 asm("nop");
efrain95 0:51c8d005e429 123 asm("nop");
efrain95 0:51c8d005e429 124 data<<=1;
efrain95 0:51c8d005e429 125 }while (--i);
efrain95 0:51c8d005e429 126 SDA=0;
efrain95 0:51c8d005e429 127 }
efrain95 0:51c8d005e429 128
efrain95 0:51c8d005e429 129 unsigned char IIC_ACK_inp (void)
efrain95 0:51c8d005e429 130 {
efrain95 0:51c8d005e429 131 unsigned char temp;
efrain95 0:51c8d005e429 132 SDA.input();//SDA entrada
efrain95 0:51c8d005e429 133 SCL=1;
efrain95 0:51c8d005e429 134 if (SDA==0) temp=0;
efrain95 0:51c8d005e429 135 else temp=1;
efrain95 0:51c8d005e429 136 SCL=0;
efrain95 0:51c8d005e429 137 SDA.output();//SDA salida
efrain95 0:51c8d005e429 138 return temp;
efrain95 0:51c8d005e429 139 }
efrain95 0:51c8d005e429 140 void IIC_byte_write(const unsigned char direccion, unsigned char dato)
efrain95 0:51c8d005e429 141 {
efrain95 0:51c8d005e429 142 IIC_start();
efrain95 0:51c8d005e429 143 IIC_send_byte(ADDRESS_WRITE);
efrain95 0:51c8d005e429 144 if (IIC_ACK_inp()==1)
efrain95 0:51c8d005e429 145 error();
efrain95 0:51c8d005e429 146 IIC_send_byte((char)direccion);
efrain95 0:51c8d005e429 147 if (IIC_ACK_inp()==1) error();
efrain95 0:51c8d005e429 148 IIC_send_byte(dato);
efrain95 0:51c8d005e429 149 if (IIC_ACK_inp()==1) error();
efrain95 0:51c8d005e429 150 IIC_stop();
efrain95 0:51c8d005e429 151 }
efrain95 0:51c8d005e429 152
efrain95 0:51c8d005e429 153 void IIC_sequential_read(const unsigned int direccion, unsigned char num_bytes)
efrain95 0:51c8d005e429 154 {
efrain95 0:51c8d005e429 155 unsigned char restador=num_bytes;
efrain95 0:51c8d005e429 156 unsigned char iterador = 0;
efrain95 0:51c8d005e429 157 IIC_start();
efrain95 0:51c8d005e429 158 IIC_send_byte(ADDRESS_WRITE);
efrain95 0:51c8d005e429 159 if (IIC_ACK_inp()==1) error();
efrain95 0:51c8d005e429 160 IIC_send_byte((char)direccion);
efrain95 0:51c8d005e429 161 if (IIC_ACK_inp()==1) error();
efrain95 0:51c8d005e429 162 IIC_start();
efrain95 0:51c8d005e429 163 IIC_send_byte(ADDRESS_READ);
efrain95 0:51c8d005e429 164 if (IIC_ACK_inp()==1) error();
efrain95 0:51c8d005e429 165
efrain95 0:51c8d005e429 166 do{
efrain95 0:51c8d005e429 167 data_accel_xyz[iterador++]=IIC_read_byte();
efrain95 0:51c8d005e429 168 IIC_ACK_output(0);
efrain95 0:51c8d005e429 169 }while(--restador);
efrain95 0:51c8d005e429 170 IIC_ACK_output(1);
efrain95 0:51c8d005e429 171 IIC_stop();
efrain95 0:51c8d005e429 172 }
efrain95 0:51c8d005e429 173
efrain95 0:51c8d005e429 174 void Data_converter() //Función para pasar los datos de 8 bits a las variables de 16 bits
efrain95 0:51c8d005e429 175 {
efrain95 0:51c8d005e429 176 ACCELX = data_accel_xyz[0]<<8 | data_accel_xyz[1];
efrain95 0:51c8d005e429 177 ACCELY = data_accel_xyz[2]<<8;
efrain95 0:51c8d005e429 178 ACCELY|= data_accel_xyz[3];
efrain95 0:51c8d005e429 179 ACCELZ = data_accel_xyz[4]<<8;
efrain95 0:51c8d005e429 180 ACCELZ|= data_accel_xyz[5];
efrain95 0:51c8d005e429 181 }
efrain95 0:51c8d005e429 182
efrain95 0:51c8d005e429 183 void ANGULOS(){ //Función para sacar los ángulos correspondientes de cada eje
efrain95 0:51c8d005e429 184 angleX=atan((double)ACCELX/(sqrt((double)(ACCELZ*ACCELZ+ACCELY*ACCELY))))*180/PI;
efrain95 0:51c8d005e429 185 angleY=atan((double)ACCELY/(sqrt((double)(ACCELZ*ACCELZ+ACCELX*ACCELX))))*180/PI;
efrain95 0:51c8d005e429 186 angleZ=atan((double)ACCELZ/(sqrt((double)(ACCELX*ACCELX+ACCELY*ACCELY))))*180/PI;
efrain95 0:51c8d005e429 187 }
efrain95 0:51c8d005e429 188 void FORCE(){ //Función para sacar la fuerza respecto a cada eje
efrain95 0:51c8d005e429 189 x=(double)ACCELX;
efrain95 0:51c8d005e429 190 forceX=x/lsb;
efrain95 0:51c8d005e429 191 forceY=(double)ACCELY/(16384.0);
efrain95 0:51c8d005e429 192 forceZ=(double)ACCELZ/(16384.0);
efrain95 0:51c8d005e429 193
efrain95 0:51c8d005e429 194 }
efrain95 0:51c8d005e429 195
efrain95 0:51c8d005e429 196 int main() {
efrain95 0:51c8d005e429 197 IIC_init();
efrain95 0:51c8d005e429 198 //configuracion del registro acelerometro
efrain95 0:51c8d005e429 199 IIC_byte_write(0x6B,0b00000000);
efrain95 0:51c8d005e429 200 IIC_init();
efrain95 0:51c8d005e429 201 IIC_byte_write(accel_reg_dir,ACCEL_CONFIGURATION );
efrain95 0:51c8d005e429 202 while(true) {
efrain95 0:51c8d005e429 203 IIC_init();
efrain95 0:51c8d005e429 204 IIC_sequential_read(accel_data,6);
efrain95 0:51c8d005e429 205 Data_converter();
efrain95 0:51c8d005e429 206 ANGULOS();
efrain95 0:51c8d005e429 207 FORCE();
efrain95 0:51c8d005e429 208 pc.printf("Angle: X:%f Y:%f Z:%f \n",angleX,angleY,angleZ);
efrain95 0:51c8d005e429 209 pc.printf("\r");
efrain95 0:51c8d005e429 210 pc.printf("Ac(g): X:%f Y:%f Z:%f \n",forceX,forceY,forceZ);
efrain95 0:51c8d005e429 211 pc.printf("\r");
efrain95 0:51c8d005e429 212
efrain95 0:51c8d005e429 213 }
efrain95 0:51c8d005e429 214 }