修正済みby皆川
Dependencies: mbed Servo cansat_integrated_2 BMP180
Dependents: cansat_integrated_2
main.cpp@1:bb89b58cfa0e, 2021-10-27 (annotated)
- Committer:
- tsubasa_nakajima
- Date:
- Wed Oct 27 19:11:01 2021 +0000
- Revision:
- 1:bb89b58cfa0e
- Parent:
- 0:e7b7def631c2
- Child:
- 2:d2cb6b50a8c4
almost complete
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" |
minanao | 0:e7b7def631c2 | 3 | #include "getGPS.h" |
minanao | 0:e7b7def631c2 | 4 | #include "Servo.h" |
tsubasa_nakajima | 1:bb89b58cfa0e | 5 | #include "Movement.h" |
tsubasa_nakajima | 1:bb89b58cfa0e | 6 | #include "direction.h" |
minanao | 0:e7b7def631c2 | 7 | #define PIN_SDA D4 |
minanao | 0:e7b7def631c2 | 8 | #define PIN_SCL D5 |
minanao | 0:e7b7def631c2 | 9 | |
tsubasa_nakajima | 1:bb89b58cfa0e | 10 | GPS gps(D1,D0); |
minanao | 0:e7b7def631c2 | 11 | DigitalOut Nichrome(A6); |
minanao | 0:e7b7def631c2 | 12 | BMP180 bmp180(PIN_SDA,PIN_SCL); |
minanao | 0:e7b7def631c2 | 13 | double bias_la=0,bias_lo=0; |
minanao | 0:e7b7def631c2 | 14 | |
tsubasa_nakajima | 1:bb89b58cfa0e | 15 | //nの階乗を計算する関数 |
tsubasa_nakajima | 1:bb89b58cfa0e | 16 | int fact(int n) |
tsubasa_nakajima | 1:bb89b58cfa0e | 17 | { |
tsubasa_nakajima | 1:bb89b58cfa0e | 18 | int i, result = 1; |
tsubasa_nakajima | 1:bb89b58cfa0e | 19 | |
tsubasa_nakajima | 1:bb89b58cfa0e | 20 | if(n == 0) |
tsubasa_nakajima | 1:bb89b58cfa0e | 21 | return 1; |
tsubasa_nakajima | 1:bb89b58cfa0e | 22 | else |
tsubasa_nakajima | 1:bb89b58cfa0e | 23 | { |
tsubasa_nakajima | 1:bb89b58cfa0e | 24 | for(i = 1;i <= n;i++) |
tsubasa_nakajima | 1:bb89b58cfa0e | 25 | { |
tsubasa_nakajima | 1:bb89b58cfa0e | 26 | result *= i; |
tsubasa_nakajima | 1:bb89b58cfa0e | 27 | } |
tsubasa_nakajima | 1:bb89b58cfa0e | 28 | return result; |
tsubasa_nakajima | 1:bb89b58cfa0e | 29 | } |
tsubasa_nakajima | 1:bb89b58cfa0e | 30 | } |
tsubasa_nakajima | 1:bb89b58cfa0e | 31 | |
tsubasa_nakajima | 1:bb89b58cfa0e | 32 | float my_pow(float x, int n) |
tsubasa_nakajima | 1:bb89b58cfa0e | 33 | { |
tsubasa_nakajima | 1:bb89b58cfa0e | 34 | int i; |
tsubasa_nakajima | 1:bb89b58cfa0e | 35 | float pow_result = 1; |
tsubasa_nakajima | 1:bb89b58cfa0e | 36 | |
tsubasa_nakajima | 1:bb89b58cfa0e | 37 | if(n == 0) |
tsubasa_nakajima | 1:bb89b58cfa0e | 38 | return 1; |
tsubasa_nakajima | 1:bb89b58cfa0e | 39 | else |
tsubasa_nakajima | 1:bb89b58cfa0e | 40 | { |
tsubasa_nakajima | 1:bb89b58cfa0e | 41 | for(i = 0;i < n;i++) |
tsubasa_nakajima | 1:bb89b58cfa0e | 42 | { |
tsubasa_nakajima | 1:bb89b58cfa0e | 43 | pow_result *= x; |
tsubasa_nakajima | 1:bb89b58cfa0e | 44 | } |
tsubasa_nakajima | 1:bb89b58cfa0e | 45 | return pow_result; |
tsubasa_nakajima | 1:bb89b58cfa0e | 46 | } |
tsubasa_nakajima | 1:bb89b58cfa0e | 47 | } |
tsubasa_nakajima | 1:bb89b58cfa0e | 48 | |
tsubasa_nakajima | 1:bb89b58cfa0e | 49 | float my_exp(float x) |
tsubasa_nakajima | 1:bb89b58cfa0e | 50 | { |
tsubasa_nakajima | 1:bb89b58cfa0e | 51 | int i; |
tsubasa_nakajima | 1:bb89b58cfa0e | 52 | float result = 0; |
tsubasa_nakajima | 1:bb89b58cfa0e | 53 | |
tsubasa_nakajima | 1:bb89b58cfa0e | 54 | |
tsubasa_nakajima | 1:bb89b58cfa0e | 55 | for(i = 1;i <= 25; i++) |
tsubasa_nakajima | 1:bb89b58cfa0e | 56 | { |
tsubasa_nakajima | 1:bb89b58cfa0e | 57 | result += my_pow(x, i) / fact(i); |
tsubasa_nakajima | 1:bb89b58cfa0e | 58 | } |
tsubasa_nakajima | 1:bb89b58cfa0e | 59 | |
tsubasa_nakajima | 1:bb89b58cfa0e | 60 | return result + 1; |
tsubasa_nakajima | 1:bb89b58cfa0e | 61 | } |
tsubasa_nakajima | 1:bb89b58cfa0e | 62 | |
tsubasa_nakajima | 1:bb89b58cfa0e | 63 | float my_log(float x) |
tsubasa_nakajima | 1:bb89b58cfa0e | 64 | { |
tsubasa_nakajima | 1:bb89b58cfa0e | 65 | int i; |
tsubasa_nakajima | 1:bb89b58cfa0e | 66 | float result1, result2; |
tsubasa_nakajima | 1:bb89b58cfa0e | 67 | |
tsubasa_nakajima | 1:bb89b58cfa0e | 68 | x -= 1; |
tsubasa_nakajima | 1:bb89b58cfa0e | 69 | result1 = 0; |
tsubasa_nakajima | 1:bb89b58cfa0e | 70 | result2 = 0; |
tsubasa_nakajima | 1:bb89b58cfa0e | 71 | |
tsubasa_nakajima | 1:bb89b58cfa0e | 72 | for(i = 1;i <= 40;i++) |
tsubasa_nakajima | 1:bb89b58cfa0e | 73 | { |
tsubasa_nakajima | 1:bb89b58cfa0e | 74 | if(i % 2 == 1) |
tsubasa_nakajima | 1:bb89b58cfa0e | 75 | result1 += my_pow(x, i) / i; |
tsubasa_nakajima | 1:bb89b58cfa0e | 76 | else |
tsubasa_nakajima | 1:bb89b58cfa0e | 77 | |
tsubasa_nakajima | 1:bb89b58cfa0e | 78 | result2 += my_pow(x, i) / i; |
tsubasa_nakajima | 1:bb89b58cfa0e | 79 | } |
tsubasa_nakajima | 1:bb89b58cfa0e | 80 | |
tsubasa_nakajima | 1:bb89b58cfa0e | 81 | return result1 - result2; |
tsubasa_nakajima | 1:bb89b58cfa0e | 82 | } |
tsubasa_nakajima | 1:bb89b58cfa0e | 83 | |
tsubasa_nakajima | 1:bb89b58cfa0e | 84 | //累乗 |
tsubasa_nakajima | 1:bb89b58cfa0e | 85 | float mypow(float x, float y) |
tsubasa_nakajima | 1:bb89b58cfa0e | 86 | { |
tsubasa_nakajima | 1:bb89b58cfa0e | 87 | return my_exp(y * my_log(x)); |
tsubasa_nakajima | 1:bb89b58cfa0e | 88 | } |
tsubasa_nakajima | 1:bb89b58cfa0e | 89 | |
tsubasa_nakajima | 1:bb89b58cfa0e | 90 | //高度計算 |
tsubasa_nakajima | 1:bb89b58cfa0e | 91 | float calculate_h(float dP0FIX,float dp,float dt){ |
tsubasa_nakajima | 1:bb89b58cfa0e | 92 | float dpow = 1.0/5.256; |
tsubasa_nakajima | 1:bb89b58cfa0e | 93 | float dP0 = 1013.25; |
tsubasa_nakajima | 1:bb89b58cfa0e | 94 | float a = (dt+(float)273.15)/(float)0.0065; |
tsubasa_nakajima | 1:bb89b58cfa0e | 95 | float s = (mypow(dP0/dp,dpow)- mypow(dP0/dP0FIX,dpow))*a - 27; |
tsubasa_nakajima | 1:bb89b58cfa0e | 96 | return s; |
tsubasa_nakajima | 1:bb89b58cfa0e | 97 | } |
tsubasa_nakajima | 1:bb89b58cfa0e | 98 | |
tsubasa_nakajima | 1:bb89b58cfa0e | 99 | //海面気圧計算 |
tsubasa_nakajima | 1:bb89b58cfa0e | 100 | float calculate_dp0(float dp,float dt){ |
tsubasa_nakajima | 1:bb89b58cfa0e | 101 | float s = dp*mypow(1 - (0.0065*27)/(dt+0.0065*27+273.15),-5.257); |
tsubasa_nakajima | 1:bb89b58cfa0e | 102 | return s; |
tsubasa_nakajima | 1:bb89b58cfa0e | 103 | } |
tsubasa_nakajima | 1:bb89b58cfa0e | 104 | |
minanao | 0:e7b7def631c2 | 105 | int landing_judgement(){ |
tsubasa_nakajima | 1:bb89b58cfa0e | 106 | |
tsubasa_nakajima | 1:bb89b58cfa0e | 107 | int x ,y ,n1 , n2 ; |
tsubasa_nakajima | 1:bb89b58cfa0e | 108 | int landing_judgement=0 ; |
tsubasa_nakajima | 1:bb89b58cfa0e | 109 | float h,dp,dt,dp0; |
tsubasa_nakajima | 1:bb89b58cfa0e | 110 | float a ,b ,dp_ave,dt_ave; |
tsubasa_nakajima | 1:bb89b58cfa0e | 111 | |
tsubasa_nakajima | 1:bb89b58cfa0e | 112 | bmp180.Initialize(27,BMP180_OSS_ULTRA_LOW_POWER);//27は府大の海抜高度 |
tsubasa_nakajima | 1:bb89b58cfa0e | 113 | |
tsubasa_nakajima | 1:bb89b58cfa0e | 114 | for(int i=0;i<15;i++){ |
minanao | 0:e7b7def631c2 | 115 | if(bmp180.ReadData(&dt,&dp)){ |
minanao | 0:e7b7def631c2 | 116 | a = a + dp; |
minanao | 0:e7b7def631c2 | 117 | b = b + dt; |
minanao | 0:e7b7def631c2 | 118 | n1 = n1 + 1; |
minanao | 0:e7b7def631c2 | 119 | n2 = n2 + 1; |
minanao | 0:e7b7def631c2 | 120 | wait(1); |
minanao | 0:e7b7def631c2 | 121 | } |
minanao | 0:e7b7def631c2 | 122 | } |
minanao | 0:e7b7def631c2 | 123 | |
minanao | 0:e7b7def631c2 | 124 | dp_ave = a / n1; |
minanao | 0:e7b7def631c2 | 125 | dt_ave = b / n2; |
minanao | 0:e7b7def631c2 | 126 | dp0 = calculate_dp0(dp_ave,dt_ave); |
tsubasa_nakajima | 1:bb89b58cfa0e | 127 | |
minanao | 0:e7b7def631c2 | 128 | while(x<10){ |
minanao | 0:e7b7def631c2 | 129 | if(bmp180.ReadData(&dt,&dp)){ |
minanao | 0:e7b7def631c2 | 130 | h = calculate_h(dp0,dp,dt); |
minanao | 0:e7b7def631c2 | 131 | if(h >= 30){ |
minanao | 0:e7b7def631c2 | 132 | x = x + 1; |
minanao | 0:e7b7def631c2 | 133 | } |
minanao | 0:e7b7def631c2 | 134 | wait(1); |
minanao | 0:e7b7def631c2 | 135 | } |
minanao | 0:e7b7def631c2 | 136 | } |
minanao | 0:e7b7def631c2 | 137 | //10秒以上高度30mにいた場合離陸判定 |
minanao | 0:e7b7def631c2 | 138 | |
minanao | 0:e7b7def631c2 | 139 | wait(10); |
minanao | 0:e7b7def631c2 | 140 | |
minanao | 0:e7b7def631c2 | 141 | while(y<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 <= 10){ |
minanao | 0:e7b7def631c2 | 145 | y = y + 1; |
minanao | 0:e7b7def631c2 | 146 | } |
minanao | 0:e7b7def631c2 | 147 | wait(1); |
minanao | 0:e7b7def631c2 | 148 | } |
minanao | 0:e7b7def631c2 | 149 | } |
minanao | 0:e7b7def631c2 | 150 | |
minanao | 0:e7b7def631c2 | 151 | wait(5); |
minanao | 0:e7b7def631c2 | 152 | |
minanao | 0:e7b7def631c2 | 153 | landing_judgement = landing_judgement + 1; |
minanao | 0:e7b7def631c2 | 154 | return landing_judgement; |
minanao | 0:e7b7def631c2 | 155 | |
minanao | 0:e7b7def631c2 | 156 | //離陸判定後、10秒以上高度10m以下にいた場合着地判定 |
minanao | 0:e7b7def631c2 | 157 | |
tsubasa_nakajima | 1:bb89b58cfa0e | 158 | } |
tsubasa_nakajima | 1:bb89b58cfa0e | 159 | |
minanao | 0:e7b7def631c2 | 160 | int parachute_separation() |
minanao | 0:e7b7def631c2 | 161 | { |
minanao | 0:e7b7def631c2 | 162 | Nichrome=1; |
minanao | 0:e7b7def631c2 | 163 | wait(10); |
minanao | 0:e7b7def631c2 | 164 | Nichrome=0; |
minanao | 0:e7b7def631c2 | 165 | return 0; |
minanao | 0:e7b7def631c2 | 166 | } |
minanao | 0:e7b7def631c2 | 167 | |
minanao | 0:e7b7def631c2 | 168 | int main(void) |
minanao | 0:e7b7def631c2 | 169 | { |
minanao | 0:e7b7def631c2 | 170 | int land_judgement_1=0,sep_judge=1; |
tsubasa_nakajima | 1:bb89b58cfa0e | 171 | |
minanao | 0:e7b7def631c2 | 172 | //着地判定 |
minanao | 0:e7b7def631c2 | 173 | while(1) |
minanao | 0:e7b7def631c2 | 174 | { |
tsubasa_nakajima | 1:bb89b58cfa0e | 175 | land_judgement_1 = landing_judgement(); |
minanao | 0:e7b7def631c2 | 176 | if(land_judgement_1==1) |
minanao | 0:e7b7def631c2 | 177 | { |
minanao | 0:e7b7def631c2 | 178 | break; |
minanao | 0:e7b7def631c2 | 179 | } |
minanao | 0:e7b7def631c2 | 180 | } |
tsubasa_nakajima | 1:bb89b58cfa0e | 181 | |
minanao | 0:e7b7def631c2 | 182 | wait(30); |
minanao | 0:e7b7def631c2 | 183 | |
tsubasa_nakajima | 1:bb89b58cfa0e | 184 | //中間地点を経由してゴール地点まで自律移動 |
tsubasa_nakajima | 1:bb89b58cfa0e | 185 | direction.walk(); |
minanao | 0:e7b7def631c2 | 186 | |
tsubasa_nakajima | 1:bb89b58cfa0e | 187 | return 0; |
minanao | 0:e7b7def631c2 | 188 | |
minanao | 0:e7b7def631c2 | 189 | } |
minanao | 0:e7b7def631c2 | 190 | |
minanao | 0:e7b7def631c2 | 191 | |
minanao | 0:e7b7def631c2 | 192 |