kohei oikawa / Mbed 2 deprecated 2017_11_1114

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

Dependencies:   BMP180 MPU6050 mbed

Fork of Sample_BMP180 by CORE

main.cpp@3:217b56515009, 2017-10-21 (annotated)

Committer:
oichan
Date:
Sat Oct 21 16:10:49 2017 +0000
Revision:
3:217b56515009
Parent:
2:6238109d564d
Child:
4:ae20e73f4924
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 3:217b56515009 6 #define LAUNCH_JUDGE_ACC_g -1.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 2:6238109d564d 12 BMP180 bmp(dp5,dp27);
oichan 2:6238109d564d 13 MPU6050 mpu(dp5,dp27);
oichan 2:6238109d564d 14 Timer para_timer;
oichan 2:6238109d564d 15 Timer leaf_timer;
oichan 2:6238109d564d 16 Ticker readtimer;
oichan 3:217b56515009 17 PwmOut mg996(dp1);
oichan 3:217b56515009 18 PwmOut sg92(dp2);
oichan 2:6238109d564d 19 DigitalOut myled(LED1);
oichan 3:217b56515009 20 //Serial pc(USBTX,USBRX);
mikawataru 0:0ff20d8e9090 21
oichan 2:6238109d564d 22 /* 自作関数 */
oichan 2:6238109d564d 23 void _flight();
oichan 2:6238109d564d 24 void _open();
oichan 2:6238109d564d 25 void _close();
oichan 2:6238109d564d 26 void _leaf_lock();
oichan 2:6238109d564d 27 void _leaf_unlock();
oichan 2:6238109d564d 28 float _getAlt();
oichan 2:6238109d564d 29 float _getAcc();
oichan 2:6238109d564d 30 float _median(float data[], int num);
mikawataru 0:0ff20d8e9090 31
oichan 2:6238109d564d 32 /* グローバル変数 */
oichan 2:6238109d564d 33 int Alt_cnt;
oichan 2:6238109d564d 34 float a[3],acc;
oichan 2:6238109d564d 35 float pressure,temperature,altitude;
oichan 2:6238109d564d 36 float Alt_buff[10],Acc_buff[10];
oichan 2:6238109d564d 37 float land_alt;
oichan 2:6238109d564d 38 float t;
oichan 2:6238109d564d 39
mikawataru 0:0ff20d8e9090 40
mikawataru 0:0ff20d8e9090 41 int main() {
oichan 3:217b56515009 42 // pc.printf("Hello!");
oichan 3:217b56515009 43 mg996.period_ms(20);
oichan 2:6238109d564d 44 sg92.period_ms(20);
oichan 2:6238109d564d 45 _close();
oichan 2:6238109d564d 46 _leaf_lock();
oichan 2:6238109d564d 47 mpu.setAcceleroRange(0);
mikawataru 0:0ff20d8e9090 48 bmp.Initialize(64,BMP180_OSS_ULTRA_LOW_POWER);
oichan 2:6238109d564d 49 _flight();
oichan 2:6238109d564d 50 while(1);
oichan 2:6238109d564d 51 }
oichan 2:6238109d564d 52
oichan 2:6238109d564d 53
oichan 3:217b56515009 54 /* フライトモード用関数 */
oichan 2:6238109d564d 55 void _flight(){
mikawataru 0:0ff20d8e9090 56
oichan 2:6238109d564d 57 /* 地上高度取得 */
oichan 2:6238109d564d 58 for(int i=0;i<10;i++){
oichan 2:6238109d564d 59 Alt_buff[i] = _getAlt();
oichan 2:6238109d564d 60 }
oichan 2:6238109d564d 61 land_alt = _median(Alt_buff,10);
oichan 3:217b56515009 62 // for(int i=0;i<10;i++)pc.printf("%f\n\r",Alt_buff[i]);
oichan 2:6238109d564d 63
oichan 2:6238109d564d 64 /* 発射判定 */
oichan 2:6238109d564d 65 while(1){
oichan 3:217b56515009 66 myled = !myled;
oichan 2:6238109d564d 67 for(int i=0;i<10;i++){
oichan 2:6238109d564d 68 acc = _getAcc();
oichan 2:6238109d564d 69 Acc_buff[i] = acc;
oichan 2:6238109d564d 70 }
oichan 2:6238109d564d 71 acc = _median(Acc_buff,10);
oichan 2:6238109d564d 72 if(acc >= LAUNCH_JUDGE_ACC_g){
oichan 2:6238109d564d 73 para_timer.start();
oichan 3:217b56515009 74 // for(int i=0;i<10;i++)pc.printf("%f\n\r",Acc_buff[i]);
oichan 2:6238109d564d 75 break;
oichan 2:6238109d564d 76 }
oichan 2:6238109d564d 77 }
oichan 2:6238109d564d 78
oichan 2:6238109d564d 79 /* 開放判定 */
oichan 2:6238109d564d 80 while(1){
oichan 2:6238109d564d 81 Alt_cnt = 0;
oichan 2:6238109d564d 82 Alt_buff[0] = _getAlt();
oichan 2:6238109d564d 83 for(int i=1;i<10;i++){
oichan 2:6238109d564d 84 Alt_buff[i] = _getAlt();
oichan 2:6238109d564d 85 if(Alt_buff[i] < Alt_buff[i-1]) Alt_cnt++;
oichan 2:6238109d564d 86 }
oichan 2:6238109d564d 87 if(Alt_cnt > 6 || para_timer.read() >= PARA_OPEN_TIME_s){
oichan 2:6238109d564d 88 _open();
oichan 2:6238109d564d 89 leaf_timer.start();
oichan 2:6238109d564d 90 para_timer.stop();
oichan 3:217b56515009 91 // for(int i=0;i<10;i++)pc.printf("%f\n\r",Alt_buff[i]);
oichan 2:6238109d564d 92 break;
oichan 2:6238109d564d 93 }
oichan 2:6238109d564d 94 }
oichan 2:6238109d564d 95
oichan 2:6238109d564d 96 /* リーフィング解除判定 */
oichan 2:6238109d564d 97 while(1){
oichan 2:6238109d564d 98 for(int i=0;i<10;i++){
oichan 2:6238109d564d 99 Alt_buff[i] = _getAlt();
oichan 2:6238109d564d 100 }
oichan 2:6238109d564d 101 altitude = _median(Alt_buff,10);
oichan 2:6238109d564d 102 if(altitude <= LEAF_UNLOCK_ALT_m || leaf_timer.read() >= LEAF_UNLOCK_TIME_s){
oichan 2:6238109d564d 103 _leaf_unlock();
oichan 2:6238109d564d 104 leaf_timer.stop();
oichan 3:217b56515009 105 // for(int i=0;i<10;i++)pc.printf("%f\n\r",Alt_buff[i]);
oichan 2:6238109d564d 106 break;
oichan 2:6238109d564d 107 }
mikawataru 0:0ff20d8e9090 108 }
mikawataru 0:0ff20d8e9090 109 }
mikawataru 0:0ff20d8e9090 110
oichan 2:6238109d564d 111
oichan 2:6238109d564d 112 /* サーボ用関数 */
oichan 3:217b56515009 113 void _close() {mg996.pulsewidth(0.0006);}
oichan 3:217b56515009 114 void _open() {mg996.pulsewidth(0.0023);}
oichan 2:6238109d564d 115 void _leaf_lock() {sg92.pulsewidth(0.0005);}
oichan 2:6238109d564d 116 void _leaf_unlock() {sg92.pulsewidth(0.0023);}
oichan 2:6238109d564d 117
oichan 2:6238109d564d 118
oichan 2:6238109d564d 119 /* 高度取得関数 */
oichan 2:6238109d564d 120 float _getAlt(){
oichan 2:6238109d564d 121 bmp.ReadData(&temperature,&pressure);
oichan 2:6238109d564d 122 return (pow((p0/pressure), (1.0f/5.257f))-1.0f)*(temperature+273.15f)/0.0065f;
oichan 2:6238109d564d 123 }
oichan 2:6238109d564d 124
oichan 2:6238109d564d 125
oichan 2:6238109d564d 126 /* 加速度取得関数 */
oichan 2:6238109d564d 127 float _getAcc(){
oichan 2:6238109d564d 128 mpu.getAccelero(a);
oichan 2:6238109d564d 129 return sqrt(pow(a[0]/9.81,2)+pow(a[1]/9.81,2)+pow(a[2]/9.81,2));
oichan 2:6238109d564d 130 }
oichan 2:6238109d564d 131
oichan 2:6238109d564d 132
oichan 2:6238109d564d 133 /* 中央値計算関数 */
oichan 2:6238109d564d 134 float _median(float data[], int num){//todo:処理時間計測
oichan 2:6238109d564d 135 float *data_cpy, ans;
oichan 2:6238109d564d 136 data_cpy = new float[num];
oichan 2:6238109d564d 137 memcpy(data_cpy,data,sizeof(float)*num);
oichan 2:6238109d564d 138
oichan 2:6238109d564d 139 for(int i=0; i<num; i++){
oichan 2:6238109d564d 140 for(int j=0; j<num-i-1; j++){
oichan 2:6238109d564d 141 if(data_cpy[j]>data_cpy[j+1]){
oichan 2:6238109d564d 142 float buff = data_cpy[j+1];
oichan 2:6238109d564d 143 data_cpy[j+1] = data_cpy[j];
oichan 2:6238109d564d 144 data_cpy[j] = buff;
oichan 2:6238109d564d 145 }
oichan 2:6238109d564d 146 }
oichan 2:6238109d564d 147 }
oichan 2:6238109d564d 148
oichan 2:6238109d564d 149 if(num%2!=0) ans = data_cpy[num/2];
oichan 2:6238109d564d 150 else ans = (data_cpy[num/2-1]+data_cpy[num/2])/2.0;
oichan 2:6238109d564d 151 delete[] data_cpy;
oichan 2:6238109d564d 152 return ans;
oichan 2:6238109d564d 153 }