修正済みby皆川
Dependencies: mbed Servo cansat_integrated_2 BMP180
Dependents: cansat_integrated_2
direction.h@3:a583276d9fef, 2021-10-28 (annotated)
- Committer:
- tsubasa_nakajima
- Date:
- Thu Oct 28 07:57:02 2021 +0000
- Revision:
- 3:a583276d9fef
- Parent:
- 2:d2cb6b50a8c4
- Child:
- 5:e1001bfc423a
fixed error
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
minanao | 0:e7b7def631c2 | 1 | #include "mbed.h" |
minanao | 0:e7b7def631c2 | 2 | #include "getGPS.h" |
tsubasa_nakajima | 2:d2cb6b50a8c4 | 3 | #include "Servo.h" |
tsubasa_nakajima | 3:a583276d9fef | 4 | |
tsubasa_nakajima | 3:a583276d9fef | 5 | // 球面三角法により、大円距離(メートル)を求める |
tsubasa_nakajima | 3:a583276d9fef | 6 | double distance(double lat1, double lng1, double lat2, double lng2){ |
tsubasa_nakajima | 3:a583276d9fef | 7 | // 円周率 |
tsubasa_nakajima | 3:a583276d9fef | 8 | const double pi = 3.14159265359; |
tsubasa_nakajima | 3:a583276d9fef | 9 | |
tsubasa_nakajima | 3:a583276d9fef | 10 | // 緯度経度をラジアンに変換 |
tsubasa_nakajima | 3:a583276d9fef | 11 | double rlat1 = lat1 * pi / 180; |
tsubasa_nakajima | 3:a583276d9fef | 12 | double rlng1 = lng1 * pi / 180; |
tsubasa_nakajima | 3:a583276d9fef | 13 | double rlat2 = lat2 * pi / 180; |
tsubasa_nakajima | 3:a583276d9fef | 14 | double rlng2 = lng2 * pi / 180; |
tsubasa_nakajima | 3:a583276d9fef | 15 | |
tsubasa_nakajima | 3:a583276d9fef | 16 | // 2点の中心角(ラジアン)を求める |
tsubasa_nakajima | 3:a583276d9fef | 17 | double a = |
tsubasa_nakajima | 3:a583276d9fef | 18 | sin(rlat1) * sin(rlat2) + |
tsubasa_nakajima | 3:a583276d9fef | 19 | cos(rlat1) * cos(rlat2) * |
tsubasa_nakajima | 3:a583276d9fef | 20 | cos(rlng1 - rlng2); |
tsubasa_nakajima | 3:a583276d9fef | 21 | double rr = acos(a); |
tsubasa_nakajima | 3:a583276d9fef | 22 | |
tsubasa_nakajima | 3:a583276d9fef | 23 | // 地球赤道半径(メートル) |
tsubasa_nakajima | 3:a583276d9fef | 24 | const double earth_radius = 6378140; |
tsubasa_nakajima | 3:a583276d9fef | 25 | |
tsubasa_nakajima | 3:a583276d9fef | 26 | // 2点間の距離(メートル) |
tsubasa_nakajima | 3:a583276d9fef | 27 | double distance = earth_radius * rr; |
tsubasa_nakajima | 3:a583276d9fef | 28 | |
tsubasa_nakajima | 3:a583276d9fef | 29 | return distance; |
tsubasa_nakajima | 3:a583276d9fef | 30 | } |
minanao | 0:e7b7def631c2 | 31 | |
tsubasa_nakajima | 3:a583276d9fef | 32 | float calculate_theta(float x_0,float y_0,float x_1,float y_1,float x_2,float y_2){ |
tsubasa_nakajima | 3:a583276d9fef | 33 | |
tsubasa_nakajima | 3:a583276d9fef | 34 | //float x_0 ,y_0: 目的地 , float x_1 ,y_1: 現在地, float x_2 ,y_2: 20秒前の現在地 |
tsubasa_nakajima | 3:a583276d9fef | 35 | //theta_0:目的地の角度,theta_1:CanSatの角度theta:CanSatから見た目的地の角度(-180) |
tsubasa_nakajima | 3:a583276d9fef | 36 | //theta_0,theta_1は北が90、東が0 |
tsubasa_nakajima | 3:a583276d9fef | 37 | //theta:CanSatから見た目的地の角度(-180<=theta<=180で、角度は正面が0で反時計回り) |
tsubasa_nakajima | 3:a583276d9fef | 38 | |
tsubasa_nakajima | 3:a583276d9fef | 39 | float theta_0,theta_1; |
tsubasa_nakajima | 3:a583276d9fef | 40 | |
tsubasa_nakajima | 3:a583276d9fef | 41 | if(x_0 == x_1 && x_1 == x_2){ |
tsubasa_nakajima | 3:a583276d9fef | 42 | |
tsubasa_nakajima | 3:a583276d9fef | 43 | if(y_0 - y_1 > 0){ |
tsubasa_nakajima | 3:a583276d9fef | 44 | theta_0 = 90; |
tsubasa_nakajima | 3:a583276d9fef | 45 | } |
tsubasa_nakajima | 3:a583276d9fef | 46 | if(y_0 - y_1 < 0){ |
tsubasa_nakajima | 3:a583276d9fef | 47 | theta_0 = 270; |
tsubasa_nakajima | 3:a583276d9fef | 48 | } |
tsubasa_nakajima | 3:a583276d9fef | 49 | if(y_0 - y_1 == 0){ |
tsubasa_nakajima | 3:a583276d9fef | 50 | theta_0 = 0; |
tsubasa_nakajima | 3:a583276d9fef | 51 | } |
tsubasa_nakajima | 3:a583276d9fef | 52 | |
tsubasa_nakajima | 3:a583276d9fef | 53 | if(y_1 - y_2 > 0){ |
tsubasa_nakajima | 3:a583276d9fef | 54 | theta_1 = 90; |
tsubasa_nakajima | 3:a583276d9fef | 55 | } |
tsubasa_nakajima | 3:a583276d9fef | 56 | if(y_1 - y_2 < 0){ |
tsubasa_nakajima | 3:a583276d9fef | 57 | theta_1 = 270; |
tsubasa_nakajima | 3:a583276d9fef | 58 | } |
tsubasa_nakajima | 3:a583276d9fef | 59 | if(y_1 - y_2 == 0){ |
tsubasa_nakajima | 3:a583276d9fef | 60 | theta_1 = 0; |
tsubasa_nakajima | 3:a583276d9fef | 61 | } |
tsubasa_nakajima | 3:a583276d9fef | 62 | } |
tsubasa_nakajima | 3:a583276d9fef | 63 | |
tsubasa_nakajima | 3:a583276d9fef | 64 | if(x_0 == x_1 && x_1 != x_2){ |
tsubasa_nakajima | 3:a583276d9fef | 65 | theta_1 = atan((y_1 - y_2)/(x_1- x_2)); |
tsubasa_nakajima | 3:a583276d9fef | 66 | |
tsubasa_nakajima | 3:a583276d9fef | 67 | if(y_0 - y_1 > 0){ |
tsubasa_nakajima | 3:a583276d9fef | 68 | theta_0 = 90; |
tsubasa_nakajima | 3:a583276d9fef | 69 | } |
tsubasa_nakajima | 3:a583276d9fef | 70 | |
tsubasa_nakajima | 3:a583276d9fef | 71 | if(y_0 - y_1 < 0){ |
tsubasa_nakajima | 3:a583276d9fef | 72 | theta_0 = 270; |
tsubasa_nakajima | 3:a583276d9fef | 73 | } |
tsubasa_nakajima | 3:a583276d9fef | 74 | |
tsubasa_nakajima | 3:a583276d9fef | 75 | if(y_0 - y_1 == 0){ |
tsubasa_nakajima | 3:a583276d9fef | 76 | theta_0 = 0; |
tsubasa_nakajima | 3:a583276d9fef | 77 | } |
tsubasa_nakajima | 3:a583276d9fef | 78 | |
tsubasa_nakajima | 3:a583276d9fef | 79 | if(y_1 - y_2 > 0 && x_1 - x_2 < 0){ |
tsubasa_nakajima | 3:a583276d9fef | 80 | theta_1 = theta_1 - 180; |
tsubasa_nakajima | 3:a583276d9fef | 81 | } |
tsubasa_nakajima | 3:a583276d9fef | 82 | if(y_1 - y_2 == 0 && x_1 - x_2 < 0){ |
tsubasa_nakajima | 3:a583276d9fef | 83 | theta_1 = 180; |
tsubasa_nakajima | 3:a583276d9fef | 84 | } |
tsubasa_nakajima | 3:a583276d9fef | 85 | if(y_1 - y_2 < 0 && x_1 - x_2 > 0){ |
tsubasa_nakajima | 3:a583276d9fef | 86 | theta_1 = theta_1 + 180; |
tsubasa_nakajima | 3:a583276d9fef | 87 | } |
tsubasa_nakajima | 3:a583276d9fef | 88 | } |
tsubasa_nakajima | 3:a583276d9fef | 89 | |
tsubasa_nakajima | 3:a583276d9fef | 90 | if(x_0 != x_1 && x_1 == x_2){ |
tsubasa_nakajima | 3:a583276d9fef | 91 | |
tsubasa_nakajima | 3:a583276d9fef | 92 | theta_0 = atan((y_0 - y_1)/(x_0 - x_1)); |
tsubasa_nakajima | 3:a583276d9fef | 93 | |
tsubasa_nakajima | 3:a583276d9fef | 94 | if(y_0 - y_1 > 0 && x_0 - x_1 < 0){ |
tsubasa_nakajima | 3:a583276d9fef | 95 | theta_0 = theta_0 - 180; |
tsubasa_nakajima | 3:a583276d9fef | 96 | } |
tsubasa_nakajima | 3:a583276d9fef | 97 | if(y_0 - y_1 == 0 && x_0 - x_1 < 0){ |
tsubasa_nakajima | 3:a583276d9fef | 98 | theta_0 = 180; |
tsubasa_nakajima | 3:a583276d9fef | 99 | } |
tsubasa_nakajima | 3:a583276d9fef | 100 | if(y_0 - y_1 < 0 && x_0 - x_1 > 0){ |
tsubasa_nakajima | 3:a583276d9fef | 101 | theta_0 = theta_0 + 180; |
tsubasa_nakajima | 3:a583276d9fef | 102 | } |
tsubasa_nakajima | 3:a583276d9fef | 103 | |
tsubasa_nakajima | 3:a583276d9fef | 104 | if(y_1 - y_2 > 0){ |
tsubasa_nakajima | 3:a583276d9fef | 105 | theta_1 = 90; |
tsubasa_nakajima | 3:a583276d9fef | 106 | } |
tsubasa_nakajima | 3:a583276d9fef | 107 | if(y_1 - y_2 < 0){ |
tsubasa_nakajima | 3:a583276d9fef | 108 | theta_1 = 270; |
tsubasa_nakajima | 3:a583276d9fef | 109 | } |
tsubasa_nakajima | 3:a583276d9fef | 110 | if(y_1 - y_2 == 0){ |
tsubasa_nakajima | 3:a583276d9fef | 111 | theta_1 = 0; |
tsubasa_nakajima | 3:a583276d9fef | 112 | } |
tsubasa_nakajima | 3:a583276d9fef | 113 | } |
tsubasa_nakajima | 3:a583276d9fef | 114 | |
tsubasa_nakajima | 3:a583276d9fef | 115 | else{ |
tsubasa_nakajima | 3:a583276d9fef | 116 | theta_0 = atan((y_0 - y_1)/(x_0 - x_1)); |
tsubasa_nakajima | 3:a583276d9fef | 117 | theta_1 = atan((y_1 - y_2)/(x_1- x_2)); |
tsubasa_nakajima | 3:a583276d9fef | 118 | |
tsubasa_nakajima | 3:a583276d9fef | 119 | if(y_0 - y_1 > 0 && x_0 - x_1 < 0){ |
tsubasa_nakajima | 3:a583276d9fef | 120 | theta_0 = theta_0 - 180; |
tsubasa_nakajima | 3:a583276d9fef | 121 | } |
tsubasa_nakajima | 3:a583276d9fef | 122 | if(y_0 - y_1 == 0 && x_0 - x_1 < 0){ |
tsubasa_nakajima | 3:a583276d9fef | 123 | theta_0 = 180; |
tsubasa_nakajima | 3:a583276d9fef | 124 | } |
tsubasa_nakajima | 3:a583276d9fef | 125 | if(y_0 - y_1 < 0 && x_0 - x_1 > 0){ |
tsubasa_nakajima | 3:a583276d9fef | 126 | theta_0 = theta_0 + 180; |
tsubasa_nakajima | 3:a583276d9fef | 127 | } |
tsubasa_nakajima | 3:a583276d9fef | 128 | |
tsubasa_nakajima | 3:a583276d9fef | 129 | if(y_1 - y_2 > 0 && x_1 - x_2 < 0){ |
tsubasa_nakajima | 3:a583276d9fef | 130 | theta_1 = theta_1 - 180; |
tsubasa_nakajima | 3:a583276d9fef | 131 | } |
tsubasa_nakajima | 3:a583276d9fef | 132 | if(y_1 - y_2 == 0 && x_1 - x_2 < 0){ |
tsubasa_nakajima | 3:a583276d9fef | 133 | theta_1 = 180; |
tsubasa_nakajima | 3:a583276d9fef | 134 | } |
tsubasa_nakajima | 3:a583276d9fef | 135 | if(y_1 - y_2 < 0 && x_1- x_2 > 0){ |
tsubasa_nakajima | 3:a583276d9fef | 136 | theta_1 = theta_1 + 180; |
tsubasa_nakajima | 3:a583276d9fef | 137 | } |
tsubasa_nakajima | 3:a583276d9fef | 138 | |
tsubasa_nakajima | 3:a583276d9fef | 139 | |
tsubasa_nakajima | 3:a583276d9fef | 140 | } |
tsubasa_nakajima | 3:a583276d9fef | 141 | |
tsubasa_nakajima | 3:a583276d9fef | 142 | float theta = theta_0 - theta_1; |
tsubasa_nakajima | 3:a583276d9fef | 143 | if(theta < -180){ |
tsubasa_nakajima | 3:a583276d9fef | 144 | theta = 360 - theta; |
tsubasa_nakajima | 3:a583276d9fef | 145 | } |
tsubasa_nakajima | 3:a583276d9fef | 146 | if(theta > 180){ |
tsubasa_nakajima | 3:a583276d9fef | 147 | theta = -360 + theta; |
tsubasa_nakajima | 3:a583276d9fef | 148 | } |
tsubasa_nakajima | 3:a583276d9fef | 149 | |
tsubasa_nakajima | 3:a583276d9fef | 150 | return theta; |
tsubasa_nakajima | 3:a583276d9fef | 151 | } |
tsubasa_nakajima | 3:a583276d9fef | 152 | |
tsubasa_nakajima | 2:d2cb6b50a8c4 | 153 | //停止 |
tsubasa_nakajima | 2:d2cb6b50a8c4 | 154 | void stop(){ |
tsubasa_nakajima | 2:d2cb6b50a8c4 | 155 | Servo servo1(D7); |
tsubasa_nakajima | 2:d2cb6b50a8c4 | 156 | Servo servo2(A3); |
tsubasa_nakajima | 2:d2cb6b50a8c4 | 157 | Servo servo3(A1); |
tsubasa_nakajima | 2:d2cb6b50a8c4 | 158 | Servo servo4(D12); |
tsubasa_nakajima | 2:d2cb6b50a8c4 | 159 | Servo servo5(D10); |
tsubasa_nakajima | 2:d2cb6b50a8c4 | 160 | Servo servo6(A5); |
minanao | 0:e7b7def631c2 | 161 | |
tsubasa_nakajima | 2:d2cb6b50a8c4 | 162 | servo1 = 0.5; |
tsubasa_nakajima | 2:d2cb6b50a8c4 | 163 | servo2 = 0.5; |
tsubasa_nakajima | 2:d2cb6b50a8c4 | 164 | servo3 = 0.5; |
tsubasa_nakajima | 2:d2cb6b50a8c4 | 165 | servo4 = 0.5; |
tsubasa_nakajima | 2:d2cb6b50a8c4 | 166 | servo5 = 0.5; |
tsubasa_nakajima | 2:d2cb6b50a8c4 | 167 | servo6 = 0.5; |
tsubasa_nakajima | 2:d2cb6b50a8c4 | 168 | wait(1); |
minanao | 0:e7b7def631c2 | 169 | } |
tsubasa_nakajima | 2:d2cb6b50a8c4 | 170 | |
tsubasa_nakajima | 2:d2cb6b50a8c4 | 171 | //前進 |
tsubasa_nakajima | 2:d2cb6b50a8c4 | 172 | void move_forward(int time = 20) |
tsubasa_nakajima | 2:d2cb6b50a8c4 | 173 | { |
tsubasa_nakajima | 2:d2cb6b50a8c4 | 174 | Servo servo1(D7); |
tsubasa_nakajima | 2:d2cb6b50a8c4 | 175 | Servo servo2(A3); |
tsubasa_nakajima | 2:d2cb6b50a8c4 | 176 | Servo servo3(A1); |
tsubasa_nakajima | 2:d2cb6b50a8c4 | 177 | Servo servo4(D12); |
tsubasa_nakajima | 2:d2cb6b50a8c4 | 178 | Servo servo5(D10); |
tsubasa_nakajima | 2:d2cb6b50a8c4 | 179 | Servo servo6(A5); |
tsubasa_nakajima | 2:d2cb6b50a8c4 | 180 | |
tsubasa_nakajima | 2:d2cb6b50a8c4 | 181 | servo1 = 0; |
tsubasa_nakajima | 2:d2cb6b50a8c4 | 182 | servo2 = 0; |
tsubasa_nakajima | 2:d2cb6b50a8c4 | 183 | servo3 = 0; |
tsubasa_nakajima | 2:d2cb6b50a8c4 | 184 | servo4 = 0; |
tsubasa_nakajima | 2:d2cb6b50a8c4 | 185 | servo5 = 0; |
tsubasa_nakajima | 2:d2cb6b50a8c4 | 186 | servo6 = 0; |
tsubasa_nakajima | 2:d2cb6b50a8c4 | 187 | wait(time); |
minanao | 0:e7b7def631c2 | 188 | } |
tsubasa_nakajima | 2:d2cb6b50a8c4 | 189 | |
tsubasa_nakajima | 2:d2cb6b50a8c4 | 190 | //後退 |
tsubasa_nakajima | 2:d2cb6b50a8c4 | 191 | void move_backward() |
tsubasa_nakajima | 2:d2cb6b50a8c4 | 192 | { |
tsubasa_nakajima | 2:d2cb6b50a8c4 | 193 | Servo servo1(D7); |
tsubasa_nakajima | 2:d2cb6b50a8c4 | 194 | Servo servo2(A3); |
tsubasa_nakajima | 2:d2cb6b50a8c4 | 195 | Servo servo3(A1); |
tsubasa_nakajima | 2:d2cb6b50a8c4 | 196 | Servo servo4(D12); |
tsubasa_nakajima | 2:d2cb6b50a8c4 | 197 | Servo servo5(D10); |
tsubasa_nakajima | 2:d2cb6b50a8c4 | 198 | Servo servo6(A5); |
tsubasa_nakajima | 2:d2cb6b50a8c4 | 199 | |
tsubasa_nakajima | 2:d2cb6b50a8c4 | 200 | servo1 = 1; |
tsubasa_nakajima | 2:d2cb6b50a8c4 | 201 | servo2 = 1; |
tsubasa_nakajima | 2:d2cb6b50a8c4 | 202 | servo3 = 1; |
tsubasa_nakajima | 2:d2cb6b50a8c4 | 203 | servo4 = 1; |
tsubasa_nakajima | 2:d2cb6b50a8c4 | 204 | servo5 = 1; |
tsubasa_nakajima | 2:d2cb6b50a8c4 | 205 | servo6 = 1; |
tsubasa_nakajima | 2:d2cb6b50a8c4 | 206 | wait(5); |
tsubasa_nakajima | 2:d2cb6b50a8c4 | 207 | } |
tsubasa_nakajima | 2:d2cb6b50a8c4 | 208 | |
tsubasa_nakajima | 2:d2cb6b50a8c4 | 209 | //右に曲がる |
tsubasa_nakajima | 2:d2cb6b50a8c4 | 210 | void turn_right(int theta = 15) |
tsubasa_nakajima | 2:d2cb6b50a8c4 | 211 | { |
tsubasa_nakajima | 2:d2cb6b50a8c4 | 212 | Servo servo1(D7); |
tsubasa_nakajima | 2:d2cb6b50a8c4 | 213 | Servo servo2(A3); |
tsubasa_nakajima | 2:d2cb6b50a8c4 | 214 | Servo servo3(A1); |
tsubasa_nakajima | 2:d2cb6b50a8c4 | 215 | Servo servo4(D12); |
tsubasa_nakajima | 2:d2cb6b50a8c4 | 216 | Servo servo5(D10); |
tsubasa_nakajima | 2:d2cb6b50a8c4 | 217 | Servo servo6(A5); |
tsubasa_nakajima | 2:d2cb6b50a8c4 | 218 | |
tsubasa_nakajima | 2:d2cb6b50a8c4 | 219 | servo1 = 1; |
tsubasa_nakajima | 2:d2cb6b50a8c4 | 220 | servo2 = 1; |
tsubasa_nakajima | 2:d2cb6b50a8c4 | 221 | servo3 = 1; |
tsubasa_nakajima | 2:d2cb6b50a8c4 | 222 | servo4 = 0; |
tsubasa_nakajima | 2:d2cb6b50a8c4 | 223 | servo5 = 0; |
tsubasa_nakajima | 2:d2cb6b50a8c4 | 224 | servo6 = 0; |
tsubasa_nakajima | 2:d2cb6b50a8c4 | 225 | wait(theta/15); |
minanao | 0:e7b7def631c2 | 226 | } |
tsubasa_nakajima | 2:d2cb6b50a8c4 | 227 | |
tsubasa_nakajima | 2:d2cb6b50a8c4 | 228 | //左に曲がる |
tsubasa_nakajima | 2:d2cb6b50a8c4 | 229 | void turn_left(int theta = 15) |
tsubasa_nakajima | 2:d2cb6b50a8c4 | 230 | { |
tsubasa_nakajima | 2:d2cb6b50a8c4 | 231 | Servo servo1(D7); |
tsubasa_nakajima | 2:d2cb6b50a8c4 | 232 | Servo servo2(A3); |
tsubasa_nakajima | 2:d2cb6b50a8c4 | 233 | Servo servo3(A1); |
tsubasa_nakajima | 2:d2cb6b50a8c4 | 234 | Servo servo4(D12); |
tsubasa_nakajima | 2:d2cb6b50a8c4 | 235 | Servo servo5(D10); |
tsubasa_nakajima | 2:d2cb6b50a8c4 | 236 | Servo servo6(A5); |
tsubasa_nakajima | 2:d2cb6b50a8c4 | 237 | |
tsubasa_nakajima | 2:d2cb6b50a8c4 | 238 | servo1 = 0; |
tsubasa_nakajima | 2:d2cb6b50a8c4 | 239 | servo2 = 0; |
tsubasa_nakajima | 2:d2cb6b50a8c4 | 240 | servo3 = 0; |
tsubasa_nakajima | 2:d2cb6b50a8c4 | 241 | servo4 = 1; |
tsubasa_nakajima | 2:d2cb6b50a8c4 | 242 | servo5 = 1; |
tsubasa_nakajima | 2:d2cb6b50a8c4 | 243 | servo6 = 1; |
tsubasa_nakajima | 2:d2cb6b50a8c4 | 244 | wait(theta/15); |
minanao | 0:e7b7def631c2 | 245 | } |
tsubasa_nakajima | 1:bb89b58cfa0e | 246 | |
tsubasa_nakajima | 2:d2cb6b50a8c4 | 247 | //倒れているときの処理 |
tsubasa_nakajima | 2:d2cb6b50a8c4 | 248 | void wakeup(int time) |
tsubasa_nakajima | 2:d2cb6b50a8c4 | 249 | { |
tsubasa_nakajima | 2:d2cb6b50a8c4 | 250 | Servo servo1(D7); |
tsubasa_nakajima | 2:d2cb6b50a8c4 | 251 | Servo servo2(A3); |
tsubasa_nakajima | 2:d2cb6b50a8c4 | 252 | Servo servo3(A1); |
tsubasa_nakajima | 2:d2cb6b50a8c4 | 253 | Servo servo4(D12); |
tsubasa_nakajima | 2:d2cb6b50a8c4 | 254 | Servo servo5(D10); |
tsubasa_nakajima | 2:d2cb6b50a8c4 | 255 | Servo servo6(A5); |
tsubasa_nakajima | 1:bb89b58cfa0e | 256 | |
tsubasa_nakajima | 2:d2cb6b50a8c4 | 257 | int i; |
tsubasa_nakajima | 2:d2cb6b50a8c4 | 258 | for(i=1;i<=time;i++) |
tsubasa_nakajima | 2:d2cb6b50a8c4 | 259 | { |
tsubasa_nakajima | 2:d2cb6b50a8c4 | 260 | move_forward(5); |
tsubasa_nakajima | 2:d2cb6b50a8c4 | 261 | move_backward(); |
tsubasa_nakajima | 2:d2cb6b50a8c4 | 262 | turn_right(); |
tsubasa_nakajima | 2:d2cb6b50a8c4 | 263 | turn_left(); |
tsubasa_nakajima | 1:bb89b58cfa0e | 264 | } |
tsubasa_nakajima | 1:bb89b58cfa0e | 265 | |
tsubasa_nakajima | 1:bb89b58cfa0e | 266 | class direction |
tsubasa_nakajima | 1:bb89b58cfa0e | 267 | { |
tsubasa_nakajima | 1:bb89b58cfa0e | 268 | private: |
tsubasa_nakajima | 1:bb89b58cfa0e | 269 | |
tsubasa_nakajima | 1:bb89b58cfa0e | 270 | int s; |
tsubasa_nakajima | 1:bb89b58cfa0e | 271 | float x_0 ,y_0; //中間地点の座標(未定) |
tsubasa_nakajima | 1:bb89b58cfa0e | 272 | float x_01,y_01; //ゴール地点の座標(未定) |
tsubasa_nakajima | 1:bb89b58cfa0e | 273 | float x_1 ,y_1; //現在地 |
tsubasa_nakajima | 1:bb89b58cfa0e | 274 | float x_2 ,y_2; //20秒前の現在地 |
tsubasa_nakajima | 1:bb89b58cfa0e | 275 | float theta; //CanSatから見た目的地の角度 |
tsubasa_nakajima | 1:bb89b58cfa0e | 276 | float d,d1,d2; //dはCanSatの20秒間の移動距離,d1はCanSatと中間地点の距離、d2はゴール地点との距離(単位はm) |
tsubasa_nakajima | 1:bb89b58cfa0e | 277 | |
tsubasa_nakajima | 1:bb89b58cfa0e | 278 | public: |
tsubasa_nakajima | 1:bb89b58cfa0e | 279 | |
tsubasa_nakajima | 1:bb89b58cfa0e | 280 | //歩行 |
tsubasa_nakajima | 2:d2cb6b50a8c4 | 281 | void walk(){ |
tsubasa_nakajima | 2:d2cb6b50a8c4 | 282 | GPS gps(D1, D0); |
tsubasa_nakajima | 2:d2cb6b50a8c4 | 283 | s = 0; |
tsubasa_nakajima | 2:d2cb6b50a8c4 | 284 | x_0 = 0; |
tsubasa_nakajima | 2:d2cb6b50a8c4 | 285 | y_0 = 0; |
tsubasa_nakajima | 2:d2cb6b50a8c4 | 286 | x_01 = 0; |
tsubasa_nakajima | 2:d2cb6b50a8c4 | 287 | y_01 = 0; |
tsubasa_nakajima | 2:d2cb6b50a8c4 | 288 | |
tsubasa_nakajima | 1:bb89b58cfa0e | 289 | while(s<1){ |
tsubasa_nakajima | 1:bb89b58cfa0e | 290 | if(gps.getgps()){ |
tsubasa_nakajima | 1:bb89b58cfa0e | 291 | x_1 = gps.longitude; |
tsubasa_nakajima | 1:bb89b58cfa0e | 292 | y_1 = gps.latitude; |
tsubasa_nakajima | 2:d2cb6b50a8c4 | 293 | move_forward(); |
tsubasa_nakajima | 1:bb89b58cfa0e | 294 | s = s + 1; |
tsubasa_nakajima | 1:bb89b58cfa0e | 295 | } |
tsubasa_nakajima | 1:bb89b58cfa0e | 296 | } |
tsubasa_nakajima | 2:d2cb6b50a8c4 | 297 | |
tsubasa_nakajima | 2:d2cb6b50a8c4 | 298 | while(1){ |
tsubasa_nakajima | 2:d2cb6b50a8c4 | 299 | if(gps.getgps()){ //現在地取得 |
tsubasa_nakajima | 2:d2cb6b50a8c4 | 300 | x_2 = x_1; |
tsubasa_nakajima | 2:d2cb6b50a8c4 | 301 | y_2 = y_2; //20(+回転or復帰時間)秒前に居た地点の緯度と経度を取得 |
tsubasa_nakajima | 2:d2cb6b50a8c4 | 302 | x_1 = gps.longitude; |
tsubasa_nakajima | 2:d2cb6b50a8c4 | 303 | y_1 = gps.latitude; //現在地の緯度と経度を取得 |
tsubasa_nakajima | 2:d2cb6b50a8c4 | 304 | d1 = distance(y_1,x_1,y_0,x_0); |
tsubasa_nakajima | 2:d2cb6b50a8c4 | 305 | d = distance(y_1,x_1,y_2,x_2); |
tsubasa_nakajima | 2:d2cb6b50a8c4 | 306 | theta = calculate_theta(x_0,y_0,x_1,y_1,x_2,y_2); |
tsubasa_nakajima | 1:bb89b58cfa0e | 307 | |
tsubasa_nakajima | 2:d2cb6b50a8c4 | 308 | //中間地点に到達したらbreak |
tsubasa_nakajima | 2:d2cb6b50a8c4 | 309 | if(d1 < 15){ |
tsubasa_nakajima | 2:d2cb6b50a8c4 | 310 | stop(); |
tsubasa_nakajima | 2:d2cb6b50a8c4 | 311 | break; |
tsubasa_nakajima | 2:d2cb6b50a8c4 | 312 | } |
tsubasa_nakajima | 2:d2cb6b50a8c4 | 313 | |
tsubasa_nakajima | 2:d2cb6b50a8c4 | 314 | //移動距離が短ければ横転と判定し復帰 |
tsubasa_nakajima | 2:d2cb6b50a8c4 | 315 | if(d < 3){ |
tsubasa_nakajima | 2:d2cb6b50a8c4 | 316 | wakeup(2); |
tsubasa_nakajima | 2:d2cb6b50a8c4 | 317 | stop(); |
tsubasa_nakajima | 2:d2cb6b50a8c4 | 318 | } |
tsubasa_nakajima | 2:d2cb6b50a8c4 | 319 | |
tsubasa_nakajima | 2:d2cb6b50a8c4 | 320 | //Θによって回転か直進か決定 |
tsubasa_nakajima | 2:d2cb6b50a8c4 | 321 | if(-30 < theta && theta < 30){ |
tsubasa_nakajima | 2:d2cb6b50a8c4 | 322 | move_forward(); |
tsubasa_nakajima | 2:d2cb6b50a8c4 | 323 | } |
tsubasa_nakajima | 2:d2cb6b50a8c4 | 324 | if(theta >=30){ |
tsubasa_nakajima | 2:d2cb6b50a8c4 | 325 | turn_right(theta); |
tsubasa_nakajima | 2:d2cb6b50a8c4 | 326 | stop(); |
tsubasa_nakajima | 2:d2cb6b50a8c4 | 327 | move_forward(); |
tsubasa_nakajima | 2:d2cb6b50a8c4 | 328 | } |
tsubasa_nakajima | 2:d2cb6b50a8c4 | 329 | if(theta<=-30){ |
tsubasa_nakajima | 2:d2cb6b50a8c4 | 330 | turn_left(theta); |
tsubasa_nakajima | 2:d2cb6b50a8c4 | 331 | stop(); |
tsubasa_nakajima | 2:d2cb6b50a8c4 | 332 | move_forward(); |
tsubasa_nakajima | 2:d2cb6b50a8c4 | 333 | } |
tsubasa_nakajima | 2:d2cb6b50a8c4 | 334 | |
tsubasa_nakajima | 2:d2cb6b50a8c4 | 335 | } |
tsubasa_nakajima | 2:d2cb6b50a8c4 | 336 | } |
tsubasa_nakajima | 2:d2cb6b50a8c4 | 337 | while(s < 2){ |
tsubasa_nakajima | 2:d2cb6b50a8c4 | 338 | if(gps.getgps()){ |
tsubasa_nakajima | 2:d2cb6b50a8c4 | 339 | x_1 = gps.longitude; |
tsubasa_nakajima | 2:d2cb6b50a8c4 | 340 | y_1 = gps.latitude; |
tsubasa_nakajima | 2:d2cb6b50a8c4 | 341 | move_forward(); |
tsubasa_nakajima | 2:d2cb6b50a8c4 | 342 | s = s + 1; |
tsubasa_nakajima | 2:d2cb6b50a8c4 | 343 | } |
tsubasa_nakajima | 2:d2cb6b50a8c4 | 344 | } |
tsubasa_nakajima | 2:d2cb6b50a8c4 | 345 | |
tsubasa_nakajima | 2:d2cb6b50a8c4 | 346 | while(1){ |
minanao | 0:e7b7def631c2 | 347 | |
tsubasa_nakajima | 1:bb89b58cfa0e | 348 | if(gps.getgps()){ //現在地取得 |
tsubasa_nakajima | 1:bb89b58cfa0e | 349 | x_2 = x_1; |
tsubasa_nakajima | 1:bb89b58cfa0e | 350 | y_2 = y_2; //20(+回転or復帰時間)秒前に居た地点の緯度と経度を取得 |
tsubasa_nakajima | 1:bb89b58cfa0e | 351 | x_1 = gps.longitude; |
tsubasa_nakajima | 1:bb89b58cfa0e | 352 | y_1 = gps.latitude; //現在地の緯度と経度を取得 |
tsubasa_nakajima | 2:d2cb6b50a8c4 | 353 | d2 = distance(y_1,x_1,y_01,x_01); |
tsubasa_nakajima | 2:d2cb6b50a8c4 | 354 | d = distance(y_1,x_1,y_2,x_2); |
tsubasa_nakajima | 2:d2cb6b50a8c4 | 355 | theta = calculate_theta(x_01,y_01,x_1,y_1,x_2,y_2); |
tsubasa_nakajima | 1:bb89b58cfa0e | 356 | |
tsubasa_nakajima | 2:d2cb6b50a8c4 | 357 | //ゴール地点に到達したらbreak |
tsubasa_nakajima | 2:d2cb6b50a8c4 | 358 | if(d2 < 15){ |
tsubasa_nakajima | 2:d2cb6b50a8c4 | 359 | stop(); |
tsubasa_nakajima | 1:bb89b58cfa0e | 360 | break; |
tsubasa_nakajima | 1:bb89b58cfa0e | 361 | } |
tsubasa_nakajima | 1:bb89b58cfa0e | 362 | |
tsubasa_nakajima | 1:bb89b58cfa0e | 363 | //移動距離が短ければ横転と判定し復帰 |
tsubasa_nakajima | 1:bb89b58cfa0e | 364 | if(d < 3){ |
tsubasa_nakajima | 2:d2cb6b50a8c4 | 365 | wakeup(2); |
tsubasa_nakajima | 2:d2cb6b50a8c4 | 366 | stop(); |
tsubasa_nakajima | 1:bb89b58cfa0e | 367 | } |
tsubasa_nakajima | 1:bb89b58cfa0e | 368 | |
tsubasa_nakajima | 2:d2cb6b50a8c4 | 369 | //thetaによって回転か直進か決定 |
tsubasa_nakajima | 1:bb89b58cfa0e | 370 | if(-30 < theta && theta < 30){ |
tsubasa_nakajima | 2:d2cb6b50a8c4 | 371 | move_forward(); |
tsubasa_nakajima | 1:bb89b58cfa0e | 372 | } |
tsubasa_nakajima | 1:bb89b58cfa0e | 373 | if(theta >=30){ |
tsubasa_nakajima | 2:d2cb6b50a8c4 | 374 | turn_right(theta); |
tsubasa_nakajima | 2:d2cb6b50a8c4 | 375 | move_forward(); |
tsubasa_nakajima | 1:bb89b58cfa0e | 376 | } |
tsubasa_nakajima | 2:d2cb6b50a8c4 | 377 | if(theta<= -30){ |
tsubasa_nakajima | 2:d2cb6b50a8c4 | 378 | turn_left(theta); |
tsubasa_nakajima | 2:d2cb6b50a8c4 | 379 | move_forward(); |
tsubasa_nakajima | 1:bb89b58cfa0e | 380 | } |
tsubasa_nakajima | 1:bb89b58cfa0e | 381 | |
tsubasa_nakajima | 1:bb89b58cfa0e | 382 | } |
tsubasa_nakajima | 1:bb89b58cfa0e | 383 | |
minanao | 0:e7b7def631c2 | 384 | |
tsubasa_nakajima | 2:d2cb6b50a8c4 | 385 | } |
tsubasa_nakajima | 1:bb89b58cfa0e | 386 | |
tsubasa_nakajima | 2:d2cb6b50a8c4 | 387 | stop(); |
minanao | 0:e7b7def631c2 | 388 | } |
minanao | 0:e7b7def631c2 | 389 | |
tsubasa_nakajima | 2:d2cb6b50a8c4 | 390 | |
tsubasa_nakajima | 2:d2cb6b50a8c4 | 391 | }; |
minanao | 0:e7b7def631c2 | 392 | |
tsubasa_nakajima | 2:d2cb6b50a8c4 | 393 |