Diff: main.cpp

Revision:

0:51c8d005e429

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Tue Mar 21 06:22:38 2017 +0000
@@ -0,0 +1,214 @@
+#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");
+       
+    }
+}