CansatB_2021 / Mbed 2 deprecated cansat_integrated_2

修正済み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?

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