main.cpp

00001 /*
00002 EPSONのセンサを試すために組んだ20210423。
00003 */
00004 
00005 #include "mbed.h"
00006 #include "string.h"
00007 
00008 #include "QMC5883L.h"//地磁気センサのライブラリ
00009 #include "SDFileSystem.h"//sdカードを操作するためのライブラリ
00010 
00011 #define SCALE_FACTOR_GYRO 0.016
00012 #define SCALE_FACTOR_ACC 0.20
00013 #define GRAVITIONAL_ACCELERATION 9.80665
00014 
00015 Serial pc(USBTX, USBRX); // tx, rx
00016 //QMC5883L compass(p9,p10);
00017 //Serial device(p9, p10);  // tx, rx
00018 Serial device(p28, p27);  // tx, rx
00019 SDFileSystem sd(p5, p6, p7, p8, "sd"); // the pinout on the mbed Cool Components workshop board
00020 DigitalIn switch_blue(p21);//sdカードへの書き込みを終了させる
00021 
00022 /*-----------------プロトタイプ宣言----------------*/
00023 void pc_rx ();
00024 void dev_rx ();
00025 void call_window1();
00026 void call_window0();
00027 void GLOB_CMD_read();
00028 void DIAG_STAT_read();
00029 void UART_CTRL_write();
00030 /*サンプリングモードへの移行*/
00031 void move_to_sampling_mode();
00032 /*IMUが動作可能かどうかの確認*/
00033 void power_on_sequence1();
00034 /*IMUが動作可能かどうかの確認*/
00035 void power_on_sequence2();
00036 
00037 /*X軸周りの角速度を算出*/
00038 float read_angular_rate_x();
00039 /*Y軸周りの角速度を算出*/
00040 float read_angular_rate_y();
00041 /*Z軸周りの角速度を算出*/
00042 float read_angular_rate_z();
00043 
00044 /*X軸の加速度を算出*/
00045 float read_acceleration_x();
00046 /*Y軸の加速度を算出*/
00047 float read_acceleration_y();
00048 /*Z軸の加速度を算出*/
00049 float read_acceleration_z();
00050 /*-----------------------------------------------*/
00051 /*-------------タイマー関数---------------*/
00052 Timer passed_time;
00053 /*---------------------------------------*/
00054 /*-------------------各種の変数-----------------*/
00055 float gyro_val[3];//角速度の値
00056 float angle_val[3];//角度の値
00057 
00058 float acc_val[3];//加速度の値
00059 float acc_angle[2];//加速度から計算した角度の値
00060 
00061 int16_t mag_val[3];//地磁気の値
00062 
00063 float time_new,time_old;
00064 /*---------------------------------------------*/
00065 
00066 
00067 int main() {
00068     
00069     pc.baud(460800);
00070     device.baud(460800);
00071     
00072     pc.printf("Baud rate setting\r\n");
00073         
00074    // pc.attach(pc_rx, Serial::RxIrq);
00075    //device.attach(dev_rx, Serial::RxIrq);
00076     
00077     /*
00078     power on
00079     wait 800ms
00080     */
00081     wait(1.0);
00082     
00083     //compass.init();//地磁気センサの起動
00084     
00085     power_on_sequence1();//IMUが動作可能かどうかの確認
00086     power_on_sequence2();//IMUが動作可能かどうかの確認
00087     UART_CTRL_write();//IMUのボーレートを480600,手動モードへ移行
00088     move_to_sampling_mode();//サンプリングモードへの移行
00089     
00090     passed_time.start();
00091     time_old=passed_time.read();
00092     
00093     /*ジャイロで計算する初期角度の設定*/
00094     angle_val[0]=0.0;
00095     angle_val[1]=0.0;
00096     angle_val[2]=0.0;
00097     /*----------------------------*/
00098     /*
00099     mkdir("/sd/IMU_logger", 0777);
00100     
00101     FILE *fp = fopen("/sd/IMU_logger/log.csv", "w");
00102         
00103     if(fp == NULL) {
00104         error("Could not open file for write\n");
00105     }
00106     */
00107     /*-------------------------------------------------------*/
00108     
00109     pc.printf("Writing start!\r\n");
00110     
00111     while(1){
00112         
00113         gyro_val[0]=read_angular_rate_x();//X軸周りの角速度の算出
00114         gyro_val[1]=read_angular_rate_y();//Y軸周りの角速度の算出
00115         gyro_val[2]=read_angular_rate_z();//Z軸周りの角速度の算出
00116         
00117         acc_val[0]=read_acceleration_x();//X軸の加速度の算出
00118         acc_val[1]=read_acceleration_y();//Y軸の加速度の算出
00119         acc_val[2]=read_acceleration_z();//Z軸の加速度の算出
00120         
00121         /*
00122         mag_val[0]=compass.getMagXvalue();//X軸の地磁気の算出
00123         mag_val[1]=compass.getMagYvalue();//Y軸の地磁気の算出
00124         mag_val[2]=compass.getMagZvalue();//Z軸の地磁気の算出
00125         */
00126         
00127         time_new=passed_time.read();//時間更新
00128         
00129         //angle_val[0]+=(time_new-time_old)*gyro_val[0];//X軸周りの角度の算出
00130         //angle_val[1]+=(time_new-time_old)*gyro_val[1];//Y軸周りの角度の算出
00131         //angle_val[2]+=(time_new-time_old)*gyro_val[2];//Z軸周りの角度の算出
00132         
00133         pc.printf("%f,",time_new);
00134         pc.printf("%f,%f,%f,",gyro_val[0],gyro_val[1],gyro_val[2]);
00135         //pc.printf("%f:%f:%f\r\n",angle_val[0],angle_val[1],angle_val[2]);        
00136         pc.printf("%f,%f,%f\r\n",acc_val[0],acc_val[1],acc_val[2]);        
00137         
00138         //fprintf(fp,"%f,%f,%f,%f\r\n",time_new,angle_val[0],angle_val[1],angle_val[2]);//sdファイルに書き込み
00139         //pc.printf("%f,%f,%f,%f\r\n",time_new,angle_val[0],angle_val[1],angle_val[2]);//sdファイルに書き込み
00140         
00141         time_old=time_new;
00142         /*
00143         
00144         if(switch_blue==1){//青いタクトスイッチが押されている事が確認されたら
00145                    
00146             //fclose(fp);//sdカードに作ったファイルを閉じる(書き込みをやめる)
00147             pc.printf("Writing finish!");
00148             break;//while文から離脱
00149                     
00150         }else{}
00151         */
00152         
00153         wait(0.01);
00154         
00155         }//while 終わり
00156 }//main終わり
00157 
00158 /*------------------関数の宣言------------------*/
00159 void pc_rx () {
00160     device.putc(pc.getc());
00161 }
00162  
00163 void dev_rx () {
00164     pc.putc(device.getc());
00165 }
00166 
00167 void call_window1(){
00168     pc.printf("call_window1\r\n");
00169     device.putc(0xFE);
00170     device.putc(0x01);
00171     device.putc(0x0d);
00172  }
00173 
00174 void call_window0(){
00175     device.putc(0xFE);
00176     device.putc(0x00);
00177     device.putc(0x0d);
00178  }
00179   
00180 void GLOB_CMD_read(){//IMUが使えるかどうかの確認
00181     pc.printf("GLOB_CMD_read\r\n");
00182     char val[2];
00183     short i;
00184     
00185     device.putc(0x0A);
00186     device.putc(0x00);
00187     device.putc(0x0d);
00188       
00189     while(device.getc()!=0x0A){
00190       }
00191     val[0] = device.getc();
00192     val[1] = device.getc();
00193     
00194     pc.printf("%c",val[0]);
00195     pc.printf("%c",val[1]);
00196     
00197     i=(val[0]<<8)+val[1];
00198     if(i==0){
00199         pc.printf("IMU is ready.1\r\n");
00200         }else{
00201             pc.printf("IMU is not ready.1\r\n");
00202             }
00203             
00204     }
00205 
00206 void DIAG_STAT_read(){
00207     
00208     char val[2];
00209     short i;
00210     
00211     device.putc(0x04);
00212     device.putc(0x00);
00213     device.putc(0x0d);    
00214     
00215       
00216     while(device.getc()!=0x04){
00217       }
00218     val[0] = device.getc();
00219     val[1] = device.getc();
00220     
00221     i=(val[0]<<8)+val[1];
00222     if(i==0){
00223         pc.printf("IMU is ready.2\r\n");
00224         }else{
00225             pc.printf("IMU is not ready.2\r\n");
00226             }
00227     }
00228 
00229 void UART_CTRL_write(){//IMUのボーレートを480600,手動モードへ移行
00230     
00231     call_window1();
00232     
00233     device.putc(0x09);
00234     device.putc(0x00);
00235     device.putc(0x0d);
00236     
00237     device.putc(0x08);
00238     device.putc(0x00);
00239     device.putc(0x0d);
00240         
00241     }
00242         
00243 /*IMUが動作可能かどうかの確認*/
00244 void power_on_sequence1(){
00245     pc.printf("power on sequence 1\r\n");
00246     call_window1();
00247     GLOB_CMD_read();
00248     }
00249 
00250 /*IMUが動作可能かどうかの確認*/
00251 void power_on_sequence2(){
00252     pc.printf("power on sequence 2\r\n");
00253     call_window0();
00254     DIAG_STAT_read();
00255     }
00256 
00257 void move_to_sampling_mode(){
00258     call_window0();
00259     
00260     device.putc(0x83);
00261     device.putc(0x01);
00262     device.putc(0x0d);    
00263     }
00264     
00265 float read_angular_rate_x(){
00266     
00267     char val[4];
00268     float gyro_x;
00269     
00270     call_window0();
00271     
00272     device.putc(0x12);//register of gyro x (High)
00273     device.putc(0x00);
00274     device.putc(0x0d);    
00275     
00276     while(device.getc()!=0x12){
00277       }
00278     val[0] = device.getc();
00279     val[1] = device.getc();
00280 
00281     device.putc(0x14);//register of gyro x (Low)
00282     device.putc(0x00);
00283     device.putc(0x0d);    
00284     
00285     while(device.getc()!=0x14){
00286       }
00287     val[2] = device.getc();
00288     val[3] = device.getc();
00289     
00290     gyro_x=(val[0]<<24)+(val[1]<<16)+(val[2]<<8)+val[3];
00291     //SCALE_FACTOR 0.016
00292     gyro_x*=(SCALE_FACTOR_GYRO/65536);
00293     
00294     return gyro_x;
00295     
00296     }
00297     
00298 float read_angular_rate_y(){
00299     
00300     char val[4];
00301     float gyro_y;
00302     
00303     call_window0();
00304     
00305     device.putc(0x16);//register of gyro y (High)
00306     device.putc(0x00);
00307     device.putc(0x0d);    
00308     
00309     while(device.getc()!=0x16){
00310       }
00311     val[0] = device.getc();
00312     val[1] = device.getc();
00313 
00314     device.putc(0x18);//register of gyro y (Low)
00315     device.putc(0x00);
00316     device.putc(0x0d);    
00317     
00318     while(device.getc()!=0x18){
00319       }
00320     val[2] = device.getc();
00321     val[3] = device.getc();
00322     
00323     gyro_y=(val[0]<<24)+(val[1]<<16)+(val[2]<<8)+val[3];
00324     //SCALE_FACTOR 0.016
00325     gyro_y*=(SCALE_FACTOR_GYRO/65536);
00326     
00327     return gyro_y;
00328     
00329     }
00330     
00331 float read_angular_rate_z(){
00332     
00333     char val[4];
00334     float gyro_z;
00335     
00336     call_window0();
00337     
00338     device.putc(0x1A);//register of gyro z (High)
00339     device.putc(0x00);
00340     device.putc(0x0d);    
00341     
00342     while(device.getc()!=0x1A){
00343       }
00344     val[0] = device.getc();
00345     val[1] = device.getc();
00346 
00347     device.putc(0x1C);//register of gyro z (Low)
00348     device.putc(0x00);
00349     device.putc(0x0d);    
00350     
00351     while(device.getc()!=0x1C){
00352       }
00353     val[2] = device.getc();
00354     val[3] = device.getc();
00355     
00356     gyro_z=(val[0]<<24)+(val[1]<<16)+(val[2]<<8)+val[3];
00357     //SCALE_FACTOR 0.016
00358     gyro_z*=(SCALE_FACTOR_GYRO/65536);
00359     
00360     return gyro_z;
00361     
00362     }
00363     
00364 float read_acceleration_x(){
00365     
00366     char val[4];
00367     float acceleration_x;
00368     
00369     call_window0();
00370     
00371     device.putc(0x1E);//register of gyro z (High)
00372     device.putc(0x00);
00373     device.putc(0x0d);    
00374     
00375     while(device.getc()!=0x1E){
00376       }
00377     val[0] = device.getc();
00378     val[1] = device.getc();
00379 
00380     device.putc(0x20);//register of gyro z (Low)
00381     device.putc(0x00);
00382     device.putc(0x0d);    
00383     
00384     while(device.getc()!=0x20){
00385       }
00386     val[2] = device.getc();
00387     val[3] = device.getc();
00388     
00389     acceleration_x=(val[0]<<24)+(val[1]<<16)+(val[2]<<8)+val[3];
00390     //SCALE_FACTOR 0.016
00391     acceleration_x*=((SCALE_FACTOR_ACC/65536)* (GRAVITIONAL_ACCELERATION/1000.0));
00392     
00393     return acceleration_x;
00394     
00395     }
00396     
00397 float read_acceleration_y(){
00398     
00399     char val[4];
00400     float acceleration_y;
00401     
00402     call_window0();
00403     
00404     device.putc(0x22);//register of gyro z (High)
00405     device.putc(0x00);
00406     device.putc(0x0d);    
00407     
00408     while(device.getc()!=0x22){
00409       }
00410     val[0] = device.getc();
00411     val[1] = device.getc();
00412 
00413     device.putc(0x24);//register of gyro z (Low)
00414     device.putc(0x00);
00415     device.putc(0x0d);    
00416     
00417     while(device.getc()!=0x24){
00418       }
00419     val[2] = device.getc();
00420     val[3] = device.getc();
00421     
00422     acceleration_y=(val[0]<<24)+(val[1]<<16)+(val[2]<<8)+val[3];
00423     acceleration_y*=((SCALE_FACTOR_ACC/65536)* (GRAVITIONAL_ACCELERATION/1000.0));
00424     
00425     return acceleration_y;
00426     
00427     }
00428 
00429 float read_acceleration_z(){
00430     
00431     char val[4];
00432     float acceleration_z;
00433     
00434     call_window0();
00435     
00436     device.putc(0x26);//register of gyro z (High)
00437     device.putc(0x00);
00438     device.putc(0x0d);    
00439     
00440     while(device.getc()!=0x26){
00441       }
00442     val[0] = device.getc();
00443     val[1] = device.getc();
00444 
00445     device.putc(0x28);//register of gyro z (Low)
00446     device.putc(0x00);
00447     device.putc(0x0d);    
00448     
00449     while(device.getc()!=0x28){
00450       }
00451     val[2] = device.getc();
00452     val[3] = device.getc();
00453     
00454     acceleration_z=(val[0]<<24)+(val[1]<<16)+(val[2]<<8)+val[3];
00455     acceleration_z*=((SCALE_FACTOR_ACC/65536)* (GRAVITIONAL_ACCELERATION/1000.0));
00456     
00457     return acceleration_z;
00458     
00459     }
00460     
00461 /*-------------------------------------------*/