cansat_B 2019 / Mbed 2 deprecated CanSatB2019_main_1214-2

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@13:8cc3568726a6, 2019-12-14 (annotated)

Committer:
YUPPY
Date:
Sat Dec 14 13:07:27 2019 +0000
Revision:
13:8cc3568726a6
Parent:
12:eb4befc8d2c4 
n;

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
YUPPY 4:1354e56c7dd3 19
YUPPY 2:e2b803e3bcbc 20 DigitalOut FET(p21); //FET
YUPPY 7:fae874e898b3 21 DigitalOut myled(LED1);
YUPPY 7:fae874e898b3 22 US015 hs(p12,p11);
YUPPY 7:fae874e898b3 23 DigitalIn thermo(p20);
YUPPY 7:fae874e898b3 24 DigitalOut Sb612switch(p15); //焦電スイッチ
YUPPY 4:1354e56c7dd3 25 DigitalOut Ultra(p12);
YUPPY 7:fae874e898b3 26 Serial pc(USBTX,USBRX); // tx, rx
YUPPY 7:fae874e898b3 27 JPEGCamera camera(p9, p10); // TX, RX
YUPPY 7:fae874e898b3 28 TB6612 left1(p25,p17,p16); //モーターピン
YUPPY 7:fae874e898b3 29 TB6612 right1(p26,p19,p18); //モーターピン
YUPPY 7:fae874e898b3 30 Serial xbee(p13,p14); //xbee
saeichi 10:6e02396abaf5 31 DigitalIn flight(p23); //フライトピン(in)
saeichi 10:6e02396abaf5 32 DigitalOut SW(p24); //フライトピン（out）
YUPPY 5:5aa7223226df 33
YUPPY 7:fae874e898b3 34 int main()
YUPPY 7:fae874e898b3 35 {
YUPPY 7:fae874e898b3 36
YUPPY 7:fae874e898b3 37
sasaokayuzen 11:6f553aa95d08 38 Sb612switch=0; //焦電off
YUPPY 7:fae874e898b3 39 wait(1);
YUPPY 7:fae874e898b3 40 Ultra=0;//超音波off
saeichi 10:6e02396abaf5 41 SW=1;
saeichi 10:6e02396abaf5 42 flight==1;//flight pin ついてる
YUPPY 13:8cc3568726a6 43 FET=0;//FET off
YUPPY 13:8cc3568726a6 44 myled=1;
YUPPY 5:5aa7223226df 45 printf("CanSat-B_Start!\r\n");
saeichi 10:6e02396abaf5 46
YUPPY 7:fae874e898b3 47 //FET
YUPPY 13:8cc3568726a6 48 while(1){
saeichi 10:6e02396abaf5 49 if(flight==1) {
YUPPY 13:8cc3568726a6 50 myled=1;
saeichi 10:6e02396abaf5 51 wait(10);
saeichi 10:6e02396abaf5 52 printf("mada\r\n");
saeichi 10:6e02396abaf5 53 }
saeichi 10:6e02396abaf5 54 else{
saeichi 10:6e02396abaf5 55 if(flight==1) {
YUPPY 13:8cc3568726a6 56 myled=1;
saeichi 10:6e02396abaf5 57 wait(10);
saeichi 10:6e02396abaf5 58 printf("madamada\r\n");
saeichi 10:6e02396abaf5 59 }
saeichi 10:6e02396abaf5 60 else{
YUPPY 13:8cc3568726a6 61 myled=0;
saeichi 10:6e02396abaf5 62 printf("yattar\r\n");
YUPPY 13:8cc3568726a6 63 FET=1;
YUPPY 13:8cc3568726a6 64 wait(10);
saeichi 10:6e02396abaf5 65 FET=0;
saeichi 10:6e02396abaf5 66 wait(10);
saeichi 10:6e02396abaf5 67 printf("FET End!\r\n");
saeichi 10:6e02396abaf5 68 SW=0;
saeichi 10:6e02396abaf5 69
saeichi 10:6e02396abaf5 70 break;
saeichi 10:6e02396abaf5 71 }
saeichi 10:6e02396abaf5 72 }
YUPPY 13:8cc3568726a6 73 }
YUPPY 7:fae874e898b3 74
YUPPY 7:fae874e898b3 75
YUPPY 5:5aa7223226df 76 //以下GPS
YUPPY 2:e2b803e3bcbc 77 double a;
YUPPY 2:e2b803e3bcbc 78 double b;
YUPPY 2:e2b803e3bcbc 79 double distance;
saeichi 10:6e02396abaf5 80 int k = 0;
saeichi 10:6e02396abaf5 81 int j = 0;
saeichi 10:6e02396abaf5 82
saeichi 10:6e02396abaf5 83 pc.printf("GPS Start\n");
seijakunouenimutou 12:eb4befc8d2c4 84 xbee.printf("s\n");//printf("1");
saeichi 10:6e02396abaf5 85 while(1){
seijakunouenimutou 12:eb4befc8d2c4 86 /*printf("2");*/int received_data = xbee.getc();
seijakunouenimutou 12:eb4befc8d2c4 87 if(gps.getgps()||received_data == 97){
saeichi 10:6e02396abaf5 88
saeichi 10:6e02396abaf5 89 pc.printf("(%lf,%lf)\r\n",gps.latitude,gps.longitude);//緯度と経度を表示
seijakunouenimutou 12:eb4befc8d2c4 90 k++;
YUPPY 2:e2b803e3bcbc 91 a = gps.latitude;
YUPPY 2:e2b803e3bcbc 92 b = gps.longitude;
seijakunouenimutou 12:eb4befc8d2c4 93 if(k<10){
seijakunouenimutou 12:eb4befc8d2c4 94 printf("%d",k); if(a!=0)
seijakunouenimutou 12:eb4befc8d2c4 95 xbee.printf("dont worry! GPSは動いてるよ！\n");
seijakunouenimutou 12:eb4befc8d2c4 96 }else{
seijakunouenimutou 12:eb4befc8d2c4 97 /* a = gps.latitude;
seijakunouenimutou 12:eb4befc8d2c4 98 b = gps.longitude;*/
seijakunouenimutou 12:eb4befc8d2c4 99 /* if(a!=0)
seijakunouenimutou 12:eb4befc8d2c4 100 xbee.printf("dont worry! GPSは動いてるよ！\n");*/
seijakunouenimutou 12:eb4befc8d2c4 101 printf("(a =%lf,b =%lf)\r\n",gps.latitude,gps.longitude);//a,bを表示
seijakunouenimutou 12:eb4befc8d2c4 102 break;
saeichi 10:6e02396abaf5 103 }
YUPPY 2:e2b803e3bcbc 104 }else{
saeichi 10:6e02396abaf5 105 pc.printf("NO DATA\r\n");//データ取得失敗
saeichi 10:6e02396abaf5 106 wait(1);
saeichi 10:6e02396abaf5 107 }
saeichi 10:6e02396abaf5 108 }
saeichi 10:6e02396abaf5 109 printf("moter on");
saeichi 10:6e02396abaf5 110 left1 = 100; //左モーター１００％
saeichi 10:6e02396abaf5 111 right1 = 100;//右モーター１００％
saeichi 10:6e02396abaf5 112
seijakunouenimutou 12:eb4befc8d2c4 113 wait(10);
saeichi 10:6e02396abaf5 114 printf("moter off");
saeichi 10:6e02396abaf5 115 left1 = 0; //左モーター０％
saeichi 10:6e02396abaf5 116 right1 = 0;//右モーター０％
saeichi 10:6e02396abaf5 117 printf("moter off");
saeichi 10:6e02396abaf5 118 wait(15);
saeichi 10:6e02396abaf5 119 printf("GPS restart\n");
saeichi 10:6e02396abaf5 120
saeichi 10:6e02396abaf5 121 while(1) {
saeichi 10:6e02396abaf5 122 if(gps.getgps()){
YUPPY 2:e2b803e3bcbc 123
saeichi 10:6e02396abaf5 124 pc.printf("(%lf,%lf)\r\n",gps.latitude,gps.longitude);//緯度と経度を表示
saeichi 10:6e02396abaf5 125 j ++;
saeichi 10:6e02396abaf5 126 if(j<29){
saeichi 10:6e02396abaf5 127 }else{
saeichi 10:6e02396abaf5 128 // 球面三角法により、大円距離(メートル)を求める
saeichi 10:6e02396abaf5 129 double c;
saeichi 10:6e02396abaf5 130 double d;
saeichi 10:6e02396abaf5 131 c = gps.latitude;
saeichi 10:6e02396abaf5 132 d = gps.longitude;
YUPPY 2:e2b803e3bcbc 133
saeichi 10:6e02396abaf5 134 pc.printf("(%lf,%lf)\n\r",gps.latitude,gps.longitude);//c,dを表示
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; // 緯度経度をラジアンに変換
saeichi 10:6e02396abaf5 139 double rc = c * pi / 180;
saeichi 10:6e02396abaf5 140 double rd = d * pi / 180;
YUPPY 2:e2b803e3bcbc 141
saeichi 10:6e02396abaf5 142 double e = sin(ra) * sin(rc) + cos(ra) * cos(rc) * cos(rb - rd); // 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)
YUPPY 2:e2b803e3bcbc 148
saeichi 10:6e02396abaf5 149 if (distance<5){
YUPPY 2:e2b803e3bcbc 150 }else{
saeichi 10:6e02396abaf5 151 pc.printf("(c =%lf,d =%lf)\n\r",gps.latitude,gps.longitude);//c,dを表示
YUPPY 2:e2b803e3bcbc 152 pc.printf("5m clear!");
saeichi 10:6e02396abaf5 153 xbee.printf("5m clear!");
saeichi 10:6e02396abaf5 154 break;
saeichi 10:6e02396abaf5 155 }
saeichi 10:6e02396abaf5 156 }
saeichi 10:6e02396abaf5 157 }else{
saeichi 10:6e02396abaf5 158 printf("No data");
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++){
saeichi 10:6e02396abaf5 166 pc.printf("start\r\n");
YUPPY 6:db1a62608047 167
saeichi 10:6e02396abaf5 168 left1 = 100; //左モーター１００％
saeichi 10:6e02396abaf5 169 right1 = 100;//右モーター１００％
saeichi 10:6e02396abaf5 170
saeichi 10:6e02396abaf5 171 printf("Restart\r\n" );
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
saeichi 10:6e02396abaf5 181 printf("停止\n\r");
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
saeichi 10:6e02396abaf5 192 printf("超音波on\r\n 焦電off\r\n" ) ;
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
saeichi 10:6e02396abaf5 199 printf("distance=%dmm\r\n",distance);//距離出力
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
saeichi 10:6e02396abaf5 207 printf("焦電On!\r\n ");
saeichi 10:6e02396abaf5 208
saeichi 10:6e02396abaf5 209 bool detected=false;
saeichi 10:6e02396abaf5 210 th = thermo;
saeichi 10:6e02396abaf5 211
saeichi 10:6e02396abaf5 212 if(th==1 && !detected) {//焦電反応ありの場合
saeichi 10:6e02396abaf5 213 i++;
saeichi 10:6e02396abaf5 214 detected=true;
saeichi 10:6e02396abaf5 215 pc.printf("human\r\n");
saeichi 10:6e02396abaf5 216 tm.reset();
saeichi 10:6e02396abaf5 217 tm.start();
saeichi 10:6e02396abaf5 218
saeichi 10:6e02396abaf5 219 LocalFileSystem local("local");
saeichi 10:6e02396abaf5 220 Timer timer;
saeichi 10:6e02396abaf5 221 timer.start();
saeichi 10:6e02396abaf5 222 camera.setPictureSize(JPEGCamera::SIZE320x240);
saeichi 10:6e02396abaf5 223
saeichi 10:6e02396abaf5 224 FILE *fp;
saeichi 10:6e02396abaf5 225 base64 *bs;
saeichi 10:6e02396abaf5 226 int c;
saeichi 10:6e02396abaf5 227
saeichi 10:6e02396abaf5 228 for (int r = 0; r < 1; r++) {
saeichi 10:6e02396abaf5 229 if (camera.isReady()) {
saeichi 10:6e02396abaf5 230 char filename[25];
saeichi 10:6e02396abaf5 231 snprintf(filename,25,"%s%03d%s","/local/pict",r,".jpg");
saeichi 10:6e02396abaf5 232 printf("Picture: %s ", filename);
saeichi 10:6e02396abaf5 233 if (camera.takePicture(filename)) {
saeichi 10:6e02396abaf5 234 while (camera.isProcessing()) {
saeichi 10:6e02396abaf5 235 camera.processPicture();
saeichi 10:6e02396abaf5 236
saeichi 10:6e02396abaf5 237 printf("take pictuer!");
saeichi 10:6e02396abaf5 238
saeichi 10:6e02396abaf5 239 xbee.printf("xbee connected!!\r\n");
saeichi 10:6e02396abaf5 240
saeichi 10:6e02396abaf5 241 bs = new base64();
saeichi 10:6e02396abaf5 242 bs->Encode(filename,"/local/data.txt");
saeichi 10:6e02396abaf5 243
saeichi 10:6e02396abaf5 244 printf("time = %f\n", timer.read());
saeichi 10:6e02396abaf5 245
saeichi 10:6e02396abaf5 246
saeichi 10:6e02396abaf5 247 if((fp=fopen("/local/d.txt","r"))!=NULL){
saeichi 10:6e02396abaf5 248 pc.printf("ok\r\n");
saeichi 10:6e02396abaf5 249 while((c=fgetc(fp))!=EOF){
saeichi 10:6e02396abaf5 250 xbee.printf("%c",c);
saeichi 10:6e02396abaf5 251 }
saeichi 10:6e02396abaf5 252 fclose(fp);
saeichi 10:6e02396abaf5 253 }
saeichi 10:6e02396abaf5 254 }
saeichi 10:6e02396abaf5 255 }else{
saeichi 10:6e02396abaf5 256 printf("take picture failed\r\n");
saeichi 10:6e02396abaf5 257 }
saeichi 10:6e02396abaf5 258 }else{
saeichi 10:6e02396abaf5 259 printf("camera is not ready\r\n");
saeichi 10:6e02396abaf5 260 }
saeichi 10:6e02396abaf5 261 while(1){
saeichi 10:6e02396abaf5 262
saeichi 10:6e02396abaf5 263 int received_data = xbee.getc();
saeichi 10:6e02396abaf5 264
saeichi 10:6e02396abaf5 265 if (received_data == 82 || received_data == 114){ //Rまたはｒ
saeichi 10:6e02396abaf5 266
saeichi 10:6e02396abaf5 267 xbee.printf("_________________________________________________________________________________________________________________________________\r\n");
saeichi 10:6e02396abaf5 268 if((fp=fopen("/local/d.txt","r"))!=NULL) {
saeichi 10:6e02396abaf5 269 while ((c=fgetc(fp))!=EOF){
saeichi 10:6e02396abaf5 270
saeichi 10:6e02396abaf5 271 xbee.printf("%c",c); //再送
saeichi 10:6e02396abaf5 272 }
saeichi 10:6e02396abaf5 273 fclose(fp);
saeichi 10:6e02396abaf5 274 }
saeichi 10:6e02396abaf5 275 }else{
saeichi 10:6e02396abaf5 276 break;
saeichi 10:6e02396abaf5 277 }
saeichi 10:6e02396abaf5 278 }
saeichi 10:6e02396abaf5 279
saeichi 10:6e02396abaf5 280 Sb612switch=0; //焦電off
saeichi 10:6e02396abaf5 281 wait(1);
saeichi 10:6e02396abaf5 282 }
saeichi 10:6e02396abaf5 283 }else{//焦電反応なしの場合
saeichi 10:6e02396abaf5 284 printf("not found!\r\n");
saeichi 10:6e02396abaf5 285 Sb612switch=0;
saeichi 10:6e02396abaf5 286 wait(1);
saeichi 10:6e02396abaf5 287 Ultra=0;
saeichi 10:6e02396abaf5 288 wait(1);
saeichi 10:6e02396abaf5 289 detected=false;
saeichi 10:6e02396abaf5 290 printf("後退\r\n");
saeichi 10:6e02396abaf5 291 left1 = -100; //左モーター-50％
saeichi 10:6e02396abaf5 292 right1 = -100;//右モーター-50％
saeichi 10:6e02396abaf5 293 wait(2.0);
saeichi 10:6e02396abaf5 294 left1=-50;
saeichi 10:6e02396abaf5 295 right1=-50;
YUPPY 7:fae874e898b3 296 wait(1);
YUPPY 7:fae874e898b3 297 left1=0;
YUPPY 7:fae874e898b3 298 right1=0;
YUPPY 7:fae874e898b3 299 wait(1);
YUPPY 7:fae874e898b3 300
saeichi 10:6e02396abaf5 301 printf("右折\n\r");
saeichi 10:6e02396abaf5 302 left1 = 60; //左モーター100％
saeichi 10:6e02396abaf5 303 right1 = 100;//右モーター100％
saeichi 10:6e02396abaf5 304 wait(2.0);
saeichi 10:6e02396abaf5 305 }
saeichi 10:6e02396abaf5 306 }else{//超音波distance>2000
saeichi 10:6e02396abaf5 307 printf("safety zone\r\n");
saeichi 10:6e02396abaf5 308 Ultra=0;
saeichi 10:6e02396abaf5 309 wait(1);
saeichi 10:6e02396abaf5 310 left1 = 60; //左モーター50％
saeichi 10:6e02396abaf5 311 right1 = 100;//右モーター50％
saeichi 10:6e02396abaf5 312 printf("右折\r\n");
saeichi 10:6e02396abaf5 313 wait(3);
saeichi 10:6e02396abaf5 314 left1 = 100;
saeichi 10:6e02396abaf5 315 right1 = 100;
saeichi 10:6e02396abaf5 316 wait(5);
saeichi 10:6e02396abaf5 317 left1 = 0;
saeichi 10:6e02396abaf5 318 right1 = 0;
saeichi 10:6e02396abaf5 319 wait(5);
saeichi 10:6e02396abaf5 320 }
YUPPY 4:1354e56c7dd3 321
YUPPY 5:5aa7223226df 322 }
YUPPY 7:fae874e898b3 323
saeichi 10:6e02396abaf5 324 }
YUPPY 7:fae874e898b3 325 }