これは MMA7361 3軸アナログ加速度センサ の Nucleo F401RE 用のライブラリです。詳しい説明はLibrary中のReadMe.hに記載しています。文字化けや開けない場合はダウンロードしてみてください。Google Chrome では見れるはずです。
Dependents: Nucleo_L3GD20_MMA7361_Kalman
mma7361.cpp
- Committer:
- hirokimineshita
- Date:
- 2015-02-01
- Revision:
- 3:0359af763b6c
- Parent:
- 0:ad6f3a862ed5
- Child:
- 5:83a3021f51f8
File content as of revision 3:0359af763b6c:
#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)[3],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][j]!=def){ mma7361::mat[i][j]=new_mat[i][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; }