cansat_B 2019 / Mbed 2 deprecated CanSatB2019_main_1214_essence_8

GPSは500秒過ぎたらスキップされます. xbeeで送信するコメントを増やしました。

Dependencies:   mbed

main.cpp@20:a30164d29218, 2019-12-14 (annotated)

Committer:
seijakunouenimutou
Date:
Sat Dec 14 18:30:26 2019 +0000
Revision:
20:a30164d29218
Parent:
19:7f5c0514eb33
Child:
21:548ac557ee95
cansat_b_main

Who changed what in which revision?

UserRevisionLine numberNew contents of line
YUPPY 0:6212b283430c 1 #define cansatB
YUPPY 7:fae874e898b3 2 #include "mbed.h"
YUPPY 7:fae874e898b3 3 //mbed
YUPPY 7:fae874e898b3 4 #include "getGPS.h"
YUPPY 7:fae874e898b3 5 #include "math.h"
YUPPY 7:fae874e898b3 6 //GPS
YUPPY 7:fae874e898b3 7 #include "TB6612.h"
YUPPY 7:fae874e898b3 8 //motorDriver
YUPPY 7:fae874e898b3 9 #include "JPEGCamera.h"
YUPPY 7:fae874e898b3 10 //カメラ
YUPPY 0:6212b283430c 11 #include <stdio.h>
YUPPY 7:fae874e898b3 12 #include <base64.h>
YUPPY 7:fae874e898b3 13 //XBee
YUPPY 7:fae874e898b3 14 #include "us015.h"
YUPPY 7:fae874e898b3 15 // 超音波センサ
YUPPY 7:fae874e898b3 16
YUPPY 7:fae874e898b3 17
YUPPY 2:e2b803e3bcbc 18 GPS gps (p28,p27); //GPS
seijakunouenimutou 13:63746a793db9 19 Timer time_gps;
YUPPY 4:1354e56c7dd3 20
YUPPY 2:e2b803e3bcbc 21 DigitalOut FET(p21); //FET
YUPPY 7:fae874e898b3 22 DigitalOut myled(LED1);
YUPPY 7:fae874e898b3 23 US015 hs(p12,p11);
YUPPY 7:fae874e898b3 24 DigitalIn thermo(p20);
YUPPY 7:fae874e898b3 25 DigitalOut Sb612switch(p15); //焦電スイッチ
YUPPY 4:1354e56c7dd3 26 DigitalOut Ultra(p12);
YUPPY 7:fae874e898b3 27 Serial pc(USBTX,USBRX); // tx, rx
YUPPY 7:fae874e898b3 28 JPEGCamera camera(p9, p10); // TX, RX
YUPPY 19:7f5c0514eb33 29 TB6612 left1(p25,p17,p16); //モーターピン
YUPPY 19:7f5c0514eb33 30 TB6612 right1(p26,p19,p18); //モーターピン
YUPPY 7:fae874e898b3 31 Serial xbee(p13,p14); //xbee
saeichi 10:6e02396abaf5 32 DigitalIn flight(p23); //フライトピン(in)
saeichi 10:6e02396abaf5 33 DigitalOut SW(p24); //フライトピン（out）
YUPPY 5:5aa7223226df 34
YUPPY 7:fae874e898b3 35 int main()
YUPPY 7:fae874e898b3 36 {
YUPPY 7:fae874e898b3 37
YUPPY 7:fae874e898b3 38
sasaokayuzen 11:6f553aa95d08 39 Sb612switch=0; //焦電off
YUPPY 7:fae874e898b3 40 wait(1);
YUPPY 7:fae874e898b3 41 Ultra=0;//超音波off
saeichi 10:6e02396abaf5 42 SW=1;
saeichi 10:6e02396abaf5 43 flight==1;//flight pin ついてる
YUPPY 15:1deffcfa5b1a 44 FET=0;//FET off
YUPPY 15:1deffcfa5b1a 45 myled=1;
seijakunouenimutou 20:a30164d29218 46 printf("you worry about me don't you?\r\n");
saeichi 10:6e02396abaf5 47
YUPPY 7:fae874e898b3 48 //FET
seijakunouenimutou 14:228a35fcb323 49 while(1){
saeichi 10:6e02396abaf5 50 if(flight==1) {
YUPPY 15:1deffcfa5b1a 51 myled=1;
saeichi 10:6e02396abaf5 52 wait(10);
saeichi 10:6e02396abaf5 53 printf("mada\r\n");
saeichi 10:6e02396abaf5 54 }
saeichi 10:6e02396abaf5 55 else{
saeichi 10:6e02396abaf5 56 if(flight==1) {
YUPPY 15:1deffcfa5b1a 57 myled=1;
saeichi 10:6e02396abaf5 58 wait(10);
saeichi 10:6e02396abaf5 59 printf("madamada\r\n");
saeichi 10:6e02396abaf5 60 }
saeichi 10:6e02396abaf5 61 else{
YUPPY 15:1deffcfa5b1a 62 myled=0;
saeichi 10:6e02396abaf5 63 printf("yattar\r\n");
YUPPY 17:2e0a775081ce 64 wait(20);
YUPPY 15:1deffcfa5b1a 65 FET=1;
YUPPY 15:1deffcfa5b1a 66 wait(10);
saeichi 10:6e02396abaf5 67 FET=0;
saeichi 10:6e02396abaf5 68 wait(10);
saeichi 10:6e02396abaf5 69 printf("FET End!\r\n");
saeichi 10:6e02396abaf5 70 SW=0;
saeichi 10:6e02396abaf5 71
saeichi 10:6e02396abaf5 72 break;
saeichi 10:6e02396abaf5 73 }
saeichi 10:6e02396abaf5 74 }
seijakunouenimutou 14:228a35fcb323 75 }
YUPPY 7:fae874e898b3 76
YUPPY 7:fae874e898b3 77
YUPPY 5:5aa7223226df 78 //以下GPS
seijakunouenimutou 13:63746a793db9 79 double a=0;
seijakunouenimutou 13:63746a793db9 80 double b=0;
YUPPY 2:e2b803e3bcbc 81 double distance;
seijakunouenimutou 13:63746a793db9 82 double timeout_flag=0;
saeichi 10:6e02396abaf5 83 int k = 0;
saeichi 10:6e02396abaf5 84 int j = 0;
saeichi 10:6e02396abaf5 85
saeichi 10:6e02396abaf5 86 pc.printf("GPS Start\n");
seijakunouenimutou 20:a30164d29218 87 xbee.printf("start\n");
seijakunouenimutou 13:63746a793db9 88 time_gps.start();
seijakunouenimutou 13:63746a793db9 89 while(a==0)
seijakunouenimutou 13:63746a793db9 90 {
seijakunouenimutou 13:63746a793db9 91 if(gps.getgps()){
seijakunouenimutou 13:63746a793db9 92 a=gps.latitude;
seijakunouenimutou 13:63746a793db9 93 b=gps.longitude;
seijakunouenimutou 13:63746a793db9 94 }
saeichi 10:6e02396abaf5 95
seijakunouenimutou 20:a30164d29218 96 pc.printf("%lf,%lf\n",a,b);/*着地地点の緯度と軽度を測る*/
seijakunouenimutou 20:a30164d29218 97 if(time_gps.read()>500){/*500経ったら次に進む*/
seijakunouenimutou 20:a30164d29218 98 xbee.printf("timeout!!\n");/*xbeeで時間切れの場合はtimeoutと出力する*/
seijakunouenimutou 13:63746a793db9 99 timeout_flag=1;
seijakunouenimutou 13:63746a793db9 100 break;
saeichi 10:6e02396abaf5 101 }
seijakunouenimutou 13:63746a793db9 102 }
seijakunouenimutou 13:63746a793db9 103 printf("moter on%lf",a);
saeichi 10:6e02396abaf5 104 left1 = 100; //左モーター１００％
saeichi 10:6e02396abaf5 105 right1 = 100;//右モーター１００％
saeichi 10:6e02396abaf5 106
seijakunouenimutou 12:eb4befc8d2c4 107 wait(10);
saeichi 10:6e02396abaf5 108 printf("moter off");
saeichi 10:6e02396abaf5 109 left1 = 0; //左モーター０％
saeichi 10:6e02396abaf5 110 right1 = 0;//右モーター０％
saeichi 10:6e02396abaf5 111 printf("moter off");
saeichi 10:6e02396abaf5 112 wait(15);
saeichi 10:6e02396abaf5 113
seijakunouenimutou 13:63746a793db9 114 while(!timeout_flag) {
seijakunouenimutou 13:63746a793db9 115 printf("GPS restart\n");
saeichi 10:6e02396abaf5 116 if(gps.getgps()){
YUPPY 2:e2b803e3bcbc 117
saeichi 10:6e02396abaf5 118 pc.printf("(%lf,%lf)\r\n",gps.latitude,gps.longitude);//緯度と経度を表示
saeichi 10:6e02396abaf5 119 j ++;
seijakunouenimutou 13:63746a793db9 120 if(j<10){
saeichi 10:6e02396abaf5 121 }else{
saeichi 10:6e02396abaf5 122 // 球面三角法により、大円距離(メートル)を求める
seijakunouenimutou 13:63746a793db9 123 double lat2;
seijakunouenimutou 13:63746a793db9 124 double lon2;
seijakunouenimutou 13:63746a793db9 125 lat2 = gps.latitude;
seijakunouenimutou 13:63746a793db9 126 lon2 = gps.longitude;
YUPPY 2:e2b803e3bcbc 127
seijakunouenimutou 20:a30164d29218 128 pc.printf("(%lf,%lf)\n\r",gps.latitude,gps.longitude);//動いた後の緯度と経度を表示
saeichi 10:6e02396abaf5 129 const double pi = 3.14159265359; // 円周率
YUPPY 2:e2b803e3bcbc 130
saeichi 10:6e02396abaf5 131 double ra = a * pi / 180;
saeichi 10:6e02396abaf5 132 double rb = b * pi / 180; // 緯度経度をラジアンに変換
seijakunouenimutou 13:63746a793db9 133 double rc = lat2 * pi / 180;
seijakunouenimutou 13:63746a793db9 134 double rd = lon2 * pi / 180;
YUPPY 2:e2b803e3bcbc 135
seijakunouenimutou 13:63746a793db9 136 double e = sin(ra) * sin(rc) + cos(ra) * cos(rc) * cos(rd - rb); // 2点の中心角(ラジアン)を求める
saeichi 10:6e02396abaf5 137 double rr = acos(e);
YUPPY 2:e2b803e3bcbc 138
saeichi 10:6e02396abaf5 139 const double earth_radius = 6378140; // 地球赤道半径(m)
YUPPY 2:e2b803e3bcbc 140
saeichi 10:6e02396abaf5 141 distance = earth_radius * rr; // 2点間の距離(m)
seijakunouenimutou 13:63746a793db9 142 printf("distance=%f\ne=%lf\nrr=%lf\n",distance,e,rr);
saeichi 10:6e02396abaf5 143 if (distance<5){
YUPPY 2:e2b803e3bcbc 144 }else{
seijakunouenimutou 13:63746a793db9 145 pc.printf("(a =%lf,b =%lf)\n\r",a,b);
saeichi 10:6e02396abaf5 146 pc.printf("(c =%lf,d =%lf)\n\r",gps.latitude,gps.longitude);//c,dを表示
YUPPY 2:e2b803e3bcbc 147 pc.printf("5m clear!");
saeichi 10:6e02396abaf5 148 xbee.printf("5m clear!");
saeichi 10:6e02396abaf5 149 break;
saeichi 10:6e02396abaf5 150 }
saeichi 10:6e02396abaf5 151 }
saeichi 10:6e02396abaf5 152 }else{
saeichi 10:6e02396abaf5 153 printf("No data");
saeichi 10:6e02396abaf5 154 }
saeichi 10:6e02396abaf5 155 } //GPS End */
YUPPY 5:5aa7223226df 156
YUPPY 7:fae874e898b3 157 int i=1;
saeichi 10:6e02396abaf5 158 float th;
saeichi 10:6e02396abaf5 159 Timer tm;
saeichi 10:6e02396abaf5 160 for(i=0;i<3;i++){
saeichi 10:6e02396abaf5 161 pc.printf("start\r\n");
YUPPY 6:db1a62608047 162
saeichi 10:6e02396abaf5 163 left1 = 100; //左モーター１００％
saeichi 10:6e02396abaf5 164 right1 = 100;//右モーター１００％
saeichi 10:6e02396abaf5 165
saeichi 10:6e02396abaf5 166 printf("Restart\r\n" );
saeichi 10:6e02396abaf5 167
saeichi 10:6e02396abaf5 168 wait(4);
saeichi 10:6e02396abaf5 169 left1=50;
saeichi 10:6e02396abaf5 170 right1=50;
saeichi 10:6e02396abaf5 171 wait(1);
saeichi 10:6e02396abaf5 172 left1=0;
saeichi 10:6e02396abaf5 173 right1=0;
saeichi 10:6e02396abaf5 174 wait(1);
saeichi 10:6e02396abaf5 175
saeichi 10:6e02396abaf5 176 printf("停止\n\r");
saeichi 10:6e02396abaf5 177
saeichi 10:6e02396abaf5 178 Sb612switch=0; //焦電off
saeichi 10:6e02396abaf5 179 wait(1);
saeichi 10:6e02396abaf5 180 Ultra=1;//超音波on
saeichi 10:6e02396abaf5 181 wait(1);
saeichi 10:6e02396abaf5 182
saeichi 10:6e02396abaf5 183 while(1) {
saeichi 10:6e02396abaf5 184 left1 = 0;
saeichi 10:6e02396abaf5 185 right1 = 0;
saeichi 10:6e02396abaf5 186
saeichi 10:6e02396abaf5 187 printf("超音波on\r\n 焦電off\r\n" ) ;
saeichi 10:6e02396abaf5 188
saeichi 10:6e02396abaf5 189 hs.TrigerOut();
saeichi 10:6e02396abaf5 190 wait(1);
saeichi 10:6e02396abaf5 191 int distance;
saeichi 10:6e02396abaf5 192 distance = hs.GetDistance();
saeichi 10:6e02396abaf5 193
saeichi 10:6e02396abaf5 194 printf("distance=%dmm\r\n",distance);//距離出力
saeichi 10:6e02396abaf5 195
saeichi 10:6e02396abaf5 196 if(distance<=2000){//超音波反応
saeichi 10:6e02396abaf5 197 Ultra=0;//超音波off
saeichi 10:6e02396abaf5 198 wait(1);
saeichi 10:6e02396abaf5 199 Sb612switch=1; //焦電on
saeichi 10:6e02396abaf5 200 wait(1);
saeichi 10:6e02396abaf5 201
YUPPY 16:429f6ad98e90 202 printf("焦電On!\r\n ");
YUPPY 16:429f6ad98e90 203 bool detected=false;
YUPPY 16:429f6ad98e90 204 th = thermo;
YUPPY 16:429f6ad98e90 205 if(th==1 && !detected) {//焦電反応ありの場合
YUPPY 16:429f6ad98e90 206 i++;
YUPPY 16:429f6ad98e90 207 detected=true;
YUPPY 16:429f6ad98e90 208 pc.printf("human\r\n");
YUPPY 16:429f6ad98e90 209 tm.reset();
YUPPY 16:429f6ad98e90 210 tm.start();
YUPPY 16:429f6ad98e90 211
YUPPY 16:429f6ad98e90 212 LocalFileSystem local("local");
YUPPY 16:429f6ad98e90 213 Timer timer;
YUPPY 16:429f6ad98e90 214 timer.start();
YUPPY 16:429f6ad98e90 215 camera.setPictureSize(JPEGCamera::SIZE320x240);
saeichi 10:6e02396abaf5 216
YUPPY 16:429f6ad98e90 217 FILE *fp;
YUPPY 16:429f6ad98e90 218 base64 *bs;
YUPPY 16:429f6ad98e90 219 int c;
saeichi 10:6e02396abaf5 220
YUPPY 16:429f6ad98e90 221 for (int r = 0; r < 1; r++) {
YUPPY 16:429f6ad98e90 222 if (camera.isReady()) {
YUPPY 16:429f6ad98e90 223 char filename[32];
YUPPY 16:429f6ad98e90 224 sprintf(filename, "/local/pict%03d.jpg",r);
YUPPY 16:429f6ad98e90 225 printf("Picture: %s ", filename);
YUPPY 16:429f6ad98e90 226 if (camera.takePicture(filename)) {
YUPPY 16:429f6ad98e90 227 while (camera.isProcessing()) {
YUPPY 16:429f6ad98e90 228 camera.processPicture();
YUPPY 16:429f6ad98e90 229 printf("take pictuer!");
YUPPY 16:429f6ad98e90 230 }
YUPPY 16:429f6ad98e90 231 }else{
YUPPY 16:429f6ad98e90 232 printf("take picture failed\r\n");
YUPPY 16:429f6ad98e90 233 }
YUPPY 16:429f6ad98e90 234 }else{
YUPPY 16:429f6ad98e90 235 printf("camera is not ready\r\n");
YUPPY 16:429f6ad98e90 236 }
saeichi 10:6e02396abaf5 237
YUPPY 16:429f6ad98e90 238 printf("time = %f\n", timer.read());
YUPPY 16:429f6ad98e90 239
YUPPY 16:429f6ad98e90 240
saeichi 10:6e02396abaf5 241
YUPPY 16:429f6ad98e90 242 xbee.printf("xbee connected!\r\n");
YUPPY 16:429f6ad98e90 243 bs = new base64();
YUPPY 16:429f6ad98e90 244 bs->Encode("/local/pict000.jpg","/local/data000.txt");
YUPPY 16:429f6ad98e90 245
YUPPY 16:429f6ad98e90 246 if((fp=fopen("/local/data000.txt","r"))!=NULL)
YUPPY 16:429f6ad98e90 247 {
saeichi 10:6e02396abaf5 248 pc.printf("ok\r\n");
saeichi 10:6e02396abaf5 249 while((c=fgetc(fp))!=EOF){
YUPPY 16:429f6ad98e90 250 xbee.printf("%c",c);
saeichi 10:6e02396abaf5 251 }
saeichi 10:6e02396abaf5 252 fclose(fp);
YUPPY 16:429f6ad98e90 253 }
saeichi 10:6e02396abaf5 254 }
YUPPY 16:429f6ad98e90 255 while(1){
saeichi 10:6e02396abaf5 256
YUPPY 16:429f6ad98e90 257 int received_data = xbee.getc();
saeichi 10:6e02396abaf5 258
YUPPY 16:429f6ad98e90 259 if (received_data == 82 || received_data == 114){ //Rまたはｒ
YUPPY 16:429f6ad98e90 260 xbee.printf("_________________________________________________________________________________________________________________________________\r\n");
YUPPY 16:429f6ad98e90 261 if((fp=fopen("/local/data000.txt","r"))!=NULL)
YUPPY 16:429f6ad98e90 262 {
YUPPY 16:429f6ad98e90 263 while ((c=fgetc(fp))!=EOF){
YUPPY 16:429f6ad98e90 264 xbee.printf("%c",c); //再送
YUPPY 16:429f6ad98e90 265 }
YUPPY 16:429f6ad98e90 266 fclose(fp);
YUPPY 16:429f6ad98e90 267 }
YUPPY 16:429f6ad98e90 268 }
YUPPY 16:429f6ad98e90 269 else{
YUPPY 16:429f6ad98e90 270 break;
YUPPY 16:429f6ad98e90 271 }
YUPPY 16:429f6ad98e90 272 }
saeichi 10:6e02396abaf5 273
YUPPY 16:429f6ad98e90 274 Sb612switch=0; //焦電off
YUPPY 16:429f6ad98e90 275 wait(1);
YUPPY 16:429f6ad98e90 276 }else{//焦電反応なしの場合
YUPPY 16:429f6ad98e90 277 printf("not found!\r\n");
YUPPY 16:429f6ad98e90 278
YUPPY 7:fae874e898b3 279
YUPPY 16:429f6ad98e90 280 Sb612switch=0;
YUPPY 16:429f6ad98e90 281 wait(1);
YUPPY 16:429f6ad98e90 282 Ultra=0;
YUPPY 16:429f6ad98e90 283 wait(1);
YUPPY 16:429f6ad98e90 284 detected=false;
YUPPY 16:429f6ad98e90 285 printf("後退\r\n");
YUPPY 16:429f6ad98e90 286 left1 = -100; //左モーター-5０％
YUPPY 16:429f6ad98e90 287 right1 = -100;//右モーター-5０％
YUPPY 16:429f6ad98e90 288 wait(2.0);
YUPPY 16:429f6ad98e90 289 left1=-50;
YUPPY 16:429f6ad98e90 290 right1=-50;
YUPPY 16:429f6ad98e90 291 wait(1);
YUPPY 16:429f6ad98e90 292 left1=0;
YUPPY 16:429f6ad98e90 293 right1=0;
YUPPY 16:429f6ad98e90 294 wait(1);
YUPPY 16:429f6ad98e90 295
YUPPY 16:429f6ad98e90 296 printf("右折\n\r");
YUPPY 16:429f6ad98e90 297
YUPPY 16:429f6ad98e90 298 left1 = 60; //左モーター１００％
YUPPY 16:429f6ad98e90 299 right1 = 100;//右モーター１００％
YUPPY 16:429f6ad98e90 300 wait(2.0);
YUPPY 16:429f6ad98e90 301 }
saeichi 10:6e02396abaf5 302 }else{//超音波distance>2000
saeichi 10:6e02396abaf5 303 printf("safety zone\r\n");
saeichi 10:6e02396abaf5 304 Ultra=0;
saeichi 10:6e02396abaf5 305 wait(1);
saeichi 10:6e02396abaf5 306 left1 = 60; //左モーター50％
saeichi 10:6e02396abaf5 307 right1 = 100;//右モーター50％
saeichi 10:6e02396abaf5 308 printf("右折\r\n");
saeichi 10:6e02396abaf5 309 wait(3);
saeichi 10:6e02396abaf5 310 left1 = 100;
saeichi 10:6e02396abaf5 311 right1 = 100;
saeichi 10:6e02396abaf5 312 wait(5);
saeichi 10:6e02396abaf5 313 left1 = 0;
saeichi 10:6e02396abaf5 314 right1 = 0;
saeichi 10:6e02396abaf5 315 wait(5);
saeichi 10:6e02396abaf5 316 }
YUPPY 4:1354e56c7dd3 317
YUPPY 5:5aa7223226df 318 }
YUPPY 7:fae874e898b3 319
saeichi 10:6e02396abaf5 320 }
YUPPY 7:fae874e898b3 321 }