CansatB_2021 / Mbed 2 deprecated cansat_integrated_2

修正済みby皆川

Dependencies:   mbed Servo cansat_integrated_2 BMP180

Dependents:   cansat_integrated_2

main.cpp@0:e7b7def631c2, 2021-10-21 (annotated)

Committer:
minanao
Date:
Thu Oct 21 01:58:35 2021 +0000
Revision:
0:e7b7def631c2
Child:
1:bb89b58cfa0e
integrated program

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"
minanao 0:e7b7def631c2 5 #include "calculate.h"
minanao 0:e7b7def631c2 6 #include "MPU9250.h"
minanao 0:e7b7def631c2 7 #define PIN_SDA D4
minanao 0:e7b7def631c2 8 #define PIN_SCL D5
minanao 0:e7b7def631c2 9 #define STOPTIME 1
minanao 0:e7b7def631c2 10 #define GOTIME 5
minanao 0:e7b7def631c2 11 #define ROTATETIME 1
minanao 0:e7b7def631c2 12
minanao 0:e7b7def631c2 13
minanao 0:e7b7def631c2 14 Serial xbee(PA_9, PA_10);
minanao 0:e7b7def631c2 15 GPS gps(PA_2,PA_3);
minanao 0:e7b7def631c2 16 DigitalOut Nichrome(A6);
minanao 0:e7b7def631c2 17 BMP180 bmp180(PIN_SDA,PIN_SCL);
minanao 0:e7b7def631c2 18 MPU9250 mpu9250(D4, D5);
minanao 0:e7b7def631c2 19 Servo myservo1(D7);
minanao 0:e7b7def631c2 20 Servo myservo2(A3);
minanao 0:e7b7def631c2 21 Servo myservo3(A1);
minanao 0:e7b7def631c2 22 Servo myservo4(D12);
minanao 0:e7b7def631c2 23 Servo myservo5(D10);
minanao 0:e7b7def631c2 24 Servo myservo6(A5);
minanao 0:e7b7def631c2 25 double bias_la=0,bias_lo=0;
minanao 0:e7b7def631c2 26
minanao 0:e7b7def631c2 27 int landing_judgement(){
minanao 0:e7b7def631c2 28 bmp180.Initialize(27,0);
minanao 0:e7b7def631c2 29 int i,n1,n2,landing_judgement;
minanao 0:e7b7def631c2 30 float dt,dp,h,dp0,a,b,dt_ave,dp_ave;
minanao 0:e7b7def631c2 31 for(i=0;i<15;i++){
minanao 0:e7b7def631c2 32 if(bmp180.ReadData(&dt,&dp)){
minanao 0:e7b7def631c2 33 a = a + dp;
minanao 0:e7b7def631c2 34 b = b + dt;
minanao 0:e7b7def631c2 35 n1 = n1 + 1;
minanao 0:e7b7def631c2 36 n2 = n2 + 1;
minanao 0:e7b7def631c2 37 wait(1);
minanao 0:e7b7def631c2 38 }
minanao 0:e7b7def631c2 39 }
minanao 0:e7b7def631c2 40
minanao 0:e7b7def631c2 41 dp_ave = a / n1;
minanao 0:e7b7def631c2 42 dt_ave = b / n2;
minanao 0:e7b7def631c2 43 dp0 = calculate_dp0(dp_ave,dt_ave);
minanao 0:e7b7def631c2 44 int x,y;
minanao 0:e7b7def631c2 45 while(x<10){
minanao 0:e7b7def631c2 46 if(bmp180.ReadData(&dt,&dp)){
minanao 0:e7b7def631c2 47 h = calculate_h(dp0,dp,dt);
minanao 0:e7b7def631c2 48 if(h >= 30){
minanao 0:e7b7def631c2 49 x = x + 1;
minanao 0:e7b7def631c2 50 }
minanao 0:e7b7def631c2 51 wait(1);
minanao 0:e7b7def631c2 52 }
minanao 0:e7b7def631c2 53 }
minanao 0:e7b7def631c2 54 //10秒以上高度30mにいた場合離陸判定
minanao 0:e7b7def631c2 55
minanao 0:e7b7def631c2 56 wait(10);
minanao 0:e7b7def631c2 57
minanao 0:e7b7def631c2 58 while(y<10){
minanao 0:e7b7def631c2 59 if(bmp180.ReadData(&dt,&dp)){
minanao 0:e7b7def631c2 60 h = calculate_h(dp0,dp,dt);
minanao 0:e7b7def631c2 61 if(h <= 10){
minanao 0:e7b7def631c2 62 y = y + 1;
minanao 0:e7b7def631c2 63 }
minanao 0:e7b7def631c2 64 wait(1);
minanao 0:e7b7def631c2 65 }
minanao 0:e7b7def631c2 66 }
minanao 0:e7b7def631c2 67
minanao 0:e7b7def631c2 68 wait(5);
minanao 0:e7b7def631c2 69
minanao 0:e7b7def631c2 70 landing_judgement = landing_judgement + 1;
minanao 0:e7b7def631c2 71 return landing_judgement;
minanao 0:e7b7def631c2 72
minanao 0:e7b7def631c2 73 //離陸判定後、10秒以上高度10m以下にいた場合着地判定
minanao 0:e7b7def631c2 74
minanao 0:e7b7def631c2 75 }
minanao 0:e7b7def631c2 76
minanao 0:e7b7def631c2 77
minanao 0:e7b7def631c2 78 //止まる
minanao 0:e7b7def631c2 79 void stop()
minanao 0:e7b7def631c2 80 {
minanao 0:e7b7def631c2 81 myservo1 = 0.5;
minanao 0:e7b7def631c2 82 myservo2 = 0.5;
minanao 0:e7b7def631c2 83 myservo3 = 0.5;
minanao 0:e7b7def631c2 84 myservo4 = 0.5;
minanao 0:e7b7def631c2 85 myservo5 = 0.5;
minanao 0:e7b7def631c2 86 myservo6 = 0.5;
minanao 0:e7b7def631c2 87 wait(STOPTIME);
minanao 0:e7b7def631c2 88 }
minanao 0:e7b7def631c2 89
minanao 0:e7b7def631c2 90 //前進
minanao 0:e7b7def631c2 91 void move_forward()
minanao 0:e7b7def631c2 92 {
minanao 0:e7b7def631c2 93 myservo1 = 0;
minanao 0:e7b7def631c2 94 myservo2 = 0;
minanao 0:e7b7def631c2 95 myservo3 = 0;
minanao 0:e7b7def631c2 96 myservo4 = 0;
minanao 0:e7b7def631c2 97 myservo5 = 0;
minanao 0:e7b7def631c2 98 myservo6 = 0;
minanao 0:e7b7def631c2 99 wait(GOTIME);
minanao 0:e7b7def631c2 100 }
minanao 0:e7b7def631c2 101
minanao 0:e7b7def631c2 102 //後退
minanao 0:e7b7def631c2 103 void move_backward()
minanao 0:e7b7def631c2 104 {
minanao 0:e7b7def631c2 105 myservo1 = 1;
minanao 0:e7b7def631c2 106 myservo2 = 1;
minanao 0:e7b7def631c2 107 myservo3 = 1;
minanao 0:e7b7def631c2 108 myservo4 = 1;
minanao 0:e7b7def631c2 109 myservo5 = 1;
minanao 0:e7b7def631c2 110 myservo6 = 1;
minanao 0:e7b7def631c2 111 wait(GOTIME);
minanao 0:e7b7def631c2 112 }
minanao 0:e7b7def631c2 113
minanao 0:e7b7def631c2 114 //右に曲がる
minanao 0:e7b7def631c2 115 void turn_right()
minanao 0:e7b7def631c2 116 {
minanao 0:e7b7def631c2 117 myservo1 = 1;
minanao 0:e7b7def631c2 118 myservo2 = 1;
minanao 0:e7b7def631c2 119 myservo3 = 1;
minanao 0:e7b7def631c2 120 myservo4 = 0;
minanao 0:e7b7def631c2 121 myservo5 = 0;
minanao 0:e7b7def631c2 122 myservo6 = 0;
minanao 0:e7b7def631c2 123 wait(ROTATETIME);
minanao 0:e7b7def631c2 124 }
minanao 0:e7b7def631c2 125
minanao 0:e7b7def631c2 126 //左に曲がる
minanao 0:e7b7def631c2 127 void turn_left()
minanao 0:e7b7def631c2 128 {
minanao 0:e7b7def631c2 129 myservo1 = 0;
minanao 0:e7b7def631c2 130 myservo2 = 0;
minanao 0:e7b7def631c2 131 myservo3 = 0;
minanao 0:e7b7def631c2 132 myservo4 = 1;
minanao 0:e7b7def631c2 133 myservo5 = 1;
minanao 0:e7b7def631c2 134 myservo6 = 1;
minanao 0:e7b7def631c2 135 wait(ROTATETIME);
minanao 0:e7b7def631c2 136 }
minanao 0:e7b7def631c2 137
minanao 0:e7b7def631c2 138 //倒れているときの処理
minanao 0:e7b7def631c2 139 void wakeup(int time)
minanao 0:e7b7def631c2 140 {
minanao 0:e7b7def631c2 141 int i;
minanao 0:e7b7def631c2 142 for(i=1;i<=time;i++)
minanao 0:e7b7def631c2 143 {
minanao 0:e7b7def631c2 144 move_forward()
minanao 0:e7b7def631c2 145 move_backward()
minanao 0:e7b7def631c2 146 turn_right()
minanao 0:e7b7def631c2 147 turn_left()
minanao 0:e7b7def631c2 148 }
minanao 0:e7b7def631c2 149 }
minanao 0:e7b7def631c2 150
minanao 0:e7b7def631c2 151
minanao 0:e7b7def631c2 152
minanao 0:e7b7def631c2 153 float calculation_sita(float x_0,float y_0,float x_1,float y_1,float x,float y){
minanao 0:e7b7def631c2 154 //x,yは地磁気センサの値で北の方角の角度を90度、東の方角を0度とする。
minanao 0:e7b7def631c2 155 float sita_0 = atan((y_0 - y_1)/(x_0 - x_1)); //目的地の角度
minanao 0:e7b7def631c2 156
minanao 0:e7b7def631c2 157 if(y_0 - y_1 > 0 && x_0 - x_1 < 0){
minanao 0:e7b7def631c2 158 sita_0 = sita_0 - 180;
minanao 0:e7b7def631c2 159 }
minanao 0:e7b7def631c2 160 if(y_0 - y_1 < 0 && x_0 - x_1 < 0){
minanao 0:e7b7def631c2 161 sita_0 = sita_0 + 180;
minanao 0:e7b7def631c2 162 }
minanao 0:e7b7def631c2 163
minanao 0:e7b7def631c2 164 float Omag_x=0,Omag_y=0; //地磁気センサのxy平面が描く円の中点
minanao 0:e7b7def631c2 165 float sita_1 = atan((y - Omag_y)/(x- Omag_x));　//CanSatの角度
minanao 0:e7b7def631c2 166
minanao 0:e7b7def631c2 167 if(y - Omag_y > 0 && x- Omag_x < 0){
minanao 0:e7b7def631c2 168 sita_1 = sita_1 - 180;
minanao 0:e7b7def631c2 169 }
minanao 0:e7b7def631c2 170 if(y - Omag_y < 0 && x - Omag_x < 0){
minanao 0:e7b7def631c2 171 sita_1 = sita_1 + 180;
minanao 0:e7b7def631c2 172 }
minanao 0:e7b7def631c2 173 float sita
minanao 0:e7b7def631c2 174 sita= sita_0 - sita_1; //CanSatから見た目的地の角度
minanao 0:e7b7def631c2 175 return sita
minanao 0:e7b7def631c2 176 }
minanao 0:e7b7def631c2 177
minanao 0:e7b7def631c2 178
minanao 0:e7b7def631c2 179
minanao 0:e7b7def631c2 180 int parachute_separation()
minanao 0:e7b7def631c2 181 {
minanao 0:e7b7def631c2 182 Nichrome=1;
minanao 0:e7b7def631c2 183 wait(10);
minanao 0:e7b7def631c2 184 Nichrome=0;
minanao 0:e7b7def631c2 185 return 0;
minanao 0:e7b7def631c2 186 }
minanao 0:e7b7def631c2 187
minanao 0:e7b7def631c2 188 int end_flag=0;
minanao 0:e7b7def631c2 189 double bias_la=0,bias_lo=0;
minanao 0:e7b7def631c2 190
minanao 0:e7b7def631c2 191
minanao 0:e7b7def631c2 192
minanao 0:e7b7def631c2 193 int main(void)
minanao 0:e7b7def631c2 194 {
minanao 0:e7b7def631c2 195 int land_judgement_1=0,sep_judge=1;
minanao 0:e7b7def631c2 196 //着地判定
minanao 0:e7b7def631c2 197 while(1)
minanao 0:e7b7def631c2 198 {
minanao 0:e7b7def631c2 199 land_judgement=landing_judgement()
minanao 0:e7b7def631c2 200 if(land_judgement_1==1)
minanao 0:e7b7def631c2 201 {
minanao 0:e7b7def631c2 202 xbee.printf("landing\r\n");
minanao 0:e7b7def631c2 203 break;
minanao 0:e7b7def631c2 204 }
minanao 0:e7b7def631c2 205 }
minanao 0:e7b7def631c2 206 wait(30);
minanao 0:e7b7def631c2 207 //パラシュート切り離し
minanao 0:e7b7def631c2 208 while(1)
minanao 0:e7b7def631c2 209 {
minanao 0:e7b7def631c2 210 sep_judge==1;
minanao 0:e7b7def631c2 211 sep_judge=parachute_separation();
minanao 0:e7b7def631c2 212 if(sep_judge==0)
minanao 0:e7b7def631c2 213 {
minanao 0:e7b7def631c2 214 xbee.printf("did separation\r\n");
minanao 0:e7b7def631c2 215 xbee.printf("separation complleted?\ty/n")
minanao 0:e7b7def631c2 216 int received_data = xbee.getc();
minanao 0:e7b7def631c2 217 if(received_data==89||received_data==121)
minanao 0:e7b7def631c2 218 {
minanao 0:e7b7def631c2 219 xbee.printf("separation complieted\n\r");
minanao 0:e7b7def631c2 220 break;
minanao 0:e7b7def631c2 221 }
minanao 0:e7b7def631c2 222 if(received_data==78||received_data==110)
minanao 0:e7b7def631c2 223 {
minanao 0:e7b7def631c2 224 xbee.printf("try again now\n\r");
minanao 0:e7b7def631c2 225 }
minanao 0:e7b7def631c2 226 }
minanao 0:e7b7def631c2 227 }
minanao 0:e7b7def631c2 228 //中間座標の取得
minanao 0:e7b7def631c2 229 double midpoint_latitude,midpoint_longtitude;
minanao 0:e7b7def631c2 230 xbee.printf("\r\n-----------------------\r\n");
minanao 0:e7b7def631c2 231 xbee.printf("midpoint latitude:");
minanao 0:e7b7def631c2 232 xbee.scanf("%lf",&midpoint_latitude);
minanao 0:e7b7def631c2 233 xbee.printf("\r\nmidpoint longtitude:");
minanao 0:e7b7def631c2 234 xbee.scanf("%lf",&midpoint_longtitude);
minanao 0:e7b7def631c2 235 xbee.printf("\r\n-----------------------\r\n");
minanao 0:e7b7def631c2 236 //倒れているかの判定
minanao 0:e7b7def631c2 237 int i;
minanao 0:e7b7def631c2 238 gps
minanao 0:e7b7def631c2 239 for(i=1;i<=2;i++)
minanao 0:e7b7def631c2 240 {
minanao 0:e7b7def631c2 241 move_forward();
minanao 0:e7b7def631c2 242 }
minanao 0:e7b7def631c2 243
minanao 0:e7b7def631c2 244
minanao 0:e7b7def631c2 245 move_forward();
minanao 0:e7b7def631c2 246
minanao 0:e7b7def631c2 247 //倒れているときの処理
minanao 0:e7b7def631c2 248 wakeup(2)
minanao 0:e7b7def631c2 249
minanao 0:e7b7def631c2 250
minanao 0:e7b7def631c2 251 #
minanao 0:e7b7def631c2 252 }
minanao 0:e7b7def631c2 253
minanao 0:e7b7def631c2 254
minanao 0:e7b7def631c2 255