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

--- 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();
     }