2021 ver of EPSON IMU
Dependencies: mbed QMC5883L SDFileSystem
Revision 2:a9f80f95aff9, committed 2021-04-26
- Comitter:
- Joeatsumi
- Date:
- Mon Apr 26 04:46:14 2021 +0000
- Parent:
- 1:bf98282e69a4
- Commit message:
- EPOSON IMU program in2021
Changed in this revision
main.cpp | Show annotated file Show diff for this revision Revisions of this file |
--- a/main.cpp Sat Aug 31 05:26:12 2019 +0000 +++ b/main.cpp Mon Apr 26 04:46:14 2021 +0000 @@ -1,5 +1,5 @@ -/*EPSONのセンサを試すために組んだ20190831。 -地磁気センサの方位値を姿勢によって補正できる。 +/* +EPSONのセンサを試すために組んだ20210423。 */ #include "mbed.h" @@ -13,8 +13,9 @@ #define GRAVITIONAL_ACCELERATION 9.80665 Serial pc(USBTX, USBRX); // tx, rx -QMC5883L compass(p9,p10); -Serial device(p13, p14); // tx, rx +//QMC5883L compass(p9,p10); +//Serial device(p9, p10); // tx, rx +Serial device(p28, p27); // tx, rx SDFileSystem sd(p5, p6, p7, p8, "sd"); // the pinout on the mbed Cool Components workshop board DigitalIn switch_blue(p21);//sdカードへの書き込みを終了させる @@ -68,6 +69,8 @@ pc.baud(460800); device.baud(460800); + pc.printf("Baud rate setting\r\n"); + // pc.attach(pc_rx, Serial::RxIrq); //device.attach(dev_rx, Serial::RxIrq); @@ -77,7 +80,7 @@ */ wait(1.0); - compass.init();//地磁気センサの起動 + //compass.init();//地磁気センサの起動 power_on_sequence1();//IMUが動作可能かどうかの確認 power_on_sequence2();//IMUが動作可能かどうかの確認 @@ -92,8 +95,7 @@ angle_val[1]=0.0; angle_val[2]=0.0; /*----------------------------*/ - - /*sdカードにファイルを構成する そのファイルを書き込み状態にする*/ + /* mkdir("/sd/IMU_logger", 0777); FILE *fp = fopen("/sd/IMU_logger/log.csv", "w"); @@ -101,6 +103,7 @@ if(fp == NULL) { error("Could not open file for write\n"); } + */ /*-------------------------------------------------------*/ pc.printf("Writing start!\r\n"); @@ -115,31 +118,37 @@ acc_val[1]=read_acceleration_y();//Y軸の加速度の算出 acc_val[2]=read_acceleration_z();//Z軸の加速度の算出 + /* mag_val[0]=compass.getMagXvalue();//X軸の地磁気の算出 mag_val[1]=compass.getMagYvalue();//Y軸の地磁気の算出 mag_val[2]=compass.getMagZvalue();//Z軸の地磁気の算出 + */ time_new=passed_time.read();//時間更新 - angle_val[0]+=(time_new-time_old)*gyro_val[0];//X軸周りの角度の算出 - angle_val[1]+=(time_new-time_old)*gyro_val[1];//Y軸周りの角度の算出 - angle_val[2]+=(time_new-time_old)*gyro_val[2];//Z軸周りの角度の算出 + //angle_val[0]+=(time_new-time_old)*gyro_val[0];//X軸周りの角度の算出 + //angle_val[1]+=(time_new-time_old)*gyro_val[1];//Y軸周りの角度の算出 + //angle_val[2]+=(time_new-time_old)*gyro_val[2];//Z軸周りの角度の算出 - //pc.printf("%f:%f:%f\r\n",gyro_val[0],gyro_val[1],gyro_val[2]); + pc.printf("%f,",time_new); + pc.printf("%f,%f,%f,",gyro_val[0],gyro_val[1],gyro_val[2]); //pc.printf("%f:%f:%f\r\n",angle_val[0],angle_val[1],angle_val[2]); - //pc.printf("%f:%f:%f\r\n",acc_val[0],acc_val[1],acc_val[2]); + pc.printf("%f,%f,%f\r\n",acc_val[0],acc_val[1],acc_val[2]); - fprintf(fp,"%f,%f,%f,%f\r\n",time_new,angle_val[0],angle_val[1],angle_val[2]);//sdファイルに書き込み + //fprintf(fp,"%f,%f,%f,%f\r\n",time_new,angle_val[0],angle_val[1],angle_val[2]);//sdファイルに書き込み + //pc.printf("%f,%f,%f,%f\r\n",time_new,angle_val[0],angle_val[1],angle_val[2]);//sdファイルに書き込み time_old=time_new; + /* if(switch_blue==1){//青いタクトスイッチが押されている事が確認されたら - fclose(fp);//sdカードに作ったファイルを閉じる(書き込みをやめる) + //fclose(fp);//sdカードに作ったファイルを閉じる(書き込みをやめる) pc.printf("Writing finish!"); break;//while文から離脱 }else{} + */ wait(0.01); @@ -156,6 +165,7 @@ } void call_window1(){ + pc.printf("call_window1\r\n"); device.putc(0xFE); device.putc(0x01); device.putc(0x0d); @@ -168,7 +178,7 @@ } void GLOB_CMD_read(){//IMUが使えるかどうかの確認 - + pc.printf("GLOB_CMD_read\r\n"); char val[2]; short i; @@ -181,6 +191,9 @@ val[0] = device.getc(); val[1] = device.getc(); + pc.printf("%c",val[0]); + pc.printf("%c",val[1]); + i=(val[0]<<8)+val[1]; if(i==0){ pc.printf("IMU is ready.1\r\n"); @@ -229,12 +242,14 @@ /*IMUが動作可能かどうかの確認*/ void power_on_sequence1(){ + pc.printf("power on sequence 1\r\n"); call_window1(); GLOB_CMD_read(); } /*IMUが動作可能かどうかの確認*/ void power_on_sequence2(){ + pc.printf("power on sequence 2\r\n"); call_window0(); DIAG_STAT_read(); }