Revision 4:4b3ae90ec778, committed 2018-08-09

Comitter:

Yukina

Date:

Thu Aug 09 21:39:18 2018 +0000

Parent:

3:24a8901befb6

Child:

5:f6e956e8a060

Commit message:

BBM

Changed in this revision

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/MPU6050.lib	Thu Aug 09 21:39:18 2018 +0000
@@ -0,0 +1,1 @@
+https://os.mbed.com/users/Sissors/code/MPU6050/#5c63e20c50f3

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/SDFileSystem.lib	Thu Aug 09 21:39:18 2018 +0000
@@ -0,0 +1,1 @@
+http://os.mbed.com/users/mbed_official/code/SDFileSystem/#8db0d3b02cec

--- a/main.cpp	Thu Nov 02 17:22:14 2017 +0000
+++ b/main.cpp	Thu Aug 09 21:39:18 2018 +0000
@@ -1,47 +1,330 @@
-/*
-説明
-Nucleo-F303K8とBMP180を使った気温・気圧・高度計算のサンプルプログラム
-
-ライブラリ
-https://developer.mbed.org/users/spiridion/code/BMP180/
-
-以下ピン配置
-Nucleo  BMP180
-GND-----GND-------0V
-+3V3----VIN
-D4------SDA
-D5------SCL
-
-*/
 #include "mbed.h"
 #include "math.h"
+#include "MPU6050.h"
 #include "BMP180.h"
-#define p0 1013.25f//海面気圧
+#include "SDFileSystem.h"
+
+/*マクロ処理*/
+//動作レート
+#define RATE_LOG            20
+#define RATE_OPEN           50
+//発射判定
+#define ACC_JUDGE_LAUNCH    1.5
+#define CNT_JUDGE_LAUNCH    5
+//待機時間
+#define TIME_GAP            3.0
+//頂点判定
+#define ALT_JUDGE_OPEN      1.5
+#define CNT_JUDGE_OPEN      5
+#define TIME_JUDGE_OPEN     30
+//放出判定
+#define TIME_JUDGE_OUT      60
+#define ALT_JUDGE_OUT       -1.5
+#define CNT_JUDGE_OUT       5  
+//共通    
+#define TIME_JUDGE_CNT      1.5
+#define p0                  1013.25f
+//ログ    //2018.06.28 edit
+#define NUM_DATA            20
 
-DigitalOut myled(LED1);
-Serial pc(USBTX,USBRX);
-BMP180 bmp(p28, p27);
-Timer timer;
+MPU6050         mpu(p9,p10);
+BMP180          bmp(p9,p10);
+BusOut          led(LED1,LED2,LED3,LED4);
+Serial          pc(USBTX,USBRX);
+Serial          twe(p28,p27);
+Serial          gps(p13,p14);
+PwmOut          servo1(p21);
+PwmOut          servo2(p22);
+PwmOut          servo3(p23);
+PwmOut          servo4(p24);
+DigitalOut      sw(p25);
+SDFileSystem    sd(p5,p6,p7,p8,"sd"); 
+LocalFileSystem local("local");
+
+Timer   timer_open;
+Timer   timer_log;    
+Ticker  tic_open;
+Ticker  tic_log;     
 
-float getAlt(float press, float temp);
+void    _open();
+void    _log();
+float   _getAlt();
+float   _DMS2DEG(float raw_data);
+float   _median(float data[],int num);
+int     _input(char cha);
+
+float   Alt_buf[10], Alt_gnd, Alt_md, Alt_max=-300; //TODO:Alt_maxの値を調整すること
+float   Time_alt;
+float   Data[2][NUM_DATA][8];
+int     Cnt=0, Cnt_acc=0, Cnt_alt=0;
+int     Cnt_data=0;
+int     cnt_gps;
+char    gps_data[256];
+bool    Row;
+FILE    *lfp;
+FILE    *fp;
+
+enum PHASE{SETUP=0,LAUNCH=1,WAIT=3,RISE=7,DROP1=15,DROP2=9,SEA=10} Phase;
+enum SERVO{LOCK=0,UNLOCK=1} Servo;
 
-int main() {
-    float pressure,temperature,altitude;
-    float time;   
+int main(){
+    /*センサ類初期化*/
     bmp.Initialize(64,BMP180_OSS_ULTRA_LOW_POWER);
-    pc.printf("time, temperature ,pressure, altitude\r\n");
-    timer.start();
-    
-    while(1) {
-        bmp.ReadData(&temperature,&pressure);
-        altitude = getAlt(pressure,temperature);
-        time = timer.read();
-        pc.printf("%f, %f, %f, %f \r\n",time, temperature, pressure, altitude);
-        myled =! myled;
-        wait(1);
+    twe.baud(115200);
+    sw=1;
+    /*ファイル作成*/
+    mkdir("/sd/mydir",0777);
+    fp=fopen("/sd/mydir/data.txt","a");
+    fprintf(fp,"phase,time,ax,ay,az,alt,temp,press\r\n");
+    fclose(fp);
+    lfp=fopen("/local/data.txt","a");
+    fclose(lfp);
+    /*サーボ調整*/
+    servo1.period_ms(20);
+    servo2.period_ms(20);
+    servo3.period_ms(20);
+    servo4.period_ms(20);
+    /*初めの挨拶*/
+    //pc.printf("POWER ON\r\n");
+    twe.printf("POWER ON\r\n");
+    /*入力待ち*/
+    while(1){
+        char cha = twe.getc();
+        if(_input(cha)==-1){
+            timer_log.start();  
+            tic_log.attach(&_log, 1.0/RATE_LOG); 
+            break;
+        }
+    }
+    /*フライト準備*/
+    Phase = SETUP;
+    tic_open.attach(&_open, 1.0/RATE_OPEN);
+
+    /*GPS*/
+    while(1){     
+        /*GPS取得*/
+        if(Phase != RISE){
+            if(gps.readable()){
+                gps_data[cnt_gps] = gps.getc();
+                if(gps_data[cnt_gps] == '$' || cnt_gps ==256){
+                    cnt_gps = 0;
+                    memset(gps_data,'\0',256);
+                }else if(gps_data[cnt_gps] == '\r'){
+                    float world_time, lon_east, lat_north;
+                    int rlock, sat_num;
+                    char lat,lon;
+                    if(sscanf(gps_data,"GPGGA,%f,%f,%c,%f,%c,%d,%d",&world_time,&lat_north,&lat,&lon_east,&lon,&rlock,&sat_num)>=1){
+                        if(rlock==1){
+                            lat_north = _DMS2DEG(lat_north);
+                            lon_east = _DMS2DEG(lon_east);
+                            twe.printf("%s\r\n",gps_data);
+                            twe.printf("Lat:%f,Lon:%f\r\ntime:%f,sat_num:%d\r\n",lat_north,lon_east,world_time,sat_num);
+                        }else{
+                            twe.printf("%s\r\n",gps_data);
+                        }
+                    }
+                }else{
+                    cnt_gps++;
+                }
+            }
+        }
+       
+        if(timer_log.read()>=30.0*60.0){
+            timer_log.reset();
+            /*なんかずっとこればっか書き込まれた 2018/7/1
+            fp=fopen("/sd/mydir/data.txt","w");
+            fprintf(fp,"phase,time,ax,ay,az,alt,temp,press\r\n");
+            fclose(fp);
+            */
+        }
     }
 }
 
-float getAlt(float press, float temp){
-    return (pow((p0/press), (1.0f/5.257f))-1.0f)*(temp+273.15f)/0.0065f;
-}
\ No newline at end of file
+void _open(){
+    led = Phase;
+    float acc[3],acc_buf[10],acc_abs,time_acc,time_alt;
+    switch(Phase){
+        case SETUP:     //地上高度取得
+            //pc.printf("Hello,World!\r\n");
+            twe.printf("Hello,World!\r\n");
+            for(Cnt=0;Cnt<10;Cnt++){
+                Alt_buf[Cnt] = _getAlt();
+            }
+            Alt_gnd = _median(Alt_buf,10); 
+            /*データをローカルファイルに保存*/ 
+            lfp=fopen("/local/data.txt", "a");
+            fprintf(lfp,"地上高度：%f\r\n",Alt_gnd);
+            fclose(lfp);
+
+            timer_open.start();
+            Phase = LAUNCH;
+            //pc.printf("Phase = LAUNCH\r\n");
+            twe.printf("Phase = LAUNCH\r\n");
+            break;
+
+        case LAUNCH:    //発射判定
+            for(Cnt=0;Cnt<10;Cnt++){
+                mpu.getAccelero(acc);
+                acc_buf[Cnt] = sqrt(pow(acc[0]/9.81,2)+pow(acc[1]/9.81,2)+pow(acc[2]/9.81,2)); 
+            }
+            acc_abs = _median(acc_buf,10);
+            //pc.printf("acc:%f\r\n",acc_abs);
+            //pc.printf("cnt:%d\r\n",Cnt_acc);
+            //加速度判定
+            if(acc_abs>ACC_JUDGE_LAUNCH){
+                Cnt_acc++;
+                time_acc = timer_open.read();
+            }
+            if(timer_open.read()-time_acc > TIME_JUDGE_CNT) Cnt_acc = 0;
+            if(Cnt_acc == CNT_JUDGE_LAUNCH){
+                //pc.printf("Phase = WAIT\r\n");
+                twe.printf("LAUNCHED\r\n");
+                timer_open.reset();
+                Phase = WAIT;
+            }
+            break;
+
+        case WAIT:      //待機時間
+            if(timer_open.read()>TIME_GAP) Phase = RISE;
+            break;
+
+        case RISE:      //頂点判定
+            for(Cnt=0;Cnt<5;Cnt++){
+                Alt_buf[Cnt] = _getAlt();
+                //pc.printf("alt:%f\r\n",Alt_buf[Cnt]);
+            }
+            Alt_md = _median(Alt_buf,5);
+            twe.printf("alt:%f\r\n",Alt_md);
+            //twe.printf("cnt:%d\r\n",Cnt_alt);
+            if(Alt_md > Alt_max){
+                Alt_max = Alt_md;
+                Cnt_alt = 0;
+            }
+            else if((Alt_max-Alt_md) > ALT_JUDGE_OPEN){
+                Cnt_alt++;
+                Time_alt = timer_open.read();
+            }
+            if((timer_open.read()-Time_alt) > TIME_JUDGE_CNT){
+                Cnt_alt = 0;
+            }
+            if(/*(*/Cnt_alt == CNT_JUDGE_OPEN/*)||(timer_open.read() > TIME_JUDGE_OPEN)*/){
+                //pc.printf("Phase = DROP1\r\n");
+                twe.printf("OPEN PARA\r\n");
+                Cnt_alt = 0;
+                /*ローカルファイルに最高高度を記録*/    
+                lfp = fopen("/local/data.txt", "a");
+                fprintf(lfp,"最高高度：%f\r\n",Alt_max);
+                fclose(lfp);
+
+                servo1.pulsewidth(0.0006);  //TODO:サーボホーンの角度調整
+                servo2.pulsewidth(0.0006);  //TODO:サーボホーンの角度調整
+                Phase = DROP1;
+            }
+            break;
+
+        case DROP1:     //放出機構作動？
+            for(Cnt=0;Cnt<5;Cnt++){
+                Alt_buf[Cnt] = _getAlt();
+            }
+            Alt_md = _median(Alt_buf,5);
+            twe.printf("Alt:%f\r\n",Alt_md);
+            //twe.printf("Cnt:%d\r\n",Cnt_alt);
+            if((Alt_md-Alt_gnd) > ALT_JUDGE_OUT){
+                Cnt_alt++;
+            }
+            if((Cnt_alt == CNT_JUDGE_OUT)||(timer_open.read() > TIME_JUDGE_OUT)){ //地上高度からの高度にする？到達高度低いかも？
+                //pc.printf("Phase = DROP2\r\n");
+                twe.printf("GO\r\n");
+                servo3.pulsewidth(0.0006);  //TODO:サーボホーンの角度調整
+                servo4.pulsewidth(0.0006);  //TODO:サーボホーンの角度調整
+                Phase = DROP2;
+            }
+            break;
+            
+        case DROP2:     //着水検知．サーボの電源落とす．
+            //twe.printf("cnt:%d\r\n",Cnt_alt);
+            wait(3.0);
+            sw = 0;
+            //pc.printf("Goodbye Servo\r\n");
+            twe.printf("FINISH\r\n");
+            Phase=SEA;
+            break;
+    }
+}
+
+void _log(){    
+    Data[Row][Cnt_data][0]=Phase;    
+    Data[Row][Cnt_data][1]=timer_log.read();
+    mpu.getAccelero(&Data[Row][Cnt_data][2]);
+    bmp.ReadData(&Data[Row][Cnt_data][5],&Data[Row][Cnt_data][6]);
+    Data[Row][Cnt_data][7] = (pow((p0/Data[Row][Cnt_data][6]), (1.0f/5.257f))-1.0f)*(Data[Row][Cnt_data][5]+273.15f)/0.0065f;
+    Cnt_data++;
+    //pc.printf("Cnt_data:%d\r\n",Cnt_data);
+    if(Cnt_data==NUM_DATA){
+        Cnt_data = 0;
+        Row =! Row;
+        fp = fopen("/sd/mydir/data.txt","a");
+        for(int i=0;i<NUM_DATA;i++){
+            fprintf(fp,"%2.0f,%f,%f,%f,%f,%f,%f,%f\r\n",Data[!Row][i][0],Data[!Row][i][1],Data[!Row][i][2],Data[!Row][i][3],Data[!Row][i][4],Data[!Row][i][5],Data[!Row][i][6],Data[!Row][i][7]);
+        }
+        fclose(fp);
+    }
+}
+
+float _getAlt(){
+    float altitude,pressure,temperature;
+    bmp.ReadData(&temperature,&pressure);
+    altitude = (pow((p0/pressure), (1.0f/5.257f))-1.0f)*(temperature+273.15f)/0.0065f;
+    return altitude;
+}
+
+int _input(char cha){
+    twe.printf("%c\r\n",cha);
+    if(cha!='\0'){
+//        pc.printf("%c\r\n",c);  
+    } 
+    if(cha=='F'){
+        twe.printf("flight mode on\r\n");
+        return -1;
+    }else if(cha=='u'){ //開放機構のUNLOCK
+        servo1.pulsewidth(0.0006);
+        servo2.pulsewidth(0.0006);
+    }else if(cha=='l'){ //開放機構のLOCK
+        servo1.pulsewidth(0.0024);
+        servo2.pulsewidth(0.0024);
+    }else if(cha=='a'){ //収穫機構のUNLOCK(AKERU)
+        servo3.pulsewidth(0.0010);
+        servo4.pulsewidth(0.0010);
+    }else if(cha=='s'){ //収穫機構のLOCK(SHIMERU)
+        servo3.pulsewidth(0.0024);
+        servo4.pulsewidth(0.0024);
+    }
+    return 0;
+}
+
+float _DMS2DEG(float raw_data){
+    int d=(int)(raw_data/100);
+    float m=(raw_data-(float)d*100);
+    return (float)d+m/60;
+}
+
+float _median(float data[], int num){
+    float *data_cpy, ans;
+    data_cpy = new float[num];
+    memcpy(data_cpy,data,sizeof(float)*num);
+ 
+    for(int i=0; i<num; i++){
+        for(int j=0; j<num-i-1; j++){
+            if(data_cpy[j]>data_cpy[j+1]){
+                float buff = data_cpy[j+1];
+                data_cpy[j+1] = data_cpy[j];
+                data_cpy[j] = buff;
+            }
+        }
+    }
+    
+    if(num%2!=0) ans = data_cpy[num/2];
+    else         ans = (data_cpy[num/2-1]+data_cpy[num/2])/2.0;
+    delete[] data_cpy;
+    return ans;
+}                                            
\ No newline at end of file