cansat_B 2019 / Mbed 2 deprecated CanSatB2019_main_1214_essence_8

Dependencies:   mbed

main.cpp@14:228a35fcb323, 2019-12-14 (annotated)

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