CansatB_2021 / Mbed 2 deprecated cansat_integrated

統合プログラム

Dependencies:   mbed Servo BMP180

direction.cpp@8:7209c810309d, 2021-11-01 (annotated)

Committer:
tsubasa_nakajima
Date:
Mon Nov 01 16:52:17 2021 +0000
Revision:
8:7209c810309d
completed

Who changed what in which revision?

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