CansatB_2021 / Mbed 2 deprecated cansat_integrated

統合プログラム

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?

UserRevisionLine numberNew 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 };