kohei oikawa / Mbed 2 deprecated 2017_11_1114

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

Dependencies:   BMP180 MPU6050 mbed

Fork of Sample_BMP180 by CORE

main.cpp@4:ae20e73f4924, 2017-10-22 (annotated)

Committer:
oichan
Date:
Sun Oct 22 04:36:54 2017 +0000
Revision:
4:ae20e73f4924
Parent:
3:217b56515009
Child:
5:a5cf6914e7c4
a little change

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