CansatB_2021 / Mbed 2 deprecated cansat_integrated

統合プログラム

Dependencies:   mbed Servo BMP180

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?

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