きりたんぽの開放プログラム

Dependencies:   BMP180 MPU6050 SDFileSystem mbed

Fork of SDFileSystem_HelloWorld by mbed official

Files at this revision

API Documentation at this revision

Comitter:
mizukamikatsuki
Date:
Tue Jul 31 11:43:36 2018 +0000
Parent:
1:e4d7342be507
Commit message:
?????????????

Changed in this revision

BMP180.lib Show annotated file Show diff for this revision Revisions of this file
MPU6050.lib Show annotated file Show diff for this revision Revisions of this file
SDFileSystem.lib Show annotated file Show diff for this revision Revisions of this file
main.cpp Show annotated file Show diff for this revision Revisions of this file
diff -r e4d7342be507 -r 4cdbd4b3722d BMP180.lib
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/BMP180.lib	Tue Jul 31 11:43:36 2018 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/spiridion/code/BMP180/#072073c79cfd
diff -r e4d7342be507 -r 4cdbd4b3722d MPU6050.lib
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/MPU6050.lib	Tue Jul 31 11:43:36 2018 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/Sissors/code/MPU6050/#5c63e20c50f3
diff -r e4d7342be507 -r 4cdbd4b3722d SDFileSystem.lib
--- a/SDFileSystem.lib	Tue May 16 05:18:55 2017 +0000
+++ b/SDFileSystem.lib	Tue Jul 31 11:43:36 2018 +0000
@@ -1,1 +1,1 @@
-http://mbed.org/users/mbed_official/code/SDFileSystem/#8db0d3b02cec
+http://mbed.org/users/neilt6/code/SDFileSystem/#e4d2567200db
diff -r e4d7342be507 -r 4cdbd4b3722d main.cpp
--- a/main.cpp	Tue May 16 05:18:55 2017 +0000
+++ b/main.cpp	Tue Jul 31 11:43:36 2018 +0000
@@ -1,19 +1,338 @@
 #include "mbed.h"
+#include "math.h"
+#include "MPU6050.h"
+#include "BMP180.h"
 #include "SDFileSystem.h"
- 
-SDFileSystem sd(p5, p6, p7, p8, "sd"); // the pinout on the mbed Cool Components workshop board
- 
-int main() {
-    printf("Hello World!\n");   
- 
-    mkdir("/sd/mydir", 0777);
+
+#define p0 1013.25f
+#define JUDGE_ACC   1.0
+#define JUDGE_ALT   1.0
+#define RATE_GET    20
+#define RATE_OPEN   20
+#define NUM_DATA    20
+#define NUMBER      5
+#define TIME_OPEN   14
+#define TIME_DROP   104
+
+enum PHASE{SETUP,LAUNCH,RISE,DROP,LAND} Phase;
+
+Serial twe(p13,p14);
+Serial pc(USBTX,USBRX);
+MPU6050 mpu(p9,p10);
+BMP180 bmp(p9, p10);
+SDFileSystem sd(p5, p6, p7, p8, "sd");  // the pinout on the mbed Cool Components workshop board
+Serial           gps(p28,p27);
+PwmOut servo1(p21);
+PwmOut servo2(p25);
+DigitalOut       fte(p5);
+DigitalOut       buzz(p6);
+DigitalOut       led1(LED1);
+DigitalOut       led2(LED2);
+DigitalOut       led3(LED3);
+DigitalOut       led4(LED4);
+
+
+
+Timer timer_open;
+Timer timer_log;
+Ticker tic_open;
+Ticker tic_log;
+Ticker tic_heat;
+Ticker tic_buzz;
+
+
+FILE *bp;
+FILE *lfp;
+LocalFileSystem  local("local");
+
+void  _open();
+void  _getData();
+int   _input(char cha);
+void  _heater();
+void  _buzzer();
+char  comh;
+char  comb;
+float _median(float data[], int num);
+float getAlt(float press, float temp);
+float _DMS2DEG(float raw_data);
+
+float Alt_gnd;
+float med[NUMBER];
+float alt[NUMBER];
+float med_alt;
+float max_alt=-1000;
+float mae_alt=5000;
+float med_g;
+float pressure,temperature,altitude;
+float Time;
+float Data[2][NUM_DATA][7]={0};
+char  gps_data[256];
+int   cnt_gps;
+int   Cnt;  
+int   cnt_alt=0;
+int   cnt_d=0;
+int   Cnt_Data;
+int   Cnt_GPS=0;
+bool  row;
+
+int main(){
+    twe.baud(115200);
+    mpu.setAcceleroRange(3);
+    bmp.Initialize(64,BMP180_OSS_ULTRA_LOW_POWER);
+    mkdir("/sd/mydir",0777);
+    bp=fopen("/sd/mydir/data.bin","ab");
+    fclose(bp);
+    lfp=fopen("/local/data.txt","a");
+    fclose(lfp);
+    servo1.period_ms(20);
+    servo2.period_ms(20);
+    led1=0;
+    led2=0;
+    led3=0;
+twe.printf("HELLO!\r\n");
+    led1=1;
+    led2=1;
+    led3=1;
+    led4=1;
+    fte=0;
+    buzz=0;
+    pc.printf("hello\r\n");
     
-    FILE *fp = fopen("/sd/mydir/sdtest.txt", "w");
-    if(fp == NULL) {
-        error("Could not open file for write\n");
+    while(1){
+        char cha = twe.getc();
+        if(_input(cha)==-1){
+            timer_log.start();
+            tic_log.attach(&_getData, 1.0/RATE_GET);
+            break;
+        }
+    }   
+    tic_heat.attach(&_heater,0.1);
+    led1=0;
+    led2=0;
+    led3=0;
+    led4=0;
+    Phase = SETUP;
+    tic_open.attach(&_open, 1.0/RATE_OPEN);
+ 
+    while(1){     
+        
+                
+            if(gps.readable()){
+                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();
     }
-    fprintf(fp, "Hello fun SD Card World!");
-    fclose(fp); 
+}
+
+
+
+void _open(){
+    switch(Phase){
+        case SETUP: 
+                    for(Cnt=0;Cnt<10;Cnt++){
+                        bmp.ReadData(&temperature,&pressure);
+                        altitude = getAlt(pressure,temperature);
+                        alt[Cnt]=altitude;
+                    }
+                    Alt_gnd = _median(alt,10);
+                    lfp=fopen("/local/data.txt", "a");
+                    fprintf(lfp,"地上高度:%f\r\n",Alt_gnd);
+                    fclose(lfp);
+                    Phase = LAUNCH;
+                    twe.printf("Phase=LAUNCH\r\n");
+                    pc.printf("Phase=LAUNCH\r\n");
+                    break;
+                    
+        case LAUNCH:
+                    
+                    float a[3],g;
+                    int i=0;
+                    for(i=0;i<NUMBER;i++){
+                        mpu.getAccelero(a);
+                        g=sqrt(pow(a[0],2)+(a[1],2)+(a[2],2))/9.81;
+                        med[i]=g;
+                        }
+                    med_g=_median(med,NUMBER);
+                    twe.printf("%f\n\r",med_g);
+                    if(med_g>JUDGE_ACC){
+                        tic_heat.detach();
+                        timer_open.start();
+                        pc.printf("発射\r\n");
+                        twe.printf("発射\r\n");
+                        led1=0;
+                        led4=0;
+                        Phase=RISE;
+                        twe.printf("PHASE=RISE\r\n");
+                    }
+                        led1=!led1;
+                        break;
+        
+        case RISE:
+                int j=0;
+                for(j=0;j<NUMBER;j++){
+                    bmp.ReadData(&temperature,&pressure);
+                    altitude = getAlt(pressure,temperature);
+                    alt[j]=altitude;
+                }       
+        med_alt=_median(alt,NUMBER);
+            twe.printf("%f, %f\r\n",max_alt,med_alt);
+        if(med_alt>max_alt){
+           max_alt=med_alt;
+            cnt_alt=0;
+            }else if(max_alt-med_alt>JUDGE_ALT){
+                cnt_alt++;
+            }
+            twe.printf("%d\n\r",cnt_alt);
+            if(cnt_alt>=3 || timer_open.read()>=TIME_OPEN){
+                pc.printf("頂点\r\n");
+                twe.printf("頂点\r\n");
+                 lfp = fopen("/local/data.txt", "a");
+                 fprintf(lfp,"最高高度:%f\r\n",max_alt);
+                 fclose(lfp);
+                 servo1.pulsewidth(0.0015);
+                 servo2.pulsewidth(0.0006);
+                 led2=0;
+                Phase=DROP;
+                twe.printf("PHASE=DROP");
+            }
+                led2=!led2;
+                break;
+            
+    case DROP:
+            int k=0;
+            for(k=0;k<NUMBER;k++){
+            bmp.ReadData(&temperature,&pressure);
+            altitude = getAlt(pressure,temperature);
+            alt[k]=altitude;
+        }       
+        med_alt=_median(alt,NUMBER);
+        mae_alt=med_alt;
+        if(mae_alt-med_alt<=1 && mae_alt-med_alt>=-1){
+            cnt_d++;
+        }else{
+            cnt_d=0;
+        }
+        twe.printf("%d\n\r",cnt_d);
+        if(cnt_d==3 || timer_open.read()>TIME_DROP){
+            pc.printf("着水\r\n");
+            twe.printf("着水\r\n");
+            timer_open.stop();
+            tic_buzz.attach(&_buzzer,0.1);
+            Phase=LAND;
+            pc.printf("Judge DROP\r\n");
+            twe.printf("FINISH\r\n");
+            led3=0;
+        }
+            led3=!led3;
+            break;
+    }    
+}       
+
+void _getData(){
+    Data[row][Cnt_Data][0] = timer_log.read();
+    mpu.getAccelero(&Data[row][Cnt_Data][1]);
+    bmp.ReadData(&Data[row][Cnt_Data][4],&Data[row][Cnt_Data][5]);
+    Data[row][Cnt_Data][6] = (pow((p0/Data[row][Cnt_Data][5]), (1.0f/5.257f))-1.0f)*(Data[row][Cnt_Data][4]+273.15f)/0.0065f;
+    Cnt_Data++;
+    /*Log*/
+    if(Cnt_Data==NUM_DATA){
+        Cnt_Data = 0;
+        row =! row;
+        bp = fopen("/sd/mydir/data.bin","ab");
+        fwrite(&Data[!row][0][0],sizeof(float),7*NUM_DATA,bp);
+        fclose(bp);
+    }
+}
+
+float _DMS2DEG(float raw_data){
+    int d=(int)(raw_data/100);
+    float m=(raw_data-(float)d*100);
+    return (float)d+m/60;
+}
+
+float getAlt(float press, float temp){
+    return (pow((p0/press), (1.0f/5.257f))-1.0f)*(temp+273.15f)/0.0065f;
+}
+
+float _median(float data[], int num){
+    float *data_cpy, ans;
+    data_cpy = new float[num];
+    memcpy(data_cpy,data,sizeof(float)*num);
  
-    printf("Goodbye World!\n");
+    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;    
+}
+
+int _input(char cha){
+    if(cha=='F'){
+        twe.printf("flight mode on\r\n");
+        return -1;
+    }else if(cha=='U'){
+        servo1.pulsewidth(0.0015);
+        servo2.pulsewidth(0.0006);
+    }else if(cha=='L'){
+        servo1.pulsewidth(0.0008);
+        servo2.pulsewidth(0.0014);
+    }
+    return 0;
 }
+
+void _heater(){
+    if(twe.readable()){
+    char comh=twe.getc();
+    if(comh=='H'){
+        fte=1;
+        led4=1;
+    }else if(comh=='C'){
+        fte=0;
+        led4=0;
+    }
+ }
+}
+
+void _buzzer(){
+    if(twe.readable()){
+    char comb=twe.getc();
+    if(comb=='N'){
+        buzz=1;
+        led4=1;
+    }else if(comb=='S'){
+        buzz=0;
+        led4=0;
+    }
+ }
+    
+}
+