kohei oikawa / Mbed 2 deprecated 2017_11_1114

2017.11伊豆大島共同打ち上げ実験の開放用プログラム

Dependencies:   BMP180 MPU6050 mbed

Fork of Sample_BMP180 by CORE

main.cpp@8:b10169df8f5f, 2017-11-10 (annotated)

Committer:
oichan
Date:
Fri Nov 10 12:43:15 2017 +0000
Revision:
8:b10169df8f5f
Parent:
7:4efaca1e9acd 
flight model

Who changed what in which revision?

UserRevisionLine numberNew contents of line
mikawataru 0:0ff20d8e9090 1 #include "mbed.h"
mikawataru 0:0ff20d8e9090 2 #include "math.h"
oichan 2:6238109d564d 3 #include "MPU6050.h"
mikawataru 0:0ff20d8e9090 4 #include "BMP180.h"
oichan 2:6238109d564d 5
oichan 4:ae20e73f4924 6 #define LAUNCH_JUDGE_ACC_g 3.0
oichan 8:b10169df8f5f 7 #define PARA_OPEN_TIME_s 20.0
oichan 6:e0c25c3976ae 8 #define PARA_OPEN_CNT 7
oichan 8:b10169df8f5f 9 #define PARA_OPEN_FILTER_s 8.0
oichan 8:b10169df8f5f 10 #define LEAF_UNLOCK_TIME_s 85.0
oichan 2:6238109d564d 11 #define LEAF_UNLOCK_ALT_m 300.0
oichan 2:6238109d564d 12 #define p0 1013.25f
oichan 2:6238109d564d 13
oichan 4:ae20e73f4924 14 BMP180 bmp(dp5,dp27);
oichan 4:ae20e73f4924 15 MPU6050 mpu(dp5,dp27);
oichan 4:ae20e73f4924 16 Timer para_timer;
oichan 4:ae20e73f4924 17 Timer leaf_timer;
oichan 4:ae20e73f4924 18 Ticker readtimer;
oichan 6:e0c25c3976ae 19 PwmOut mg996(dp1);
oichan 6:e0c25c3976ae 20 PwmOut sg92(dp18);
oichan 4:ae20e73f4924 21 DigitalIn oshirase1(dp11);
oichan 4:ae20e73f4924 22 DigitalInOut oshirase2(dp10);
oichan 8:b10169df8f5f 23 //DigitalOut myled(LED1);
oichan 8:b10169df8f5f 24 //Serial pc(USBTX,USBRX);
mikawataru 0:0ff20d8e9090 25
oichan 2:6238109d564d 26 /* 自作関数 */
oichan 2:6238109d564d 27 void _flight();
oichan 2:6238109d564d 28 void _open();
oichan 2:6238109d564d 29 void _close();
oichan 2:6238109d564d 30 void _leaf_lock();
oichan 2:6238109d564d 31 void _leaf_unlock();
oichan 2:6238109d564d 32 float _getAlt();
oichan 2:6238109d564d 33 float _getAcc();
oichan 2:6238109d564d 34 float _median(float data[], int num);
mikawataru 0:0ff20d8e9090 35
oichan 7:4efaca1e9acd 36
oichan 2:6238109d564d 37 /* グローバル変数 */
oichan 4:ae20e73f4924 38 int Alt_cnt;
oichan 4:ae20e73f4924 39 float a[3],Acc;
oichan 4:ae20e73f4924 40 float Pressure,Temperature,Altitude;
oichan 6:e0c25c3976ae 41 float Alt_buff[10],Alt_median_buff[10],Acc_buff[10];
oichan 4:ae20e73f4924 42 float Land_Alt;
oichan 4:ae20e73f4924 43 bool Mg996Open = true;
oichan 4:ae20e73f4924 44 bool Sg92Open = true;
oichan 2:6238109d564d 45
mikawataru 0:0ff20d8e9090 46
mikawataru 0:0ff20d8e9090 47 int main() {
oichan 8:b10169df8f5f 48 // pc.printf("Hello!\r\n");
oichan 3:217b56515009 49 mg996.period_ms(20);
oichan 2:6238109d564d 50 sg92.period_ms(20);
oichan 2:6238109d564d 51 mpu.setAcceleroRange(0);
mikawataru 0:0ff20d8e9090 52 bmp.Initialize(64,BMP180_OSS_ULTRA_LOW_POWER);
oichan 4:ae20e73f4924 53 oshirase2.input();
oichan 5:a5cf6914e7c4 54 wait(0.5);
oichan 4:ae20e73f4924 55 while(1){
oichan 4:ae20e73f4924 56 if(oshirase1==0 && oshirase2==1){
oichan 4:ae20e73f4924 57 _flight();
oichan 4:ae20e73f4924 58 break;
oichan 4:ae20e73f4924 59 }else if(oshirase1==1 && oshirase2==0){
oichan 4:ae20e73f4924 60 while(1){
oichan 4:ae20e73f4924 61 if(oshirase1==1 && oshirase2==1){
oichan 4:ae20e73f4924 62 if(Mg996Open){
oichan 4:ae20e73f4924 63 _close();
oichan 4:ae20e73f4924 64 Mg996Open = false;
oichan 4:ae20e73f4924 65 }else{
oichan 4:ae20e73f4924 66 _open();
oichan 4:ae20e73f4924 67 Mg996Open = true;
oichan 4:ae20e73f4924 68 }
oichan 4:ae20e73f4924 69 break;
oichan 4:ae20e73f4924 70 }else if(oshirase1==0 && oshirase2==0){
oichan 4:ae20e73f4924 71 if(Sg92Open){
oichan 4:ae20e73f4924 72 _leaf_lock();
oichan 4:ae20e73f4924 73 Sg92Open = false;
oichan 4:ae20e73f4924 74 }else{
oichan 4:ae20e73f4924 75 _leaf_unlock();
oichan 4:ae20e73f4924 76 Sg92Open = true;
oichan 4:ae20e73f4924 77 }
oichan 4:ae20e73f4924 78 break;
oichan 4:ae20e73f4924 79 }
oichan 4:ae20e73f4924 80 }
oichan 4:ae20e73f4924 81 }
oichan 4:ae20e73f4924 82 }
oichan 2:6238109d564d 83 while(1);
oichan 2:6238109d564d 84 }
oichan 2:6238109d564d 85
oichan 2:6238109d564d 86
oichan 3:217b56515009 87 /* フライトモード用関数 */
oichan 2:6238109d564d 88 void _flight(){
oichan 8:b10169df8f5f 89 // pc.printf("flight mode on\r\n");
oichan 4:ae20e73f4924 90 oshirase2.output();
oichan 4:ae20e73f4924 91 oshirase2 = 0;
oichan 6:e0c25c3976ae 92 _close();
oichan 6:e0c25c3976ae 93 _leaf_lock();
oichan 4:ae20e73f4924 94
oichan 2:6238109d564d 95 /* 地上高度取得 */
oichan 2:6238109d564d 96 for(int i=0;i<10;i++){
oichan 2:6238109d564d 97 Alt_buff[i] = _getAlt();
oichan 2:6238109d564d 98 }
oichan 4:ae20e73f4924 99 Land_Alt = _median(Alt_buff,10);
oichan 8:b10169df8f5f 100 // pc.printf("Land_Alt = %f\r\n",Land_Alt);
oichan 3:217b56515009 101 // for(int i=0;i<10;i++)pc.printf("%f\n\r",Alt_buff[i]);
oichan 2:6238109d564d 102
oichan 2:6238109d564d 103 /* 発射判定 */
oichan 2:6238109d564d 104 while(1){
oichan 8:b10169df8f5f 105 // myled = !myled;
oichan 2:6238109d564d 106 for(int i=0;i<10;i++){
oichan 8:b10169df8f5f 107 Acc_buff[i] = _getAcc();
oichan 2:6238109d564d 108 }
oichan 4:ae20e73f4924 109 Acc = _median(Acc_buff,10);
oichan 4:ae20e73f4924 110 if(Acc >= LAUNCH_JUDGE_ACC_g){
oichan 2:6238109d564d 111 para_timer.start();
oichan 4:ae20e73f4924 112 oshirase2 = 1;
oichan 3:217b56515009 113 // for(int i=0;i<10;i++)pc.printf("%f\n\r",Acc_buff[i]);
oichan 8:b10169df8f5f 114 // pc.printf("Acc = %f\r\n",Acc);
oichan 2:6238109d564d 115 break;
oichan 2:6238109d564d 116 }
oichan 2:6238109d564d 117 }
oichan 2:6238109d564d 118
oichan 2:6238109d564d 119 /* 開放判定 */
oichan 2:6238109d564d 120 while(1){
oichan 2:6238109d564d 121 Alt_cnt = 0;
oichan 6:e0c25c3976ae 122 for(int i=0;i<10;i++){
oichan 6:e0c25c3976ae 123 Alt_buff[i] = _getAlt();
oichan 6:e0c25c3976ae 124 }
oichan 6:e0c25c3976ae 125 Alt_median_buff[0] = _median(Alt_buff,10);
oichan 2:6238109d564d 126 for(int i=1;i<10;i++){
oichan 6:e0c25c3976ae 127 for(int j=0;j<10;j++){
oichan 6:e0c25c3976ae 128 Alt_buff[j] = _getAlt();
oichan 6:e0c25c3976ae 129 }
oichan 6:e0c25c3976ae 130 Alt_median_buff[i] = _median(Alt_buff,10);
oichan 7:4efaca1e9acd 131 // pc.printf("%f\r\n",Alt_median_buff[i]);
oichan 7:4efaca1e9acd 132 if(Alt_median_buff[i] < Alt_median_buff[i-1]-0.1) Alt_cnt++;
oichan 2:6238109d564d 133 }
oichan 7:4efaca1e9acd 134 // pc.printf("%d\r\n",Alt_cnt);
oichan 5:a5cf6914e7c4 135 if(para_timer.read() >= PARA_OPEN_FILTER_s){
oichan 7:4efaca1e9acd 136 if(Alt_cnt >= PARA_OPEN_CNT || para_timer.read() >= PARA_OPEN_TIME_s){
oichan 5:a5cf6914e7c4 137 _open();
oichan 5:a5cf6914e7c4 138 leaf_timer.start();
oichan 5:a5cf6914e7c4 139 oshirase2 = 0;
oichan 5:a5cf6914e7c4 140 para_timer.stop();
oichan 8:b10169df8f5f 141 // pc.printf("para open\r\n");
oichan 7:4efaca1e9acd 142 // for(int i=0;i<10;i++)pc.printf("%f\n\r",Alt_buff[i]);
oichan 5:a5cf6914e7c4 143 break;
oichan 5:a5cf6914e7c4 144 }
oichan 2:6238109d564d 145 }
oichan 2:6238109d564d 146 }
oichan 2:6238109d564d 147
oichan 2:6238109d564d 148 /* リーフィング解除判定 */
oichan 2:6238109d564d 149 while(1){
oichan 2:6238109d564d 150 for(int i=0;i<10;i++){
oichan 2:6238109d564d 151 Alt_buff[i] = _getAlt();
oichan 2:6238109d564d 152 }
oichan 4:ae20e73f4924 153 Altitude = _median(Alt_buff,10);
oichan 7:4efaca1e9acd 154 if(Altitude - Land_Alt <= LEAF_UNLOCK_ALT_m || leaf_timer.read() >= LEAF_UNLOCK_TIME_s){
oichan 2:6238109d564d 155 _leaf_unlock();
oichan 6:e0c25c3976ae 156 oshirase2 = 1;
oichan 2:6238109d564d 157 leaf_timer.stop();
oichan 8:b10169df8f5f 158 // pc.printf("leafing unlock\r\n");
oichan 3:217b56515009 159 // for(int i=0;i<10;i++)pc.printf("%f\n\r",Alt_buff[i]);
oichan 2:6238109d564d 160 break;
oichan 2:6238109d564d 161 }
mikawataru 0:0ff20d8e9090 162 }
mikawataru 0:0ff20d8e9090 163 }
mikawataru 0:0ff20d8e9090 164
oichan 2:6238109d564d 165
oichan 2:6238109d564d 166 /* サーボ用関数 */
oichan 8:b10169df8f5f 167 void _close() {mg996.pulsewidth(0.0005);}
oichan 8:b10169df8f5f 168 void _open() {mg996.pulsewidth(0.0024);}
oichan 8:b10169df8f5f 169 void _leaf_lock() {sg92.pulsewidth(0.0024);}
oichan 8:b10169df8f5f 170 void _leaf_unlock() {sg92.pulsewidth(0.0005);}
oichan 2:6238109d564d 171
oichan 2:6238109d564d 172
oichan 2:6238109d564d 173 /* 高度取得関数 */
oichan 2:6238109d564d 174 float _getAlt(){
oichan 7:4efaca1e9acd 175 float altitude;
oichan 4:ae20e73f4924 176 bmp.ReadData(&Temperature,&Pressure);
oichan 7:4efaca1e9acd 177 altitude = (pow((p0/Pressure), (1.0f/5.257f))-1.0f)*(Temperature+273.15f)/0.0065f;
oichan 7:4efaca1e9acd 178 // pc.printf("%f, %f, %f\r\n",Pressure,Temperature,altitude);
oichan 7:4efaca1e9acd 179 return altitude;
oichan 2:6238109d564d 180 }
oichan 2:6238109d564d 181
oichan 2:6238109d564d 182
oichan 6:e0c25c3976ae 183 /* 合成加速度取得関数 */
oichan 2:6238109d564d 184 float _getAcc(){
oichan 2:6238109d564d 185 mpu.getAccelero(a);
oichan 2:6238109d564d 186 return sqrt(pow(a[0]/9.81,2)+pow(a[1]/9.81,2)+pow(a[2]/9.81,2));
oichan 2:6238109d564d 187 }
oichan 2:6238109d564d 188
oichan 2:6238109d564d 189
oichan 2:6238109d564d 190 /* 中央値計算関数 */
oichan 2:6238109d564d 191 float _median(float data[], int num){//todo:処理時間計測
oichan 2:6238109d564d 192 float *data_cpy, ans;
oichan 2:6238109d564d 193 data_cpy = new float[num];
oichan 2:6238109d564d 194 memcpy(data_cpy,data,sizeof(float)*num);
oichan 2:6238109d564d 195
oichan 2:6238109d564d 196 for(int i=0; i<num; i++){
oichan 2:6238109d564d 197 for(int j=0; j<num-i-1; j++){
oichan 2:6238109d564d 198 if(data_cpy[j]>data_cpy[j+1]){
oichan 2:6238109d564d 199 float buff = data_cpy[j+1];
oichan 2:6238109d564d 200 data_cpy[j+1] = data_cpy[j];
oichan 2:6238109d564d 201 data_cpy[j] = buff;
oichan 2:6238109d564d 202 }
oichan 2:6238109d564d 203 }
oichan 2:6238109d564d 204 }
oichan 2:6238109d564d 205
oichan 2:6238109d564d 206 if(num%2!=0) ans = data_cpy[num/2];
oichan 2:6238109d564d 207 else ans = (data_cpy[num/2-1]+data_cpy[num/2])/2.0;
oichan 2:6238109d564d 208 delete[] data_cpy;
oichan 2:6238109d564d 209 return ans;
oichan 2:6238109d564d 210 }