cansat_B 2019 / Mbed 2 deprecated CanSatB2019_main_1215_fainal

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   mbed

main.cpp@26:c89f20803919, 2019-12-15 (annotated)

Committer:
seijakunouenimutou
Date:
Sun Dec 15 04:32:30 2019 +0000
Revision:
26:c89f20803919
Parent:
25:3a9580e58a5e 
g

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