main.cpp

00001 #include "mbed.h"
00002 #include "math.h"
00003 #include "MPU6050.h"
00004 #include "BMP180.h"
00005 
00006 #define LAUNCH_JUDGE_ACC_g  3.0
00007 #define PARA_OPEN_TIME_s    20.0
00008 #define PARA_OPEN_CNT       7
00009 #define PARA_OPEN_FILTER_s  8.0
00010 #define LEAF_UNLOCK_TIME_s  85.0
00011 #define LEAF_UNLOCK_ALT_m   300.0
00012 #define p0                  1013.25f
00013 
00014 BMP180          bmp(dp5,dp27);
00015 MPU6050         mpu(dp5,dp27);
00016 Timer           para_timer;
00017 Timer           leaf_timer;
00018 Ticker          readtimer;
00019 PwmOut          mg996(dp1);
00020 PwmOut          sg92(dp18);
00021 DigitalIn       oshirase1(dp11);
00022 DigitalInOut    oshirase2(dp10);
00023 //DigitalOut      myled(LED1);
00024 //Serial          pc(USBTX,USBRX);
00025 
00026 /*  自作関数    */
00027 void    _flight();
00028 void    _open();
00029 void    _close();
00030 void    _leaf_lock();
00031 void    _leaf_unlock();
00032 float   _getAlt();
00033 float   _getAcc();
00034 float   _median(float data[], int num);
00035 
00036 
00037 /*  グローバル変数 */
00038 int     Alt_cnt;
00039 float   a[3],Acc;
00040 float   Pressure,Temperature,Altitude;
00041 float   Alt_buff[10],Alt_median_buff[10],Acc_buff[10];
00042 float   Land_Alt;
00043 bool    Mg996Open = true;
00044 bool    Sg92Open  = true;
00045 
00046 
00047 int main() {
00048 //    pc.printf("Hello!\r\n");
00049     mg996.period_ms(20);
00050     sg92.period_ms(20);
00051     mpu.setAcceleroRange(0);
00052     bmp.Initialize(64,BMP180_OSS_ULTRA_LOW_POWER);
00053     oshirase2.input();
00054     wait(0.5);
00055     while(1){
00056         if(oshirase1==0 && oshirase2==1){
00057             _flight();
00058             break;
00059         }else if(oshirase1==1 && oshirase2==0){
00060             while(1){
00061                 if(oshirase1==1 && oshirase2==1){
00062                     if(Mg996Open){
00063                         _close();
00064                         Mg996Open = false;
00065                     }else{
00066                         _open();
00067                         Mg996Open = true;
00068                     }
00069                     break;
00070                 }else if(oshirase1==0 && oshirase2==0){
00071                     if(Sg92Open){
00072                         _leaf_lock();
00073                         Sg92Open = false;
00074                     }else{
00075                         _leaf_unlock();
00076                         Sg92Open = true;
00077                     }
00078                     break;
00079                 }
00080             }
00081         }
00082     }
00083     while(1);
00084 }
00085 
00086 
00087 /*  フライトモード用関数  */
00088 void _flight(){
00089 //    pc.printf("flight mode on\r\n");
00090     oshirase2.output();
00091     oshirase2 = 0;
00092     _close();
00093     _leaf_lock();
00094 
00095     /*  地上高度取得  */
00096     for(int i=0;i<10;i++){
00097         Alt_buff[i] = _getAlt();
00098     }
00099     Land_Alt = _median(Alt_buff,10);
00100 //    pc.printf("Land_Alt = %f\r\n",Land_Alt);
00101 //    for(int i=0;i<10;i++)pc.printf("%f\n\r",Alt_buff[i]);
00102 
00103     /*  発射判定    */
00104     while(1){
00105 //        myled = !myled;
00106         for(int i=0;i<10;i++){
00107             Acc_buff[i] = _getAcc();
00108         }
00109         Acc = _median(Acc_buff,10);
00110         if(Acc >= LAUNCH_JUDGE_ACC_g){
00111             para_timer.start();
00112             oshirase2 = 1;
00113 //            for(int i=0;i<10;i++)pc.printf("%f\n\r",Acc_buff[i]);
00114 //            pc.printf("Acc = %f\r\n",Acc);
00115             break;
00116         }
00117     }
00118 
00119     /*  開放判定    */
00120     while(1){
00121         Alt_cnt = 0;
00122         for(int i=0;i<10;i++){
00123             Alt_buff[i] = _getAlt();
00124         }
00125         Alt_median_buff[0] = _median(Alt_buff,10);
00126         for(int i=1;i<10;i++){
00127             for(int j=0;j<10;j++){
00128                 Alt_buff[j] = _getAlt();
00129             }
00130             Alt_median_buff[i] = _median(Alt_buff,10);
00131 //            pc.printf("%f\r\n",Alt_median_buff[i]);
00132             if(Alt_median_buff[i] < Alt_median_buff[i-1]-0.1) Alt_cnt++;
00133         }
00134 //        pc.printf("%d\r\n",Alt_cnt);
00135         if(para_timer.read() >= PARA_OPEN_FILTER_s){
00136             if(Alt_cnt >= PARA_OPEN_CNT || para_timer.read() >= PARA_OPEN_TIME_s){
00137                 _open();
00138                 leaf_timer.start();
00139                 oshirase2 = 0;
00140                 para_timer.stop();
00141 //                pc.printf("para open\r\n");
00142 //                for(int i=0;i<10;i++)pc.printf("%f\n\r",Alt_buff[i]);
00143                 break;
00144             }
00145         }
00146     }
00147 
00148     /*  リーフィング解除判定  */
00149     while(1){
00150         for(int i=0;i<10;i++){
00151             Alt_buff[i] = _getAlt();
00152         }
00153         Altitude = _median(Alt_buff,10);
00154         if(Altitude - Land_Alt <= LEAF_UNLOCK_ALT_m || leaf_timer.read() >= LEAF_UNLOCK_TIME_s){
00155             _leaf_unlock();
00156             oshirase2 = 1;
00157             leaf_timer.stop();
00158 //            pc.printf("leafing unlock\r\n");
00159 //            for(int i=0;i<10;i++)pc.printf("%f\n\r",Alt_buff[i]);
00160             break;
00161         }
00162     }
00163 }
00164 
00165 
00166 /*  サーボ用関数  */
00167 void _close() {mg996.pulsewidth(0.0005);}
00168 void _open() {mg996.pulsewidth(0.0024);}
00169 void _leaf_lock() {sg92.pulsewidth(0.0024);}
00170 void _leaf_unlock() {sg92.pulsewidth(0.0005);}
00171 
00172 
00173 /*  高度取得関数    */
00174 float _getAlt(){
00175     float altitude;
00176     bmp.ReadData(&Temperature,&Pressure);
00177     altitude = (pow((p0/Pressure), (1.0f/5.257f))-1.0f)*(Temperature+273.15f)/0.0065f;
00178 //    pc.printf("%f, %f, %f\r\n",Pressure,Temperature,altitude);
00179     return altitude;
00180 }
00181 
00182 
00183 /*  合成加速度取得関数 */
00184 float _getAcc(){
00185     mpu.getAccelero(a);
00186     return sqrt(pow(a[0]/9.81,2)+pow(a[1]/9.81,2)+pow(a[2]/9.81,2));
00187 }
00188 
00189 
00190 /*  中央値計算関数 */
00191 float _median(float data[], int num){//todo:処理時間計測
00192     float *data_cpy, ans;
00193     data_cpy = new float[num];
00194     memcpy(data_cpy,data,sizeof(float)*num);
00195 
00196     for(int i=0; i<num; i++){
00197         for(int j=0; j<num-i-1; j++){
00198             if(data_cpy[j]>data_cpy[j+1]){
00199                 float buff = data_cpy[j+1];
00200                 data_cpy[j+1] = data_cpy[j];
00201                 data_cpy[j] = buff;
00202             }
00203         }
00204     }
00205     
00206     if(num%2!=0) ans = data_cpy[num/2];
00207     else         ans = (data_cpy[num/2-1]+data_cpy[num/2])/2.0;
00208     delete[] data_cpy;
00209     return ans;
00210 }