Diff: mma7361.cpp

Revision:

0:ad6f3a862ed5

Child:

3:0359af763b6c

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mma7361.cpp	Sun Feb 01 03:56:22 2015 +0000
@@ -0,0 +1,279 @@
+#include "mbed.h"
+#include "mma7361.h"
+
+/*---------------------------------description-----------------------------------------*/
+
+//    range=0 : range is 1.5G
+//    range=1 : range is 6.0G
+
+/*-------------------------------about mat[3][3]---------------------------------------*/
+
+/*
+      | mat[0][0] mat[0][1] mat[0][2] |   | A00 A10 A20 |   : middle bits (=0G) of each axes when range is 1.5G
+      | mat[1][0] mat[1][1] mat[1][2] | = | A01 A11 A21 |   : middle bits (=0G) of each axes when range is 6.0G
+      | mat[2][0] mat[2][1] mat[2][2] |   | G00 G10 G20 |   : bits/G of each axes when range is 1.5G
+      | mat[3][0] mat[3][1] mat[3][2] |   | G01 G11 G21 |   : bits/G of each axes when range is 6.0G
+      
+      You mast change this value in "mma7361.h". You can see this value with function : calibration() & show_mat()
+*/
+
+/*---------------------------------description-----------------------------------------*/
+/*---------------------------------constracta------------------------------------------*/
+
+mma7361::mma7361(): dx(PA_0),dy(PA_1),dz(PA_4),ST(PH_0),GS(PH_1),pcdev(USBTX,USBRX)
+{
+    mma7361::ST=0;
+    mma7361::GS=0;
+    mma7361::range=0;
+    mma7361::clear_mat();
+}
+
+mma7361::mma7361(PinName accelx,PinName accely,PinName accelz): dx(accelx),dy(accely),dz(accelz),ST(PH_0),GS(PH_1),pcdev(USBTX,USBRX)
+{
+    mma7361::ST=0;
+    mma7361::GS=0;
+    mma7361::range=0;
+    mma7361::clear_mat();
+}
+
+mma7361::mma7361(PinName accelx,PinName accely,PinName accelz,PinName set): dx(accelx),dy(accely),dz(accelz),ST(set),GS(PH_1),pcdev(USBTX,USBRX)
+{
+    mma7361::ST=0;
+    mma7361::GS=0;
+    mma7361::range=0;
+    mma7361::clear_mat();
+}
+
+mma7361::mma7361(PinName accelx,PinName accely,PinName accelz,PinName set,PinName set_range): dx(accelx),dy(accely),dz(accelz),ST(set),GS(set_range),pcdev(USBTX,USBRX)
+{
+    mma7361::ST=0;
+    mma7361::GS=0;
+    mma7361::range=0;
+    mma7361::clear_mat();
+}
+
+/*---------------------------------constracta------------------------------------------*/
+/*-----------------------------------member--------------------------------------------*/
+
+void mma7361::calibration(){
+    mma7361::GS=0;
+    mma7361::range=0;
+    mma7361::ST=1;
+    wait_ms(250);
+    mma7361::ST=0;
+    
+    mma7361::pcdev.printf("Prease set devise with z-axis is going up\n\r");
+    mma7361::pcdev.printf("Press Enter key\n\r");
+    while(getchar()==-35);
+    int ax_0_0 = mma7361::read_bit(x_axis);
+    int ay_0_0 = mma7361::read_bit(y_axis);
+    int gz_0_0 = mma7361::read_bit(z_axis);
+    for(int j=0; j<100; j++){
+        mma7361::low_pass_filter(&ax_0_0,x_axis);
+        mma7361::low_pass_filter(&ay_0_0,y_axis);
+        mma7361::low_pass_filter(&gz_0_0,z_axis);
+    }
+    mma7361::GS=1;
+    wait_ms(250);
+    int ax_1_0 = mma7361::read_bit(x_axis);
+    int ay_1_0 = mma7361::read_bit(y_axis);
+    int gz_1_0 = mma7361::read_bit(z_axis);
+    for(int j=0; j<100; j++){
+        mma7361::low_pass_filter(&ax_1_0,x_axis);
+        mma7361::low_pass_filter(&ay_1_0,y_axis);
+        mma7361::low_pass_filter(&gz_1_0,z_axis);
+    }
+    mma7361::GS=0;
+    wait_ms(250);
+    
+    mma7361::pcdev.printf("Prease set devise with x-axis is going up\n\r");
+    mma7361::pcdev.printf("Press Enter key\n\r");
+    while(getchar()==-35);
+    int ay_0_1 = mma7361::read_bit(y_axis);
+    int az_0_0 = mma7361::read_bit(z_axis);
+    int gx_0_0 = mma7361::read_bit(x_axis);
+    for(int j=0; j<100; j++){
+        mma7361::low_pass_filter(&ay_0_1,y_axis);
+        mma7361::low_pass_filter(&az_0_0,z_axis);
+        mma7361::low_pass_filter(&gx_0_0,x_axis);
+    }
+    mma7361::GS=1;
+    wait_ms(250);
+    int ay_1_1 = mma7361::read_bit(y_axis);
+    int az_1_0 = mma7361::read_bit(z_axis);
+    int gx_1_0 = mma7361::read_bit(x_axis);
+    for(int j=0; j<100; j++){
+        mma7361::low_pass_filter(&ay_1_1,y_axis);
+        mma7361::low_pass_filter(&az_1_0,z_axis);
+        mma7361::low_pass_filter(&gx_1_0,x_axis);
+    }
+    mma7361::GS=0;
+    wait_ms(250);
+    
+    mma7361::pcdev.printf("Prease set devise with y-axis is going up\n\r");
+    mma7361::pcdev.printf("Press Enter key\n\r");
+    while(getchar()==-35);
+    int ax_0_1 = mma7361::read_bit(x_axis);
+    int az_0_1 = mma7361::read_bit(z_axis);
+    int gy_0_0 = mma7361::read_bit(y_axis);
+    for(int j=0; j<100; j++){
+        mma7361::low_pass_filter(&ax_0_1,x_axis);
+        mma7361::low_pass_filter(&az_0_1,z_axis);
+        mma7361::low_pass_filter(&gy_0_0,y_axis);
+    }
+    mma7361::GS=1;
+    wait_ms(250);
+    int ax_1_1 = mma7361::read_bit(x_axis);
+    int az_1_1 = mma7361::read_bit(z_axis);
+    int gy_1_0 = mma7361::read_bit(y_axis);
+    for(int j=0; j<100; j++){
+        mma7361::low_pass_filter(&ax_1_1,x_axis);
+        mma7361::low_pass_filter(&az_1_1,z_axis);
+        mma7361::low_pass_filter(&gy_1_0,y_axis);
+    }
+    mma7361::GS=0;
+    wait_ms(250);
+    
+    ax_0_0=(ax_0_0+ax_0_1)/2;
+    ay_0_0=(ay_0_0+ay_0_1)/2;
+    az_0_0=(az_0_0+az_0_1)/2;
+    ax_1_0=(ax_1_0+ax_1_1)/2;
+    ay_1_0=(ay_1_0+ay_1_1)/2;
+    az_1_0=(az_1_0+az_1_1)/2;
+    gx_0_0-=ax_0_0;
+    gy_0_0-=ay_0_0;
+    gz_0_0-=az_0_0;
+    gx_1_0-=ax_0_1;
+    gy_1_0-=ay_0_1;
+    gz_1_0-=az_0_1;
+    
+    mma7361::set_each_num(ax_0_0,ay_0_0,az_0_0,ax_1_0,ay_1_0,az_1_0,gx_0_0,gy_0_0,gz_0_0,gx_1_0,gy_1_0,gz_1_0,0);
+    wait_ms(100);
+}
+
+void mma7361::set_each_num(int ax_0_0,int ay_0_0,int az_0_0,int ax_1_0,int ay_1_0,int az_1_0,int gx_0_0,int gy_0_0,int gz_0_0,int gx_1_0,int gy_1_0,int gz_1_0,int new_range){
+    //def or -1 -> don' change value
+    mma7361::GS=new_range;
+    mma7361::range=new_range;
+    int temp[12];
+    temp[0]=ax_0_0;
+    temp[1]=ay_0_0;
+    temp[2]=az_0_0;
+    temp[3]=ax_1_0;
+    temp[4]=ay_1_0;
+    temp[5]=az_1_0;
+    temp[6]=gx_0_0;
+    temp[7]=gy_0_0;
+    temp[8]=gz_0_0;
+    temp[9]=gx_1_0;
+    temp[10]=gy_1_0;
+    temp[11]=gz_1_0;
+    for(int i=0; i<4; i++){
+        for(int j=0; j<3; j++){
+            if(temp[i*3+j]!=def){
+                mma7361::mat[i][j]=temp[i*3+j];
+            }
+        }
+    }
+}
+
+void mma7361::set_num(int *new_mat,int new_range){
+    mma7361::GS=new_range;
+    mma7361::range=new_range;
+    for(int i=0; i<4; i++){
+        for(int j=0; j<3; j++){
+            if(new_mat[i*3+j]!=def){
+                mma7361::mat[i][j]=new_mat[i*3+j];
+            }
+        }
+    }
+}
+
+void mma7361::set_range(int new_range){
+    mma7361::GS=new_range;
+    mma7361::range=new_range;
+}
+
+void mma7361::clear_mat(){
+    mma7361::set_each_num(A00,A10,A20,A01,A11,A21,G00,G10,G20,G01,G11,G21,mma7361::range);
+}
+
+int mma7361::read_bit(int axis){
+    int v;
+    switch(axis){
+    case 0:
+        v=mma7361::dx.read_u16();
+        break;
+    case 1:
+        v=mma7361::dy.read_u16();
+        break;
+    case 2:
+        v=mma7361::dz.read_u16();
+        break;
+    default:
+        v=def;
+        break;
+    }
+    return v;
+}
+
+int mma7361::read_bit_0(int axis){
+    int v;
+    v=mma7361::read_bit(axis);
+    v-=mat[mma7361::range][axis];
+    return v;
+}
+
+float mma7361::read_by_g(int axis){
+    float v;
+    v=mma7361::read_bit_0(axis);
+    v/=mat[mma7361::range+2][axis];
+    return v;
+}
+    
+void mma7361::low_pass_filter(int *int_bit,int axis){
+    *int_bit=*int_bit*0.9+mma7361::read_bit(axis)*0.1;
+}
+
+void mma7361::show_mat(){
+    for(int i=0; i<4; i++){
+        for(int j=0; j<3; j++){
+            mma7361::pcdev.printf("%d ",mat[i][j]);
+        }
+        mma7361::pcdev.printf("\n\r");
+    }
+}
+
+float mma7361::bit_to_g(int bit,int axis){
+    float v=0;
+    v=bit-mma7361::mat[mma7361::range][axis];
+    v/=mma7361::mat[mma7361::range+2][axis];
+    return v;
+}
+
+void mma7361::set_1st_bit(int *variable,int axis,float direction_G){
+    *variable=mma7361::mat[mma7361::range][axis]+mma7361::mat[mma7361::range+2][axis]*direction_G;
+}
+
+
+float mma7361::rotate(int *a_value,int a_axis,int *b_value,int b_axis){
+    float ga,gb,g;
+    ga=mma7361::bit_to_g(*a_value,a_axis);
+    gb=mma7361::bit_to_g(*b_value,b_axis);
+    if(abs(ga)<THHO){
+        ga=0;
+    }
+    if(abs(gb)<THHO){
+        return 0;
+    }else{
+        g=ga/gb;
+        g=atan(g);
+        return g;
+    }
+}
+
+float mma7361::rad_to_deg(float rad){
+    float deg;
+    deg=rad*180/PI;
+    return deg;
+}
\ No newline at end of file