修正済みby皆川
Dependencies: mbed Servo cansat_integrated_2 BMP180
Dependents: cansat_integrated_2
direction.cpp@8:7209c810309d, 2021-11-01 (annotated)
- Committer:
- tsubasa_nakajima
- Date:
- Mon Nov 01 16:52:17 2021 +0000
- Revision:
- 8:7209c810309d
- Child:
- 9:897f36298db7
completed
Who changed what in which revision?
User | Revision | Line number | New 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 | } |