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?
User | Revision | Line number | New 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 | } |