![](/media/cache/group/default_image.jpg.50x50_q85.jpg)
統合プログラム
Dependencies: mbed Servo BMP180
Landing_Judgement.h@4:6e24a1b3edca, 2021-10-28 (annotated)
- Committer:
- tsubasa_nakajima
- Date:
- Thu Oct 28 08:31:00 2021 +0000
- Revision:
- 4:6e24a1b3edca
- Parent:
- 3:a583276d9fef
- Child:
- 5:e1001bfc423a
add sep para
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
minanao | 0:e7b7def631c2 | 1 | #include "mbed.h" |
minanao | 0:e7b7def631c2 | 2 | #include "BMP180.h" |
tsubasa_nakajima | 3:a583276d9fef | 3 | #define PIN_SDA D4 |
tsubasa_nakajima | 3:a583276d9fef | 4 | #define PIN_SCL D5 |
tsubasa_nakajima | 3:a583276d9fef | 5 | |
tsubasa_nakajima | 4:6e24a1b3edca | 6 | //nの階乗を計算する関数 |
tsubasa_nakajima | 4:6e24a1b3edca | 7 | int fact(int n){ |
tsubasa_nakajima | 4:6e24a1b3edca | 8 | int i, result = 1; |
tsubasa_nakajima | 4:6e24a1b3edca | 9 | if(n == 0){ |
tsubasa_nakajima | 4:6e24a1b3edca | 10 | return 1; |
tsubasa_nakajima | 4:6e24a1b3edca | 11 | } |
tsubasa_nakajima | 4:6e24a1b3edca | 12 | else |
tsubasa_nakajima | 4:6e24a1b3edca | 13 | { |
tsubasa_nakajima | 4:6e24a1b3edca | 14 | for(i = 1;i <= n;i++) |
tsubasa_nakajima | 4:6e24a1b3edca | 15 | { |
tsubasa_nakajima | 4:6e24a1b3edca | 16 | result *= i; |
tsubasa_nakajima | 4:6e24a1b3edca | 17 | } |
tsubasa_nakajima | 4:6e24a1b3edca | 18 | return result; |
tsubasa_nakajima | 4:6e24a1b3edca | 19 | } |
tsubasa_nakajima | 4:6e24a1b3edca | 20 | } |
tsubasa_nakajima | 4:6e24a1b3edca | 21 | |
tsubasa_nakajima | 4:6e24a1b3edca | 22 | float my_pow(float x, int n){ |
tsubasa_nakajima | 4:6e24a1b3edca | 23 | int i; |
tsubasa_nakajima | 4:6e24a1b3edca | 24 | float pow_result = 1; |
tsubasa_nakajima | 4:6e24a1b3edca | 25 | |
tsubasa_nakajima | 4:6e24a1b3edca | 26 | if(n == 0) |
tsubasa_nakajima | 4:6e24a1b3edca | 27 | return 1; |
tsubasa_nakajima | 4:6e24a1b3edca | 28 | else |
tsubasa_nakajima | 4:6e24a1b3edca | 29 | { |
tsubasa_nakajima | 4:6e24a1b3edca | 30 | for(i = 0;i < n;i++) |
tsubasa_nakajima | 4:6e24a1b3edca | 31 | { |
tsubasa_nakajima | 4:6e24a1b3edca | 32 | pow_result *= x; |
tsubasa_nakajima | 4:6e24a1b3edca | 33 | } |
tsubasa_nakajima | 4:6e24a1b3edca | 34 | return pow_result; |
tsubasa_nakajima | 4:6e24a1b3edca | 35 | } |
tsubasa_nakajima | 4:6e24a1b3edca | 36 | } |
tsubasa_nakajima | 4:6e24a1b3edca | 37 | |
tsubasa_nakajima | 4:6e24a1b3edca | 38 | float my_exp(float x) |
tsubasa_nakajima | 4:6e24a1b3edca | 39 | { |
tsubasa_nakajima | 4:6e24a1b3edca | 40 | int i; |
tsubasa_nakajima | 4:6e24a1b3edca | 41 | float result = 0; |
tsubasa_nakajima | 4:6e24a1b3edca | 42 | |
tsubasa_nakajima | 4:6e24a1b3edca | 43 | |
tsubasa_nakajima | 4:6e24a1b3edca | 44 | for(i = 1;i <= 25; i++) |
tsubasa_nakajima | 4:6e24a1b3edca | 45 | { |
tsubasa_nakajima | 4:6e24a1b3edca | 46 | result += my_pow(x, i) / fact(i); |
tsubasa_nakajima | 4:6e24a1b3edca | 47 | } |
tsubasa_nakajima | 4:6e24a1b3edca | 48 | |
tsubasa_nakajima | 4:6e24a1b3edca | 49 | return result + 1; |
tsubasa_nakajima | 4:6e24a1b3edca | 50 | } |
tsubasa_nakajima | 4:6e24a1b3edca | 51 | |
tsubasa_nakajima | 4:6e24a1b3edca | 52 | float my_log(float x) |
tsubasa_nakajima | 4:6e24a1b3edca | 53 | { |
tsubasa_nakajima | 4:6e24a1b3edca | 54 | int i; |
tsubasa_nakajima | 4:6e24a1b3edca | 55 | float result1, result2; |
tsubasa_nakajima | 4:6e24a1b3edca | 56 | |
tsubasa_nakajima | 4:6e24a1b3edca | 57 | x -= 1; |
tsubasa_nakajima | 4:6e24a1b3edca | 58 | result1 = 0; |
tsubasa_nakajima | 4:6e24a1b3edca | 59 | result2 = 0; |
tsubasa_nakajima | 4:6e24a1b3edca | 60 | |
tsubasa_nakajima | 4:6e24a1b3edca | 61 | for(i = 1;i <= 40;i++) |
tsubasa_nakajima | 4:6e24a1b3edca | 62 | { |
tsubasa_nakajima | 4:6e24a1b3edca | 63 | if(i % 2 == 1) |
tsubasa_nakajima | 4:6e24a1b3edca | 64 | result1 += my_pow(x, i) / i; |
tsubasa_nakajima | 4:6e24a1b3edca | 65 | else |
tsubasa_nakajima | 4:6e24a1b3edca | 66 | |
tsubasa_nakajima | 4:6e24a1b3edca | 67 | result2 += my_pow(x, i) / i; |
tsubasa_nakajima | 4:6e24a1b3edca | 68 | } |
tsubasa_nakajima | 4:6e24a1b3edca | 69 | |
tsubasa_nakajima | 4:6e24a1b3edca | 70 | return result1 - result2; |
tsubasa_nakajima | 4:6e24a1b3edca | 71 | } |
tsubasa_nakajima | 4:6e24a1b3edca | 72 | |
tsubasa_nakajima | 4:6e24a1b3edca | 73 | //累乗 |
tsubasa_nakajima | 4:6e24a1b3edca | 74 | float mypow(float x, float y) |
tsubasa_nakajima | 4:6e24a1b3edca | 75 | { |
tsubasa_nakajima | 4:6e24a1b3edca | 76 | return my_exp(y * my_log(x)); |
tsubasa_nakajima | 4:6e24a1b3edca | 77 | } |
tsubasa_nakajima | 4:6e24a1b3edca | 78 | |
tsubasa_nakajima | 4:6e24a1b3edca | 79 | //高度計算 |
tsubasa_nakajima | 4:6e24a1b3edca | 80 | float calculate_h(float dP0FIX,float dp,float dt){ |
tsubasa_nakajima | 4:6e24a1b3edca | 81 | float dpow = 1.0/5.256; |
tsubasa_nakajima | 4:6e24a1b3edca | 82 | float dP0 = 1013.25; |
tsubasa_nakajima | 4:6e24a1b3edca | 83 | float a = (dt+(float)273.15)/(float)0.0065; |
tsubasa_nakajima | 4:6e24a1b3edca | 84 | float s = (mypow(dP0/dp,dpow)- mypow(dP0/dP0FIX,dpow))*a - 27; |
tsubasa_nakajima | 4:6e24a1b3edca | 85 | return s; |
tsubasa_nakajima | 4:6e24a1b3edca | 86 | } |
tsubasa_nakajima | 4:6e24a1b3edca | 87 | |
tsubasa_nakajima | 4:6e24a1b3edca | 88 | //海面気圧計算 |
tsubasa_nakajima | 4:6e24a1b3edca | 89 | float calculate_dp0(float dp,float dt){ |
tsubasa_nakajima | 4:6e24a1b3edca | 90 | float s = dp*mypow(1 - (0.0065*27)/(dt+0.0065*27+273.15),-5.257); |
tsubasa_nakajima | 4:6e24a1b3edca | 91 | return s; |
tsubasa_nakajima | 4:6e24a1b3edca | 92 | } |
tsubasa_nakajima | 4:6e24a1b3edca | 93 | |
tsubasa_nakajima | 3:a583276d9fef | 94 | BMP180 bmp180(PIN_SDA,PIN_SCL); |
minanao | 0:e7b7def631c2 | 95 | |
minanao | 0:e7b7def631c2 | 96 | class Landing_Judgement:public BMP180{ |
minanao | 0:e7b7def631c2 | 97 | private: |
tsubasa_nakajima | 1:bb89b58cfa0e | 98 | |
minanao | 0:e7b7def631c2 | 99 | int x ,y ,n1 , n2 ; |
tsubasa_nakajima | 3:a583276d9fef | 100 | int landing_judgement ; |
minanao | 0:e7b7def631c2 | 101 | float h,dp,dt,dp0; |
minanao | 0:e7b7def631c2 | 102 | float a ,b ,dp_ave,dt_ave; |
minanao | 0:e7b7def631c2 | 103 | //hは高度、dpは気圧、dtは温度、dp0は海面気圧 |
minanao | 0:e7b7def631c2 | 104 | |
minanao | 0:e7b7def631c2 | 105 | public: |
minanao | 0:e7b7def631c2 | 106 | /*ここの関数をlanding_judgementの中に入れ込んじゃいます |
minanao | 0:e7b7def631c2 | 107 | //landing()が1を返せば着地、0を返せば未着地 |
minanao | 0:e7b7def631c2 | 108 | int landing(){ |
minanao | 0:e7b7def631c2 | 109 | return landing_judgement; |
minanao | 0:e7b7def631c2 | 110 | } |
minanao | 0:e7b7def631c2 | 111 | */ |
minanao | 0:e7b7def631c2 | 112 | |
minanao | 0:e7b7def631c2 | 113 | //着地判定の計算を開始させる関数 |
tsubasa_nakajima | 4:6e24a1b3edca | 114 | int landing_judgement1(){ |
tsubasa_nakajima | 4:6e24a1b3edca | 115 | |
tsubasa_nakajima | 4:6e24a1b3edca | 116 | BMP180 bmp180(PIN_SDA,PIN_SCL); |
tsubasa_nakajima | 3:a583276d9fef | 117 | landing_judgement = 0; |
tsubasa_nakajima | 3:a583276d9fef | 118 | x = 0; |
tsubasa_nakajima | 3:a583276d9fef | 119 | y = 0; |
tsubasa_nakajima | 3:a583276d9fef | 120 | n1 = 0; |
tsubasa_nakajima | 3:a583276d9fef | 121 | n2 = 0; |
tsubasa_nakajima | 3:a583276d9fef | 122 | a = 0; |
tsubasa_nakajima | 3:a583276d9fef | 123 | b = 0; |
tsubasa_nakajima | 3:a583276d9fef | 124 | |
tsubasa_nakajima | 4:6e24a1b3edca | 125 | bmp180.Initialize(27,int BMP180_OSS_ULTRA_LOW_POWER); //27は府大の海抜高度 |
minanao | 0:e7b7def631c2 | 126 | |
tsubasa_nakajima | 3:a583276d9fef | 127 | for(int i=0;i<15;i++){ |
minanao | 0:e7b7def631c2 | 128 | if(bmp180.ReadData(&dt,&dp)){ |
minanao | 0:e7b7def631c2 | 129 | a = a + dp; |
minanao | 0:e7b7def631c2 | 130 | b = b + dt; |
minanao | 0:e7b7def631c2 | 131 | n1 = n1 + 1; |
minanao | 0:e7b7def631c2 | 132 | n2 = n2 + 1; |
minanao | 0:e7b7def631c2 | 133 | wait(1); |
minanao | 0:e7b7def631c2 | 134 | } |
minanao | 0:e7b7def631c2 | 135 | } |
minanao | 0:e7b7def631c2 | 136 | |
minanao | 0:e7b7def631c2 | 137 | dp_ave = a / n1; |
minanao | 0:e7b7def631c2 | 138 | dt_ave = b / n2; |
minanao | 0:e7b7def631c2 | 139 | dp0 = calculate_dp0(dp_ave,dt_ave); |
minanao | 0:e7b7def631c2 | 140 | |
minanao | 0:e7b7def631c2 | 141 | while(x<10){ |
minanao | 0:e7b7def631c2 | 142 | if(bmp180.ReadData(&dt,&dp)){ |
minanao | 0:e7b7def631c2 | 143 | h = calculate_h(dp0,dp,dt); |
minanao | 0:e7b7def631c2 | 144 | if(h >= 30){ |
minanao | 0:e7b7def631c2 | 145 | x = x + 1; |
minanao | 0:e7b7def631c2 | 146 | } |
minanao | 0:e7b7def631c2 | 147 | wait(1); |
minanao | 0:e7b7def631c2 | 148 | } |
minanao | 0:e7b7def631c2 | 149 | } |
minanao | 0:e7b7def631c2 | 150 | //10秒以上高度30mにいた場合離陸判定 |
minanao | 0:e7b7def631c2 | 151 | |
minanao | 0:e7b7def631c2 | 152 | wait(10); |
minanao | 0:e7b7def631c2 | 153 | |
minanao | 0:e7b7def631c2 | 154 | while(y<10){ |
minanao | 0:e7b7def631c2 | 155 | if(bmp180.ReadData(&dt,&dp)){ |
minanao | 0:e7b7def631c2 | 156 | h = calculate_h(dp0,dp,dt); |
minanao | 0:e7b7def631c2 | 157 | if(h <= 10){ |
minanao | 0:e7b7def631c2 | 158 | y = y + 1; |
minanao | 0:e7b7def631c2 | 159 | } |
minanao | 0:e7b7def631c2 | 160 | wait(1); |
minanao | 0:e7b7def631c2 | 161 | } |
minanao | 0:e7b7def631c2 | 162 | } |
minanao | 0:e7b7def631c2 | 163 | |
minanao | 0:e7b7def631c2 | 164 | wait(5); |
minanao | 0:e7b7def631c2 | 165 | |
minanao | 0:e7b7def631c2 | 166 | landing_judgement = landing_judgement + 1; |
minanao | 0:e7b7def631c2 | 167 | return landing_judgement; |
minanao | 0:e7b7def631c2 | 168 | |
minanao | 0:e7b7def631c2 | 169 | //離陸判定後、10秒以上高度10m以下にいた場合着地判定 |
minanao | 0:e7b7def631c2 | 170 | |
tsubasa_nakajima | 4:6e24a1b3edca | 171 | } |
tsubasa_nakajima | 4:6e24a1b3edca | 172 | }; |