Diff: main.cpp

Revision:

2:4cdbd4b3722d

Parent:

0:bdbd3d6fc5d5

--- 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;
+    }
+ }
+    
+}
+