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