CansatB_2021 / Mbed 2 deprecated cansat_integrated

統合プログラム

Dependencies:   mbed Servo BMP180

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?

UserRevisionLine numberNew 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