TankSat / Mbed 2 deprecated TankSat

fuck

Dependencies:   mbed GPS_interrupt

main.cpp@0:ae907b1bf663, 2019-08-10 (annotated)

Committer:
Okamoto009
Date:
Sat Aug 10 02:19:02 2019 +0000
Revision:
0:ae907b1bf663
fuck summer

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Okamoto009 0:ae907b1bf663 1 #include "mbed.h"
Okamoto009 0:ae907b1bf663 2 #include "GPS_interrupt.h"
Okamoto009 0:ae907b1bf663 3
Okamoto009 0:ae907b1bf663 4 Serial pc(USBTX,USBRX);
Okamoto009 0:ae907b1bf663 5
Okamoto009 0:ae907b1bf663 6 Serial gps_bus(p9,p10);
Okamoto009 0:ae907b1bf663 7 GPS_interrupt gps(&gps_bus,9600);
Okamoto009 0:ae907b1bf663 8
Okamoto009 0:ae907b1bf663 9 double tx=130.216004,ty=33.594562; //ゴール座標
Okamoto009 0:ae907b1bf663 10 double elat = 111319.528; //緯度一度当たりの距離(m)
Okamoto009 0:ae907b1bf663 11 double elon = 91904.295; //種子島における経度一度当たりの距離(m)
Okamoto009 0:ae907b1bf663 12
Okamoto009 0:ae907b1bf663 13 double pi=3.141592;
Okamoto009 0:ae907b1bf663 14
Okamoto009 0:ae907b1bf663 15 InterruptIn fpin(p25);
Okamoto009 0:ae907b1bf663 16 void up();
Okamoto009 0:ae907b1bf663 17
Okamoto009 0:ae907b1bf663 18 PwmOut servo(p22);
Okamoto009 0:ae907b1bf663 19
Okamoto009 0:ae907b1bf663 20 int a=1,b=1,c=1;
Okamoto009 0:ae907b1bf663 21
Okamoto009 0:ae907b1bf663 22 Serial sd(p28,p27);
Okamoto009 0:ae907b1bf663 23
Okamoto009 0:ae907b1bf663 24 Serial XB(p13,p14,115200);
Okamoto009 0:ae907b1bf663 25
Okamoto009 0:ae907b1bf663 26 DigitalOut mos(p21);
Okamoto009 0:ae907b1bf663 27 //------------------------------------------------------------------------------
Okamoto009 0:ae907b1bf663 28 // 超音波センサ
Okamoto009 0:ae907b1bf663 29 //------------------------------------------------------------------------------
Okamoto009 0:ae907b1bf663 30 Timer tmr;
Okamoto009 0:ae907b1bf663 31 DigitalOut trig(p23);
Okamoto009 0:ae907b1bf663 32 DigitalIn echo(p24);
Okamoto009 0:ae907b1bf663 33 int Otime,Atime; //停止時間,開始時間
Okamoto009 0:ae907b1bf663 34 float Delta,ALT; //開始時間と停止時間の差,地面からの高さ
Okamoto009 0:ae907b1bf663 35
Okamoto009 0:ae907b1bf663 36 //------------------------------------------------------------------------------
Okamoto009 0:ae907b1bf663 37 // 超音波センサを用いて機体の地面からの高さを測る関数
Okamoto009 0:ae907b1bf663 38 //------------------------------------------------------------------------------
Okamoto009 0:ae907b1bf663 39 void measureH(){
Okamoto009 0:ae907b1bf663 40 trig=1;
Okamoto009 0:ae907b1bf663 41 wait(0.00002);
Okamoto009 0:ae907b1bf663 42 trig=0;
Okamoto009 0:ae907b1bf663 43 while(echo!=1){}
Okamoto009 0:ae907b1bf663 44 Atime=tmr.read_us();
Okamoto009 0:ae907b1bf663 45 while(echo==1){}
Okamoto009 0:ae907b1bf663 46 Otime=tmr.read_us();
Okamoto009 0:ae907b1bf663 47 Delta=Otime-Atime;
Okamoto009 0:ae907b1bf663 48 ALT=((Delta)/2)*340*1000/1000000;
Okamoto009 0:ae907b1bf663 49 sd.printf("ALT = %f [mm],",ALT);
Okamoto009 0:ae907b1bf663 50 wait(0.01);
Okamoto009 0:ae907b1bf663 51 }
Okamoto009 0:ae907b1bf663 52
Okamoto009 0:ae907b1bf663 53 //------------------------------------------------------------------------------
Okamoto009 0:ae907b1bf663 54 // 移動
Okamoto009 0:ae907b1bf663 55 //------------------------------------------------------------------------------
Okamoto009 0:ae907b1bf663 56 DigitalOut dgo_R1(p18);
Okamoto009 0:ae907b1bf663 57 DigitalOut dgo_R2(p17); //p17と18は入れ替わる可能性あり
Okamoto009 0:ae907b1bf663 58 DigitalOut dgo_L1(p19);
Okamoto009 0:ae907b1bf663 59 DigitalOut dgo_L2(p20); //p19と20は入れ替わる可能性あり
Okamoto009 0:ae907b1bf663 60
Okamoto009 0:ae907b1bf663 61 //前進
Okamoto009 0:ae907b1bf663 62 void forward(){
Okamoto009 0:ae907b1bf663 63 dgo_R1=0;
Okamoto009 0:ae907b1bf663 64 dgo_R2=1;
Okamoto009 0:ae907b1bf663 65 wait(0.3);
Okamoto009 0:ae907b1bf663 66 dgo_R1=0;
Okamoto009 0:ae907b1bf663 67 dgo_R2=1;
Okamoto009 0:ae907b1bf663 68 dgo_L1=0;
Okamoto009 0:ae907b1bf663 69 dgo_L2=1;
Okamoto009 0:ae907b1bf663 70 XB.printf("Forward\r\n");
Okamoto009 0:ae907b1bf663 71 sd.printf("Forward,");
Okamoto009 0:ae907b1bf663 72 }
Okamoto009 0:ae907b1bf663 73
Okamoto009 0:ae907b1bf663 74 //後退
Okamoto009 0:ae907b1bf663 75 void back(){
Okamoto009 0:ae907b1bf663 76 dgo_R1=1;
Okamoto009 0:ae907b1bf663 77 dgo_R2=0;
Okamoto009 0:ae907b1bf663 78 dgo_L1=1;
Okamoto009 0:ae907b1bf663 79 dgo_L2=0;
Okamoto009 0:ae907b1bf663 80 XB.printf("Buck\r\n");
Okamoto009 0:ae907b1bf663 81 sd.printf("Buck,");
Okamoto009 0:ae907b1bf663 82 }
Okamoto009 0:ae907b1bf663 83
Okamoto009 0:ae907b1bf663 84 //停止
Okamoto009 0:ae907b1bf663 85 void stop(){
Okamoto009 0:ae907b1bf663 86 dgo_R1=1;
Okamoto009 0:ae907b1bf663 87 dgo_R2=1;
Okamoto009 0:ae907b1bf663 88 dgo_L1=1;
Okamoto009 0:ae907b1bf663 89 dgo_L2=1;
Okamoto009 0:ae907b1bf663 90 wait(0.50);
Okamoto009 0:ae907b1bf663 91 dgo_R1=0;
Okamoto009 0:ae907b1bf663 92 dgo_R2=0;
Okamoto009 0:ae907b1bf663 93 dgo_L1=0;
Okamoto009 0:ae907b1bf663 94 dgo_L2=0;
Okamoto009 0:ae907b1bf663 95 XB.printf("Stop\r\n");
Okamoto009 0:ae907b1bf663 96 sd.printf("Stop,");
Okamoto009 0:ae907b1bf663 97 }
Okamoto009 0:ae907b1bf663 98
Okamoto009 0:ae907b1bf663 99 //右折
Okamoto009 0:ae907b1bf663 100 void right(){
Okamoto009 0:ae907b1bf663 101 dgo_L1=0;
Okamoto009 0:ae907b1bf663 102 dgo_L2=1;
Okamoto009 0:ae907b1bf663 103 XB.printf("Turn right\r\n");
Okamoto009 0:ae907b1bf663 104 sd.printf("Turn right,");
Okamoto009 0:ae907b1bf663 105 }
Okamoto009 0:ae907b1bf663 106
Okamoto009 0:ae907b1bf663 107 //左折
Okamoto009 0:ae907b1bf663 108 void left(){
Okamoto009 0:ae907b1bf663 109 dgo_R1=0;
Okamoto009 0:ae907b1bf663 110 dgo_R2=1;
Okamoto009 0:ae907b1bf663 111 XB.printf("Turn left\r\n");
Okamoto009 0:ae907b1bf663 112 sd.printf("Turn left,");
Okamoto009 0:ae907b1bf663 113 }
Okamoto009 0:ae907b1bf663 114
Okamoto009 0:ae907b1bf663 115 void up(){
Okamoto009 0:ae907b1bf663 116 a=2;
Okamoto009 0:ae907b1bf663 117 }
Okamoto009 0:ae907b1bf663 118
Okamoto009 0:ae907b1bf663 119 //------------------------------------------------------------------------------
Okamoto009 0:ae907b1bf663 120 // 転倒回復
Okamoto009 0:ae907b1bf663 121 //------------------------------------------------------------------------------
Okamoto009 0:ae907b1bf663 122 void recover(){
Okamoto009 0:ae907b1bf663 123 for(float p=0;p<0.001;p+=0.00001){
Okamoto009 0:ae907b1bf663 124 servo.pulsewidth(0.001+p);
Okamoto009 0:ae907b1bf663 125 wait(0.01);
Okamoto009 0:ae907b1bf663 126 }
Okamoto009 0:ae907b1bf663 127 for(float p=0.001;p>0;p-=0.00001){
Okamoto009 0:ae907b1bf663 128 servo.pulsewidth(0.001+p);
Okamoto009 0:ae907b1bf663 129 wait(0.01);
Okamoto009 0:ae907b1bf663 130 }
Okamoto009 0:ae907b1bf663 131 XB.printf("Recovering.\r\n");
Okamoto009 0:ae907b1bf663 132 sd.printf("Recovering,");
Okamoto009 0:ae907b1bf663 133 }
Okamoto009 0:ae907b1bf663 134
Okamoto009 0:ae907b1bf663 135 //------------------------------------------------------------------------------
Okamoto009 0:ae907b1bf663 136 // ここからメイン文
Okamoto009 0:ae907b1bf663 137 //------------------------------------------------------------------------------
Okamoto009 0:ae907b1bf663 138 int main(){
Okamoto009 0:ae907b1bf663 139
Okamoto009 0:ae907b1bf663 140 //------------------------------------------------------------------------------
Okamoto009 0:ae907b1bf663 141 // フライトピン外れるまでの処理
Okamoto009 0:ae907b1bf663 142 //------------------------------------------------------------------------------
Okamoto009 0:ae907b1bf663 143 fpin.mode(PullUp);
Okamoto009 0:ae907b1bf663 144 while(a==1){
Okamoto009 0:ae907b1bf663 145 XB.printf("Stand by...\r\n");
Okamoto009 0:ae907b1bf663 146 //pc.printf("Stand by...\r\n");
Okamoto009 0:ae907b1bf663 147 sd.printf("Stand by...\r\n");
Okamoto009 0:ae907b1bf663 148 float utc[6]={0};
Okamoto009 0:ae907b1bf663 149 gps.getUTC(utc);
Okamoto009 0:ae907b1bf663 150 XB.printf("%d/%d/%d %d:%d:%02.2f\r\n",(int)utc[0],(int)utc[1], (int)utc[2], (int)utc[3], (int)utc[4] ,utc[5]);
Okamoto009 0:ae907b1bf663 151 //pc.printf("%d/%d/%d %d:%d:%02.2f\r\n",(int)utc[0],(int)utc[1], (int)utc[2], (int)utc[3], (int)utc[4] ,utc[5]);
Okamoto009 0:ae907b1bf663 152 sd.printf("%d/%d/%d %d:%d:%02.2f\r\n",(int)utc[0],(int)utc[1], (int)utc[2], (int)utc[3], (int)utc[4] ,utc[5]);
Okamoto009 0:ae907b1bf663 153 wait(3.0);
Okamoto009 0:ae907b1bf663 154 fpin.rise(&up);
Okamoto009 0:ae907b1bf663 155 }
Okamoto009 0:ae907b1bf663 156
Okamoto009 0:ae907b1bf663 157 //------------------------------------------------------------------------------
Okamoto009 0:ae907b1bf663 158 // ここからフライトピン外れた後の処理
Okamoto009 0:ae907b1bf663 159 //------------------------------------------------------------------------------
Okamoto009 0:ae907b1bf663 160 XB.printf("\n");
Okamoto009 0:ae907b1bf663 161 sd.printf("\n");
Okamoto009 0:ae907b1bf663 162 XB.printf("Operation start.\r\n");
Okamoto009 0:ae907b1bf663 163 sd.printf("Operation start.\r\n");
Okamoto009 0:ae907b1bf663 164
Okamoto009 0:ae907b1bf663 165 tmr.start();
Okamoto009 0:ae907b1bf663 166 while(b==1){
Okamoto009 0:ae907b1bf663 167 measureH();
Okamoto009 0:ae907b1bf663 168 XB.printf("ALT = %f [mm]\r\n",ALT);
Okamoto009 0:ae907b1bf663 169 if(ALT>=10 && ALT<=2000){
Okamoto009 0:ae907b1bf663 170 b=2;
Okamoto009 0:ae907b1bf663 171 }
Okamoto009 0:ae907b1bf663 172 else{
Okamoto009 0:ae907b1bf663 173 if(Otime>60000000){
Okamoto009 0:ae907b1bf663 174 b=2;
Okamoto009 0:ae907b1bf663 175 }
Okamoto009 0:ae907b1bf663 176 }
Okamoto009 0:ae907b1bf663 177 }
Okamoto009 0:ae907b1bf663 178
Okamoto009 0:ae907b1bf663 179 sd.printf("\r\n");
Okamoto009 0:ae907b1bf663 180 XB.printf("Now separate Para.\r\n");
Okamoto009 0:ae907b1bf663 181 sd.printf("Now separate Para.\r\n");
Okamoto009 0:ae907b1bf663 182
Okamoto009 0:ae907b1bf663 183 mos=1;
Okamoto009 0:ae907b1bf663 184 wait(3.0);
Okamoto009 0:ae907b1bf663 185 mos=0;
Okamoto009 0:ae907b1bf663 186
Okamoto009 0:ae907b1bf663 187 wait(10.0);
Okamoto009 0:ae907b1bf663 188
Okamoto009 0:ae907b1bf663 189 //------------------------------------------------------------------------------
Okamoto009 0:ae907b1bf663 190 // 着地後のXBとGPSのチェック
Okamoto009 0:ae907b1bf663 191 //------------------------------------------------------------------------------
Okamoto009 0:ae907b1bf663 192 float utc[6]={0};
Okamoto009 0:ae907b1bf663 193 gps.getUTC(utc);
Okamoto009 0:ae907b1bf663 194 XB.printf("The Tank Has Landed at %d/%d/%d %d:%d:%02.2f \r\n",(int)utc[0],(int)utc[1], (int)utc[2], (int)utc[3], (int)utc[4] ,utc[5]);
Okamoto009 0:ae907b1bf663 195 sd.printf("The Tank Has Landed at %d/%d/%d %d:%d:%02.2f \r\n",(int)utc[0],(int)utc[1], (int)utc[2], (int)utc[3], (int)utc[4] ,utc[5]);
Okamoto009 0:ae907b1bf663 196
Okamoto009 0:ae907b1bf663 197 forward();
Okamoto009 0:ae907b1bf663 198 wait(3.0);
Okamoto009 0:ae907b1bf663 199 stop();
Okamoto009 0:ae907b1bf663 200
Okamoto009 0:ae907b1bf663 201
Okamoto009 0:ae907b1bf663 202
Okamoto009 0:ae907b1bf663 203 while(1){
Okamoto009 0:ae907b1bf663 204 sd.printf("\r\n");
Okamoto009 0:ae907b1bf663 205 //----------------------------------------------------------------------
Okamoto009 0:ae907b1bf663 206 // 移動前の座標測定
Okamoto009 0:ae907b1bf663 207 //----------------------------------------------------------------------
Okamoto009 0:ae907b1bf663 208 double xy1[2]={0};
Okamoto009 0:ae907b1bf663 209 float utc[6]={0};
Okamoto009 0:ae907b1bf663 210 gps.getPosition(xy1);
Okamoto009 0:ae907b1bf663 211 gps.getUTC(utc);
Okamoto009 0:ae907b1bf663 212 sd.printf("%d/%d/%d %d:%d:%02.2f,",(int)utc[0],(int)utc[1], (int)utc[2], (int)utc[3], (int)utc[4] ,utc[5]);
Okamoto009 0:ae907b1bf663 213 XB.printf("Longitude:%3.7f,Latitude:%3.7f\r\n",xy1[0],xy1[1]);
Okamoto009 0:ae907b1bf663 214 sd.printf("Longitude:%3.7f,Latitude:%3.7f,",xy1[0],xy1[1]);
Okamoto009 0:ae907b1bf663 215
Okamoto009 0:ae907b1bf663 216 double X1=(xy1[0]-tx)*elon; //目標までの東西方向の距離
Okamoto009 0:ae907b1bf663 217 double Y1=(xy1[1]-ty)*elat; //目標までの南北方向の距離
Okamoto009 0:ae907b1bf663 218 double dis1=sqrt(X1*X1+Y1*Y1); //移動前の座標から目標までの距離
Okamoto009 0:ae907b1bf663 219
Okamoto009 0:ae907b1bf663 220 XB.printf("%3.2f[m] to target.\r\n",dis1);
Okamoto009 0:ae907b1bf663 221 sd.printf("%3.2f[m] to target,",dis1);
Okamoto009 0:ae907b1bf663 222 //------------------------------------------------------------------
Okamoto009 0:ae907b1bf663 223 // 目標が近い場合
Okamoto009 0:ae907b1bf663 224 //------------------------------------------------------------------
Okamoto009 0:ae907b1bf663 225 if(dis1<5){
Okamoto009 0:ae907b1bf663 226 forward();
Okamoto009 0:ae907b1bf663 227 wait(5);
Okamoto009 0:ae907b1bf663 228 stop();
Okamoto009 0:ae907b1bf663 229 sd.printf("\r\n");
Okamoto009 0:ae907b1bf663 230 XB.printf("Mission accomplished!!\r\n");
Okamoto009 0:ae907b1bf663 231 sd.printf("Mission accomplished!!\r\n");
Okamoto009 0:ae907b1bf663 232 break;
Okamoto009 0:ae907b1bf663 233 }
Okamoto009 0:ae907b1bf663 234
Okamoto009 0:ae907b1bf663 235 //----------------------------------------------------------------------
Okamoto009 0:ae907b1bf663 236 // 目標まで距離がある場合
Okamoto009 0:ae907b1bf663 237 //----------------------------------------------------------------------
Okamoto009 0:ae907b1bf663 238 else{
Okamoto009 0:ae907b1bf663 239 //----------------------------------------------------------------------
Okamoto009 0:ae907b1bf663 240 // 前進し、速度、角度を測定
Okamoto009 0:ae907b1bf663 241 //----------------------------------------------------------------------
Okamoto009 0:ae907b1bf663 242 forward();
Okamoto009 0:ae907b1bf663 243 wait(5.0);
Okamoto009 0:ae907b1bf663 244 double spd=gps.Knot()*1852/3600; //機体の速度
Okamoto009 0:ae907b1bf663 245 double deg=gps.Degree(); //機体の進行方向（北から右回り正）
Okamoto009 0:ae907b1bf663 246 wait(5.0);
Okamoto009 0:ae907b1bf663 247 stop();
Okamoto009 0:ae907b1bf663 248
Okamoto009 0:ae907b1bf663 249 //----------------------------------------------------------------------
Okamoto009 0:ae907b1bf663 250 // 移動後の座標測定
Okamoto009 0:ae907b1bf663 251 //----------------------------------------------------------------------
Okamoto009 0:ae907b1bf663 252 double xy2[2]={0};
Okamoto009 0:ae907b1bf663 253 gps.getPosition(xy2);
Okamoto009 0:ae907b1bf663 254 XB.printf("Longitude:%3.7f,Latitude:%3.7f\r\n",xy2[0],xy2[1]);
Okamoto009 0:ae907b1bf663 255 sd.printf("Longitude:%3.7f,Latitude:%3.7f,",xy2[0],xy2[1]);
Okamoto009 0:ae907b1bf663 256 sd.printf("%.3f[m/s], %3.2f[degree],",spd,deg);
Okamoto009 0:ae907b1bf663 257
Okamoto009 0:ae907b1bf663 258 wait(0.1);
Okamoto009 0:ae907b1bf663 259
Okamoto009 0:ae907b1bf663 260 //--------------------------------------------------------------------------
Okamoto009 0:ae907b1bf663 261 // 距離の計算
Okamoto009 0:ae907b1bf663 262 //--------------------------------------------------------------------------
Okamoto009 0:ae907b1bf663 263 double X2=(xy2[0]-tx)*elon;
Okamoto009 0:ae907b1bf663 264 double Y2=(xy2[1]-ty)*elat;
Okamoto009 0:ae907b1bf663 265 double dis2=sqrt(X2*X2+Y2*Y2); //移動後の座標から目標までの距離
Okamoto009 0:ae907b1bf663 266
Okamoto009 0:ae907b1bf663 267 double X3=(xy1[0]-xy2[0])*elon;
Okamoto009 0:ae907b1bf663 268 double Y3=(xy1[1]-xy2[1])*elat;
Okamoto009 0:ae907b1bf663 269 double dis3=sqrt(X3*X3+Y3*Y3); //移動距離
Okamoto009 0:ae907b1bf663 270 measureH();
Okamoto009 0:ae907b1bf663 271
Okamoto009 0:ae907b1bf663 272 //------------------------------------------------------------------
Okamoto009 0:ae907b1bf663 273 // 移動後の座標と目標との角度
Okamoto009 0:ae907b1bf663 274 //------------------------------------------------------------------
Okamoto009 0:ae907b1bf663 275 double an=atan2(Y2,X2)*180/pi; //atan(y,x)
Okamoto009 0:ae907b1bf663 276
Okamoto009 0:ae907b1bf663 277 /*
Okamoto009 0:ae907b1bf663 278 //--------------------------------------------------------------
Okamoto009 0:ae907b1bf663 279 // 0.1m以上前進していない場合
Okamoto009 0:ae907b1bf663 280 //--------------------------------------------------------------
Okamoto009 0:ae907b1bf663 281 if(dis3<1){
Okamoto009 0:ae907b1bf663 282 while(1){
Okamoto009 0:ae907b1bf663 283 measureH();
Okamoto009 0:ae907b1bf663 284 //------------------------------------------------------
Okamoto009 0:ae907b1bf663 285 // 転倒していない場合
Okamoto009 0:ae907b1bf663 286 //------------------------------------------------------
Okamoto009 0:ae907b1bf663 287 if(0<ALT<100){
Okamoto009 0:ae907b1bf663 288 back();
Okamoto009 0:ae907b1bf663 289 wait(5);
Okamoto009 0:ae907b1bf663 290 stop();
Okamoto009 0:ae907b1bf663 291 c=2;
Okamoto009 0:ae907b1bf663 292 }
Okamoto009 0:ae907b1bf663 293 //------------------------------------------------------
Okamoto009 0:ae907b1bf663 294 // 転倒している場合
Okamoto009 0:ae907b1bf663 295 //------------------------------------------------------
Okamoto009 0:ae907b1bf663 296 else{
Okamoto009 0:ae907b1bf663 297 forward();
Okamoto009 0:ae907b1bf663 298 wait(2);
Okamoto009 0:ae907b1bf663 299 recover();
Okamoto009 0:ae907b1bf663 300 stop();
Okamoto009 0:ae907b1bf663 301 }
Okamoto009 0:ae907b1bf663 302 }
Okamoto009 0:ae907b1bf663 303 }
Okamoto009 0:ae907b1bf663 304 */
Okamoto009 0:ae907b1bf663 305 if(100<ALT){
Okamoto009 0:ae907b1bf663 306 while(1){
Okamoto009 0:ae907b1bf663 307 XB.printf("Stumbled...\r\n");
Okamoto009 0:ae907b1bf663 308 sd.printf("Stumbled...,");
Okamoto009 0:ae907b1bf663 309 forward();
Okamoto009 0:ae907b1bf663 310 wait(1);
Okamoto009 0:ae907b1bf663 311 recover();
Okamoto009 0:ae907b1bf663 312 wait(1);
Okamoto009 0:ae907b1bf663 313 stop();
Okamoto009 0:ae907b1bf663 314 wait(3);
Okamoto009 0:ae907b1bf663 315 measureH();
Okamoto009 0:ae907b1bf663 316 if(0<ALT<100){
Okamoto009 0:ae907b1bf663 317 break;
Okamoto009 0:ae907b1bf663 318 }
Okamoto009 0:ae907b1bf663 319 }
Okamoto009 0:ae907b1bf663 320 }
Okamoto009 0:ae907b1bf663 321
Okamoto009 0:ae907b1bf663 322 //--------------------------------------------------------------
Okamoto009 0:ae907b1bf663 323 // 1m以上進んでいる場合
Okamoto009 0:ae907b1bf663 324 //--------------------------------------------------------------
Okamoto009 0:ae907b1bf663 325 else{
Okamoto009 0:ae907b1bf663 326 //----------------------------------------------------------
Okamoto009 0:ae907b1bf663 327 // 角度の計算(余弦定理）
Okamoto009 0:ae907b1bf663 328 //----------------------------------------------------------
Okamoto009 0:ae907b1bf663 329 double dir=180-acos((dis2*dis2+dis3*dis3-dis1*dis1)/(2*dis2*dis3))*180/pi ; //回転角
Okamoto009 0:ae907b1bf663 330 XB.printf("Direction : %3.2f[deg]\r\n",dir);
Okamoto009 0:ae907b1bf663 331 sd.printf("Direction : %3.2f[deg],",dir);
Okamoto009 0:ae907b1bf663 332
Okamoto009 0:ae907b1bf663 333 //----------------------------------------------------------
Okamoto009 0:ae907b1bf663 334 // 機体を回転
Okamoto009 0:ae907b1bf663 335 //----------------------------------------------------------
Okamoto009 0:ae907b1bf663 336 double t=dir/60; //角度[deg]÷回転速度[deg/s](仮に1)
Okamoto009 0:ae907b1bf663 337
Okamoto009 0:ae907b1bf663 338 if(xy2[0]>=tx&&xy2[1]>=ty){
Okamoto009 0:ae907b1bf663 339 if(0<=deg-(90-an)&&deg-(90-an)<180){
Okamoto009 0:ae907b1bf663 340 right();
Okamoto009 0:ae907b1bf663 341 wait(t);
Okamoto009 0:ae907b1bf663 342 XB.printf("%3.2f[s]\r\n",t);
Okamoto009 0:ae907b1bf663 343 sd.printf("%3.2f[s],",t);
Okamoto009 0:ae907b1bf663 344 stop();
Okamoto009 0:ae907b1bf663 345 }
Okamoto009 0:ae907b1bf663 346 else{
Okamoto009 0:ae907b1bf663 347 left();
Okamoto009 0:ae907b1bf663 348 wait(t);
Okamoto009 0:ae907b1bf663 349 XB.printf("%3.2f[s]\r\n",t);
Okamoto009 0:ae907b1bf663 350 sd.printf("%3.2f[s],",t);
Okamoto009 0:ae907b1bf663 351 stop();
Okamoto009 0:ae907b1bf663 352 }
Okamoto009 0:ae907b1bf663 353 }
Okamoto009 0:ae907b1bf663 354
Okamoto009 0:ae907b1bf663 355 if(xy2[0]<tx&&xy2[1]>=ty){
Okamoto009 0:ae907b1bf663 356 if(0<=deg-(180-(an-90))&&deg-(180-(an-90))<180){
Okamoto009 0:ae907b1bf663 357 left();
Okamoto009 0:ae907b1bf663 358 wait(t);
Okamoto009 0:ae907b1bf663 359 XB.printf("%3.2f[s]\r\n",t);
Okamoto009 0:ae907b1bf663 360 sd.printf("%3.2f[s],",t);
Okamoto009 0:ae907b1bf663 361 stop();
Okamoto009 0:ae907b1bf663 362 }
Okamoto009 0:ae907b1bf663 363 else{
Okamoto009 0:ae907b1bf663 364 right();
Okamoto009 0:ae907b1bf663 365 wait(t);
Okamoto009 0:ae907b1bf663 366 XB.printf("%3.2f[s]\r\n",t);
Okamoto009 0:ae907b1bf663 367 sd.printf("%3.2f[s],",t);
Okamoto009 0:ae907b1bf663 368 stop();
Okamoto009 0:ae907b1bf663 369 }
Okamoto009 0:ae907b1bf663 370 }
Okamoto009 0:ae907b1bf663 371
Okamoto009 0:ae907b1bf663 372 if(xy2[0]>=tx&&xy2[1]<ty){
Okamoto009 0:ae907b1bf663 373 if(0<=deg-(180-(90+an))&&deg-(180-(90+an))<180){
Okamoto009 0:ae907b1bf663 374 right();
Okamoto009 0:ae907b1bf663 375 wait(t);
Okamoto009 0:ae907b1bf663 376 XB.printf("%3.2f[s]\r\n",t);
Okamoto009 0:ae907b1bf663 377 sd.printf("%3.2f[s],",t);
Okamoto009 0:ae907b1bf663 378 stop();
Okamoto009 0:ae907b1bf663 379 }
Okamoto009 0:ae907b1bf663 380 else{
Okamoto009 0:ae907b1bf663 381 left();
Okamoto009 0:ae907b1bf663 382 wait(t);
Okamoto009 0:ae907b1bf663 383 XB.printf("%3.2f[s]\r\n",t);
Okamoto009 0:ae907b1bf663 384 sd.printf("%3.2f[s],",t);
Okamoto009 0:ae907b1bf663 385 stop();
Okamoto009 0:ae907b1bf663 386 }
Okamoto009 0:ae907b1bf663 387 }
Okamoto009 0:ae907b1bf663 388
Okamoto009 0:ae907b1bf663 389
Okamoto009 0:ae907b1bf663 390 if(xy2[0]<tx&&xy2[1]<ty){
Okamoto009 0:ae907b1bf663 391 if(0<=deg-(90-(180+an))&&deg-(90-(180+an))<180){
Okamoto009 0:ae907b1bf663 392 left();
Okamoto009 0:ae907b1bf663 393 wait(t);
Okamoto009 0:ae907b1bf663 394 XB.printf("%3.2f[s]\r\n",t);
Okamoto009 0:ae907b1bf663 395 sd.printf("%3.2f[s],",t);
Okamoto009 0:ae907b1bf663 396 stop();
Okamoto009 0:ae907b1bf663 397 }
Okamoto009 0:ae907b1bf663 398 else{
Okamoto009 0:ae907b1bf663 399 right();
Okamoto009 0:ae907b1bf663 400 wait(t);
Okamoto009 0:ae907b1bf663 401 XB.printf("%3.2f[s]\r\n",t);
Okamoto009 0:ae907b1bf663 402 sd.printf("%3.2f[s],",t);
Okamoto009 0:ae907b1bf663 403 stop();
Okamoto009 0:ae907b1bf663 404 }
Okamoto009 0:ae907b1bf663 405 }
Okamoto009 0:ae907b1bf663 406 }
Okamoto009 0:ae907b1bf663 407 }
Okamoto009 0:ae907b1bf663 408 }
Okamoto009 0:ae907b1bf663 409 }