CansatB_2021 / Mbed 2 deprecated cansat_integrated

統合プログラム

Dependencies:   mbed Servo BMP180

direction.h@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 "getGPS.h"
tsubasa_nakajima 1:bb89b58cfa0e 3 #include "Movement.h"
minanao 0:e7b7def631c2 4
minanao 0:e7b7def631c2 5 GPS gps(D1, D0);
minanao 0:e7b7def631c2 6
tsubasa_nakajima 1:bb89b58cfa0e 7 float calculate_theta(float x_0,float y_0,float x_1,float y_1,float x_2,float y_2){
tsubasa_nakajima 1:bb89b58cfa0e 8
tsubasa_nakajima 1:bb89b58cfa0e 9 //float x_0 ,y_0: 目的地 , float x_1 ,y_1: 現在地, float x_2 ,y_2: 20秒前の現在地
tsubasa_nakajima 1:bb89b58cfa0e 10 //theta_0:目的地の角度,theta_1:CanSatの角度theta:CanSatから見た目的地の角度(-180)
tsubasa_nakajima 1:bb89b58cfa0e 11 //theta_0,theta_1は北が90、東が0
tsubasa_nakajima 1:bb89b58cfa0e 12 //theta:CanSatから見た目的地の角度(-180<=theta<=180で、角度は正面が0で反時計回り)
tsubasa_nakajima 1:bb89b58cfa0e 13
tsubasa_nakajima 1:bb89b58cfa0e 14 float theta_0,theta_1;
minanao 0:e7b7def631c2 15
tsubasa_nakajima 1:bb89b58cfa0e 16 if(x_0 == x_1 && x_1 == x_2){
tsubasa_nakajima 1:bb89b58cfa0e 17
tsubasa_nakajima 1:bb89b58cfa0e 18 if(y_0 - y_1 > 0){
tsubasa_nakajima 1:bb89b58cfa0e 19 theta_0 = 90;
tsubasa_nakajima 1:bb89b58cfa0e 20 }
tsubasa_nakajima 1:bb89b58cfa0e 21 if(y_0 - y_1 < 0){
tsubasa_nakajima 1:bb89b58cfa0e 22 theta_0 = 270;
tsubasa_nakajima 1:bb89b58cfa0e 23 }
tsubasa_nakajima 1:bb89b58cfa0e 24 if(y_0 - y_1 == 0){
tsubasa_nakajima 1:bb89b58cfa0e 25 theta_0 = 0;
tsubasa_nakajima 1:bb89b58cfa0e 26 }
tsubasa_nakajima 1:bb89b58cfa0e 27
tsubasa_nakajima 1:bb89b58cfa0e 28 if(y_1 - y_2 > 0){
tsubasa_nakajima 1:bb89b58cfa0e 29 theta_1 = 90;
tsubasa_nakajima 1:bb89b58cfa0e 30 }
tsubasa_nakajima 1:bb89b58cfa0e 31 if(y_1 - y_2 < 0){
tsubasa_nakajima 1:bb89b58cfa0e 32 theta_1 = 270;
tsubasa_nakajima 1:bb89b58cfa0e 33 }
tsubasa_nakajima 1:bb89b58cfa0e 34 if(y_1 - y_2 == 0){
tsubasa_nakajima 1:bb89b58cfa0e 35 theta_1 = 0;
tsubasa_nakajima 1:bb89b58cfa0e 36 }
minanao 0:e7b7def631c2 37 }
tsubasa_nakajima 1:bb89b58cfa0e 38
tsubasa_nakajima 1:bb89b58cfa0e 39 if(x_0 == x_1 && x_1 != x_2){
tsubasa_nakajima 1:bb89b58cfa0e 40 theta_1 = atan((y_1 - y_2)/(x_1- x_2));
tsubasa_nakajima 1:bb89b58cfa0e 41
tsubasa_nakajima 1:bb89b58cfa0e 42 if(y_0 - y_1 > 0){
tsubasa_nakajima 1:bb89b58cfa0e 43 theta_0 = 90;
tsubasa_nakajima 1:bb89b58cfa0e 44 }
tsubasa_nakajima 1:bb89b58cfa0e 45
tsubasa_nakajima 1:bb89b58cfa0e 46 if(y_0 - y_1 < 0){
tsubasa_nakajima 1:bb89b58cfa0e 47 theta_0 = 270;
tsubasa_nakajima 1:bb89b58cfa0e 48 }
tsubasa_nakajima 1:bb89b58cfa0e 49
tsubasa_nakajima 1:bb89b58cfa0e 50 if(y_0 - y_1 == 0){
tsubasa_nakajima 1:bb89b58cfa0e 51 theta_0 = 0;
tsubasa_nakajima 1:bb89b58cfa0e 52 }
tsubasa_nakajima 1:bb89b58cfa0e 53
tsubasa_nakajima 1:bb89b58cfa0e 54 if(y_1 - y_2 > 0 && x_1 - x_2 < 0){
tsubasa_nakajima 1:bb89b58cfa0e 55 theta_1 = theta_1 - 180;
tsubasa_nakajima 1:bb89b58cfa0e 56 }
tsubasa_nakajima 1:bb89b58cfa0e 57 if(y_1 - y_2 == 0 && x_1 - x_2 < 0){
tsubasa_nakajima 1:bb89b58cfa0e 58 theta_1 = 180;
tsubasa_nakajima 1:bb89b58cfa0e 59 }
tsubasa_nakajima 1:bb89b58cfa0e 60 if(y_1 - y_2 < 0 && x_1 - x_2 > 0){
tsubasa_nakajima 1:bb89b58cfa0e 61 theta_1 = theta_1 + 180;
tsubasa_nakajima 1:bb89b58cfa0e 62 }
minanao 0:e7b7def631c2 63 }
minanao 0:e7b7def631c2 64
tsubasa_nakajima 1:bb89b58cfa0e 65 if(x_0 != x_1 && x_1 == x_2){
tsubasa_nakajima 1:bb89b58cfa0e 66
tsubasa_nakajima 1:bb89b58cfa0e 67 theta_0 = atan((y_0 - y_1)/(x_0 - x_1));
minanao 0:e7b7def631c2 68
tsubasa_nakajima 1:bb89b58cfa0e 69 if(y_0 - y_1 > 0 && x_0 - x_1 < 0){
tsubasa_nakajima 1:bb89b58cfa0e 70 theta_0 = theta_0 - 180;
tsubasa_nakajima 1:bb89b58cfa0e 71 }
tsubasa_nakajima 1:bb89b58cfa0e 72 if(y_0 - y_1 == 0 && x_0 - x_1 < 0){
tsubasa_nakajima 1:bb89b58cfa0e 73 theta_0 = 180;
tsubasa_nakajima 1:bb89b58cfa0e 74 }
tsubasa_nakajima 1:bb89b58cfa0e 75 if(y_0 - y_1 < 0 && x_0 - x_1 > 0){
tsubasa_nakajima 1:bb89b58cfa0e 76 theta_0 = theta_0 + 180;
tsubasa_nakajima 1:bb89b58cfa0e 77 }
tsubasa_nakajima 1:bb89b58cfa0e 78
tsubasa_nakajima 1:bb89b58cfa0e 79 if(y_1 - y_2 > 0){
tsubasa_nakajima 1:bb89b58cfa0e 80 theta_1 = 90;
tsubasa_nakajima 1:bb89b58cfa0e 81 }
tsubasa_nakajima 1:bb89b58cfa0e 82 if(y_1 - y_2 < 0){
tsubasa_nakajima 1:bb89b58cfa0e 83 theta_1 = 270;
tsubasa_nakajima 1:bb89b58cfa0e 84 }
tsubasa_nakajima 1:bb89b58cfa0e 85 if(y_1 - y_2 == 0){
tsubasa_nakajima 1:bb89b58cfa0e 86 theta_1 = 0;
tsubasa_nakajima 1:bb89b58cfa0e 87 }
minanao 0:e7b7def631c2 88 }
minanao 0:e7b7def631c2 89
tsubasa_nakajima 1:bb89b58cfa0e 90 else{
tsubasa_nakajima 1:bb89b58cfa0e 91 theta_0 = atan((y_0 - y_1)/(x_0 - x_1));
tsubasa_nakajima 1:bb89b58cfa0e 92 theta_1 = atan((y_1 - y_2)/(x_1- x_2));
minanao 0:e7b7def631c2 93
tsubasa_nakajima 1:bb89b58cfa0e 94 if(y_0 - y_1 > 0 && x_0 - x_1 < 0){
tsubasa_nakajima 1:bb89b58cfa0e 95 theta_0 = theta_0 - 180;
tsubasa_nakajima 1:bb89b58cfa0e 96 }
tsubasa_nakajima 1:bb89b58cfa0e 97 if(y_0 - y_1 == 0 && x_0 - x_1 < 0){
tsubasa_nakajima 1:bb89b58cfa0e 98 theta_0 = 180;
tsubasa_nakajima 1:bb89b58cfa0e 99 }
tsubasa_nakajima 1:bb89b58cfa0e 100 if(y_0 - y_1 < 0 && x_0 - x_1 > 0){
tsubasa_nakajima 1:bb89b58cfa0e 101 theta_0 = theta_0 + 180;
tsubasa_nakajima 1:bb89b58cfa0e 102 }
minanao 0:e7b7def631c2 103
tsubasa_nakajima 1:bb89b58cfa0e 104 if(y_1 - y_2 > 0 && x_1 - x_2 < 0){
tsubasa_nakajima 1:bb89b58cfa0e 105 theta_1 = theta_1 - 180;
tsubasa_nakajima 1:bb89b58cfa0e 106 }
tsubasa_nakajima 1:bb89b58cfa0e 107 if(y_1 - y_2 == 0 && x_1 - x_2 < 0){
tsubasa_nakajima 1:bb89b58cfa0e 108 theta_1 = 180;
tsubasa_nakajima 1:bb89b58cfa0e 109 }
tsubasa_nakajima 1:bb89b58cfa0e 110 if(y_1 - y_2 < 0 && x_1- x_2 > 0){
tsubasa_nakajima 1:bb89b58cfa0e 111 theta_1 = theta_1 + 180;
tsubasa_nakajima 1:bb89b58cfa0e 112 }
tsubasa_nakajima 1:bb89b58cfa0e 113
tsubasa_nakajima 1:bb89b58cfa0e 114
minanao 0:e7b7def631c2 115 }
minanao 0:e7b7def631c2 116
tsubasa_nakajima 1:bb89b58cfa0e 117 float theta = theta_0 - theta_1;
tsubasa_nakajima 1:bb89b58cfa0e 118 if(theta < -180){
tsubasa_nakajima 1:bb89b58cfa0e 119 theta = 360 - theta;
tsubasa_nakajima 1:bb89b58cfa0e 120 }
tsubasa_nakajima 1:bb89b58cfa0e 121 if(theta > 180){
tsubasa_nakajima 1:bb89b58cfa0e 122 theta = -360 + theta;
tsubasa_nakajima 1:bb89b58cfa0e 123 }
tsubasa_nakajima 1:bb89b58cfa0e 124
tsubasa_nakajima 1:bb89b58cfa0e 125 return theta;
tsubasa_nakajima 1:bb89b58cfa0e 126 }
tsubasa_nakajima 1:bb89b58cfa0e 127
tsubasa_nakajima 1:bb89b58cfa0e 128 // 球面三角法により、大円距離(メートル)を求める
tsubasa_nakajima 1:bb89b58cfa0e 129 double distance(double lat1, double lng1, double lat2, double lng2) {
tsubasa_nakajima 1:bb89b58cfa0e 130
tsubasa_nakajima 1:bb89b58cfa0e 131 // 円周率
tsubasa_nakajima 1:bb89b58cfa0e 132 const double pi = 3.14159265359;
tsubasa_nakajima 1:bb89b58cfa0e 133
tsubasa_nakajima 1:bb89b58cfa0e 134 // 緯度経度をラジアンに変換
tsubasa_nakajima 1:bb89b58cfa0e 135 double rlat1 = lat1 * pi / 180;
tsubasa_nakajima 1:bb89b58cfa0e 136 double rlng1 = lng1 * pi / 180;
tsubasa_nakajima 1:bb89b58cfa0e 137 double rlat2 = lat2 * pi / 180;
tsubasa_nakajima 1:bb89b58cfa0e 138 double rlng2 = lng2 * pi / 180;
tsubasa_nakajima 1:bb89b58cfa0e 139
tsubasa_nakajima 1:bb89b58cfa0e 140 // 2点の中心角(ラジアン)を求める
tsubasa_nakajima 1:bb89b58cfa0e 141 double a =
tsubasa_nakajima 1:bb89b58cfa0e 142 sin(rlat1) * sin(rlat2) +
tsubasa_nakajima 1:bb89b58cfa0e 143 cos(rlat1) * cos(rlat2) *
tsubasa_nakajima 1:bb89b58cfa0e 144 cos(rlng1 - rlng2);
tsubasa_nakajima 1:bb89b58cfa0e 145 double rr = acos(a);
tsubasa_nakajima 1:bb89b58cfa0e 146
tsubasa_nakajima 1:bb89b58cfa0e 147 // 地球赤道半径(メートル)
tsubasa_nakajima 1:bb89b58cfa0e 148 const double earth_radius = 6378140;
tsubasa_nakajima 1:bb89b58cfa0e 149
tsubasa_nakajima 1:bb89b58cfa0e 150 // 2点間の距離(メートル)
tsubasa_nakajima 1:bb89b58cfa0e 151 double distance = earth_radius * rr;
tsubasa_nakajima 1:bb89b58cfa0e 152
tsubasa_nakajima 1:bb89b58cfa0e 153 return distance;
tsubasa_nakajima 1:bb89b58cfa0e 154 }
tsubasa_nakajima 1:bb89b58cfa0e 155
tsubasa_nakajima 1:bb89b58cfa0e 156 class direction
tsubasa_nakajima 1:bb89b58cfa0e 157 {
tsubasa_nakajima 1:bb89b58cfa0e 158 private:
tsubasa_nakajima 1:bb89b58cfa0e 159
tsubasa_nakajima 1:bb89b58cfa0e 160 int s;
tsubasa_nakajima 1:bb89b58cfa0e 161 float x_0 ,y_0; //中間地点の座標(未定)
tsubasa_nakajima 1:bb89b58cfa0e 162 float x_01,y_01; //ゴール地点の座標(未定)
tsubasa_nakajima 1:bb89b58cfa0e 163 float x_1 ,y_1; //現在地
tsubasa_nakajima 1:bb89b58cfa0e 164 float x_2 ,y_2; //20秒前の現在地
tsubasa_nakajima 1:bb89b58cfa0e 165 float theta; //CanSatから見た目的地の角度
tsubasa_nakajima 1:bb89b58cfa0e 166 float d,d1,d2; //dはCanSatの20秒間の移動距離,d1はCanSatと中間地点の距離、d2はゴール地点との距離(単位はm)
tsubasa_nakajima 1:bb89b58cfa0e 167
tsubasa_nakajima 1:bb89b58cfa0e 168 public:
tsubasa_nakajima 1:bb89b58cfa0e 169
tsubasa_nakajima 1:bb89b58cfa0e 170 //歩行
tsubasa_nakajima 1:bb89b58cfa0e 171 int walk();
tsubasa_nakajima 1:bb89b58cfa0e 172 };
tsubasa_nakajima 1:bb89b58cfa0e 173
tsubasa_nakajima 1:bb89b58cfa0e 174 int direction::walk(){
tsubasa_nakajima 1:bb89b58cfa0e 175 s = 0;
tsubasa_nakajima 1:bb89b58cfa0e 176 x_0 = NULL;
tsubasa_nakajima 1:bb89b58cfa0e 177 y_0 = NULL;
tsubasa_nakajima 1:bb89b58cfa0e 178 x_01 = NULL;
tsubasa_nakajima 1:bb89b58cfa0e 179 y_01 = NULL;
tsubasa_nakajima 1:bb89b58cfa0e 180 while(s<1){
tsubasa_nakajima 1:bb89b58cfa0e 181 if(gps.getgps()){
tsubasa_nakajima 1:bb89b58cfa0e 182 x_1 = gps.longitude;
tsubasa_nakajima 1:bb89b58cfa0e 183 y_1 = gps.latitude;
tsubasa_nakajima 1:bb89b58cfa0e 184 s = s + 1;
tsubasa_nakajima 1:bb89b58cfa0e 185 Movement.move_forward();
tsubasa_nakajima 1:bb89b58cfa0e 186 }
tsubasa_nakajima 1:bb89b58cfa0e 187 }
tsubasa_nakajima 1:bb89b58cfa0e 188
tsubasa_nakajima 1:bb89b58cfa0e 189 while(1) {
minanao 0:e7b7def631c2 190
tsubasa_nakajima 1:bb89b58cfa0e 191 if(gps.getgps()){ //現在地取得
tsubasa_nakajima 1:bb89b58cfa0e 192 x_2 = x_1;
tsubasa_nakajima 1:bb89b58cfa0e 193 y_2 = y_2; //20(+回転or復帰時間)秒前に居た地点の緯度と経度を取得
tsubasa_nakajima 1:bb89b58cfa0e 194 x_1 = gps.longitude;
tsubasa_nakajima 1:bb89b58cfa0e 195 y_1 = gps.latitude; //現在地の緯度と経度を取得
tsubasa_nakajima 1:bb89b58cfa0e 196
tsubasa_nakajima 1:bb89b58cfa0e 197 //中間地点に到達したらbreak
tsubasa_nakajima 1:bb89b58cfa0e 198 d1 = distance(y_1,x_1,y_0,x_0);
tsubasa_nakajima 1:bb89b58cfa0e 199 if(d1 < 15){
tsubasa_nakajima 1:bb89b58cfa0e 200 Movement.stop();
tsubasa_nakajima 1:bb89b58cfa0e 201 break;
tsubasa_nakajima 1:bb89b58cfa0e 202 }
tsubasa_nakajima 1:bb89b58cfa0e 203
tsubasa_nakajima 1:bb89b58cfa0e 204 //移動距離が短ければ横転と判定し復帰
tsubasa_nakajima 1:bb89b58cfa0e 205 d = distance(y_1,x_1,y_2,x_2);
tsubasa_nakajima 1:bb89b58cfa0e 206 if(d < 3){
tsubasa_nakajima 1:bb89b58cfa0e 207 Movement.wakeup(2);
tsubasa_nakajima 1:bb89b58cfa0e 208 Movement.stop();
tsubasa_nakajima 1:bb89b58cfa0e 209 }
tsubasa_nakajima 1:bb89b58cfa0e 210
tsubasa_nakajima 1:bb89b58cfa0e 211 //Θによって回転か直進か決定
tsubasa_nakajima 1:bb89b58cfa0e 212 theta = calculate_theta(float x_0,float y_0,float x_1,float y_1,float x_2,float y_2);
tsubasa_nakajima 1:bb89b58cfa0e 213 if(-30 < theta && theta < 30){
tsubasa_nakajima 1:bb89b58cfa0e 214 Movement.move_forward();
tsubasa_nakajima 1:bb89b58cfa0e 215 }
tsubasa_nakajima 1:bb89b58cfa0e 216 if(theta >=30){
tsubasa_nakajima 1:bb89b58cfa0e 217 Movement.turn_right(int theta);
tsubasa_nakajima 1:bb89b58cfa0e 218 Movement.stop();
tsubasa_nakajima 1:bb89b58cfa0e 219 Movement.move_forward();
tsubasa_nakajima 1:bb89b58cfa0e 220 }
tsubasa_nakajima 1:bb89b58cfa0e 221 if(theta　<= -30){
tsubasa_nakajima 1:bb89b58cfa0e 222 Movement.turn_left(int theta);
tsubasa_nakajima 1:bb89b58cfa0e 223 Movement.stop();
tsubasa_nakajima 1:bb89b58cfa0e 224 Movement.move_forward();
tsubasa_nakajima 1:bb89b58cfa0e 225 }
tsubasa_nakajima 1:bb89b58cfa0e 226
tsubasa_nakajima 1:bb89b58cfa0e 227 }
tsubasa_nakajima 1:bb89b58cfa0e 228
minanao 0:e7b7def631c2 229
tsubasa_nakajima 1:bb89b58cfa0e 230 }
tsubasa_nakajima 1:bb89b58cfa0e 231
tsubasa_nakajima 1:bb89b58cfa0e 232 　　　 while(1) {
tsubasa_nakajima 1:bb89b58cfa0e 233
tsubasa_nakajima 1:bb89b58cfa0e 234 if(gps.getgps()){ //現在地取得
tsubasa_nakajima 1:bb89b58cfa0e 235 x_2 = x_1;
tsubasa_nakajima 1:bb89b58cfa0e 236 y_2 = y_2; //20(+回転or復帰時間)秒前に居た地点の緯度と経度を取得
tsubasa_nakajima 1:bb89b58cfa0e 237 x_1 = gps.longitude;
tsubasa_nakajima 1:bb89b58cfa0e 238 y_1 = gps.latitude; //現在地の緯度と経度を取得
tsubasa_nakajima 1:bb89b58cfa0e 239
tsubasa_nakajima 1:bb89b58cfa0e 240 //ゴール地点に到達したらbreak
tsubasa_nakajima 1:bb89b58cfa0e 241 d2 = distance(y_1,x_1,y_01,x_01);
tsubasa_nakajima 1:bb89b58cfa0e 242 if(d2 < 15){
tsubasa_nakajima 1:bb89b58cfa0e 243 Movement.stop();
tsubasa_nakajima 1:bb89b58cfa0e 244 break;
tsubasa_nakajima 1:bb89b58cfa0e 245 }
tsubasa_nakajima 1:bb89b58cfa0e 246
tsubasa_nakajima 1:bb89b58cfa0e 247 //移動距離が短ければ横転と判定し復帰
tsubasa_nakajima 1:bb89b58cfa0e 248 d = distance(y_1,x_1,y_2,x_2);
tsubasa_nakajima 1:bb89b58cfa0e 249 if(d < 3){
tsubasa_nakajima 1:bb89b58cfa0e 250 Movement.wakeup(2);
tsubasa_nakajima 1:bb89b58cfa0e 251 Movement.stop();
tsubasa_nakajima 1:bb89b58cfa0e 252 }
tsubasa_nakajima 1:bb89b58cfa0e 253
tsubasa_nakajima 1:bb89b58cfa0e 254 //thetaによって回転か直進か決定
tsubasa_nakajima 1:bb89b58cfa0e 255 theta = calculate_theta(float x_0,float y_0,float x_1,float y_1,float x_2,float y_2);
tsubasa_nakajima 1:bb89b58cfa0e 256 if(-30 < theta && theta < 30){
tsubasa_nakajima 1:bb89b58cfa0e 257 Movement.move_forward();
tsubasa_nakajima 1:bb89b58cfa0e 258 }
tsubasa_nakajima 1:bb89b58cfa0e 259 if(theta >=30){
tsubasa_nakajima 1:bb89b58cfa0e 260 Movement.turn_right(int theta);
tsubasa_nakajima 1:bb89b58cfa0e 261 Movement.move_forward();
tsubasa_nakajima 1:bb89b58cfa0e 262 }
tsubasa_nakajima 1:bb89b58cfa0e 263 if(theta　<= -30){
tsubasa_nakajima 1:bb89b58cfa0e 264 Movement.turn_left(int theta);
tsubasa_nakajima 1:bb89b58cfa0e 265 Movement.move_forward();
tsubasa_nakajima 1:bb89b58cfa0e 266 }
tsubasa_nakajima 1:bb89b58cfa0e 267
tsubasa_nakajima 1:bb89b58cfa0e 268 }
tsubasa_nakajima 1:bb89b58cfa0e 269
tsubasa_nakajima 1:bb89b58cfa0e 270
minanao 0:e7b7def631c2 271 }
minanao 0:e7b7def631c2 272
tsubasa_nakajima 1:bb89b58cfa0e 273 Movement.stop();
minanao 0:e7b7def631c2 274
tsubasa_nakajima 1:bb89b58cfa0e 275 return 0;
tsubasa_nakajima 1:bb89b58cfa0e 276 }