送信のところを変えました。
Embed:
(wiki syntax)
Show/hide line numbers
main.cpp
00001 #define cansatB 00002 #include "mbed.h" 00003 //mbed 00004 #include "getGPS.h" 00005 #include "math.h" 00006 //GPS 00007 #include "TB6612.h" 00008 //motorDriver 00009 #include "JPEGCamera.h" 00010 //カメラ 00011 #include <stdio.h> 00012 #include <base64.h> 00013 //XBee 00014 #include "us015.h" 00015 // 超音波センサ 00016 00017 00018 GPS gps (p28,p27); //GPS 00019 Timer time_gps; 00020 00021 DigitalOut FET(p21); //FET 00022 DigitalOut myled(LED1); 00023 US015 hs(p12,p11); 00024 DigitalIn thermo(p20); 00025 DigitalOut Sb612switch(p15); //焦電スイッチ 00026 DigitalOut Ultra(p12); 00027 Serial pc(USBTX,USBRX); // tx, rx 00028 JPEGCamera camera(p9, p10); // TX, RX 00029 TB6612 left1(p25,p17,p16); //モーターピン 00030 TB6612 right1(p26,p19,p18); //モーターピン 00031 Serial xbee(p13,p14); //xbee 00032 DigitalIn flight(p23); //フライトピン(in) 00033 DigitalOut SW(p24); //フライトピン(out) 00034 00035 int main() 00036 { 00037 00038 00039 Sb612switch=0; //焦電off 00040 wait(1); 00041 Ultra=0;//超音波off 00042 SW=1; 00043 flight==1;//flight pin ついてる 00044 FET=0;//FET off 00045 myled=1; 00046 xbee.printf("you worry about me don't you?\r\n"); 00047 00048 //FET 00049 while(1){ 00050 if(flight==1) { 00051 myled=1; 00052 wait(10); 00053 printf("mada\r\n"); 00054 } 00055 else{ 00056 if(flight==1) { 00057 myled=1; 00058 wait(10); 00059 xbee.printf("madamada\r\n"); 00060 } 00061 else{ 00062 myled=0; 00063 xbee.printf("yattar\r\n"); 00064 wait(20); 00065 FET=1; 00066 wait(10); 00067 FET=0; 00068 wait(10); 00069 xbee.printf("FET End!\r\n"); 00070 SW=0; 00071 00072 break; 00073 } 00074 } 00075 } 00076 00077 00078 //以下GPS 00079 double a=0; 00080 double b=0; 00081 double distance; 00082 double timeout_flag=0; 00083 int k = 0; 00084 int j = 0; 00085 00086 pc.printf("GPS Start\n"); 00087 xbee.printf("start\n"); 00088 time_gps.start(); 00089 while(a==0) 00090 { 00091 if(gps.getgps()){ 00092 a=gps.latitude; 00093 b=gps.longitude; 00094 } 00095 00096 pc.printf("%lf,%lf\n",a,b);/*着地地点の緯度と軽度を測る*/ 00097 if(time_gps.read()>500){/*500経ったら次に進む*/ 00098 xbee.printf("timeout!!\n");/*xbeeで時間切れの場合はtimeoutと出力する*/ 00099 timeout_flag=1; 00100 break; 00101 } 00102 } 00103 printf("moter on%lf",a); 00104 left1 = 100; //左モーター100% 00105 right1 = 100;//右モーター100% 00106 00107 wait(10); 00108 printf("moter off"); 00109 left1 = 0; //左モーター0% 00110 right1 = 0;//右モーター0% 00111 printf("moter off"); 00112 wait(15); 00113 00114 while(!timeout_flag) { 00115 printf("GPS restart\n"); 00116 if(gps.getgps()){ 00117 00118 pc.printf("(%lf,%lf)\r\n",gps.latitude,gps.longitude);//緯度と経度を表示 00119 j ++; 00120 if(j<10){ 00121 }else{ 00122 // 球面三角法により、大円距離(メートル)を求める 00123 double lat2; 00124 double lon2; 00125 lat2 = gps.latitude; 00126 lon2 = gps.longitude; 00127 00128 pc.printf("(%lf,%lf)\n\r",gps.latitude,gps.longitude);//動いた後の緯度と経度を表示 00129 const double pi = 3.14159265359; // 円周率 00130 00131 double ra = a * pi / 180; 00132 double rb = b * pi / 180; // 緯度経度をラジアンに変換 00133 double rc = lat2 * pi / 180; 00134 double rd = lon2 * pi / 180; 00135 00136 double e = sin(ra) * sin(rc) + cos(ra) * cos(rc) * cos(rd - rb); // 2点の中心角(ラジアン)を求める 00137 double rr = acos(e); 00138 00139 const double earth_radius = 6378140; // 地球赤道半径(m) 00140 00141 distance = earth_radius * rr; // 2点間の距離(m) 00142 printf("distance=%f\ne=%lf\nrr=%lf\n",distance,e,rr); 00143 if (distance<5){ 00144 }else{ 00145 pc.printf("(a =%lf,b =%lf)\n\r",a,b); 00146 pc.printf("(c =%lf,d =%lf)\n\r",gps.latitude,gps.longitude);//c,dを表示 00147 pc.printf("5m clear!"); 00148 xbee.printf("5m clear!"); 00149 break; 00150 } 00151 } 00152 }else{ 00153 printf("No data"); 00154 } 00155 } //GPS End */ 00156 00157 int i=1; 00158 float th; 00159 Timer tm; 00160 for(i=0;i<3;i++){ 00161 pc.printf("start\r\n"); 00162 00163 left1 = 100; //左モーター100% 00164 right1 = 100;//右モーター100% 00165 00166 printf("Restart\r\n" ); 00167 00168 wait(4); 00169 left1=50; 00170 right1=50; 00171 wait(1); 00172 left1=0; 00173 right1=0; 00174 wait(1); 00175 00176 printf("停止\n\r"); 00177 00178 Sb612switch=0; //焦電off 00179 wait(1); 00180 Ultra=1;//超音波on 00181 wait(1); 00182 00183 while(1) { 00184 left1 = 0; 00185 right1 = 0; 00186 00187 printf("超音波on\r\n 焦電off\r\n" ) ; 00188 00189 hs.TrigerOut(); 00190 wait(1); 00191 int distance; 00192 distance = hs.GetDistance(); 00193 00194 printf("distance=%dmm\r\n",distance);//距離出力 00195 00196 if(distance<=2000){//超音波反応 00197 Ultra=0;//超音波off 00198 wait(1); 00199 Sb612switch=1; //焦電on 00200 wait(1); 00201 00202 printf("焦電On!\r\n "); 00203 bool detected=false; 00204 th = thermo; 00205 if(th==1 && !detected) {//焦電反応ありの場合 00206 i++; 00207 detected=true; 00208 xbee.printf("human\r\n"); 00209 tm.reset(); 00210 tm.start(); 00211 00212 LocalFileSystem local("local"); 00213 Timer timer; 00214 timer.start(); 00215 camera.setPictureSize(JPEGCamera::SIZE320x240); 00216 00217 FILE *fp; 00218 base64 *bs; 00219 int c; 00220 for (int r = 0; r < 1; r++) { 00221 if (camera.isReady()) { 00222 char filename[25]; 00223 snprintf(filename,25,"%s%03d%s","/local/pict",r,".jpg"); 00224 printf("Picture: %s ", filename); 00225 if (camera.takePicture(filename)) { 00226 while (camera.isProcessing()) { 00227 camera.processPicture(); 00228 00229 xbee.printf("take pictuer!"); 00230 00231 xbee.printf("xbee connected!!\r\n"); 00232 00233 bs = new base64(); 00234 bs->Encode(filename,"/local/data.txt"); 00235 00236 printf("time = %f\n", timer.read()); 00237 00238 00239 if((fp=fopen("/local/d.txt","r"))!=NULL){ 00240 pc.printf("ok\r\n"); 00241 while((c=fgetc(fp))!=EOF){ 00242 xbee.printf("%c",c); 00243 } 00244 fclose(fp); 00245 } 00246 } 00247 }else{ 00248 printf("take picture failed\r\n"); 00249 } 00250 }else{ 00251 printf("camera is not ready\r\n"); 00252 } 00253 while(1){ 00254 00255 int received_data = xbee.getc(); 00256 00257 if (received_data == 82 || received_data == 114){ //Rまたはr 00258 00259 xbee.printf("_________________________________________________________________________________________________________________________________\r\n"); 00260 if((fp=fopen("/local/d.txt","r"))!=NULL) { 00261 while ((c=fgetc(fp))!=EOF){ 00262 00263 xbee.printf("%c",c); //再送 00264 } 00265 fclose(fp); 00266 } 00267 }else{ 00268 break; 00269 } 00270 } 00271 00272 Sb612switch=0; //焦電off 00273 wait(1); 00274 } 00275 }else{//焦電反応なしの場合 00276 00277 00278 printf("not found!\r\n"); 00279 00280 00281 Sb612switch=0; 00282 wait(1); 00283 Ultra=0; 00284 wait(1); 00285 detected=false; 00286 printf("後退\r\n"); 00287 left1 = -100; //左モーター-50% 00288 right1 = -100;//右モーター-50% 00289 wait(2.0); 00290 left1=-50; 00291 right1=-50; 00292 wait(1); 00293 left1=0; 00294 right1=0; 00295 wait(1); 00296 00297 printf("右折\n\r"); 00298 00299 left1 = 60; //左モーター100% 00300 right1 = 100;//右モーター100% 00301 wait(2.0); 00302 } 00303 }else{//超音波distance>2000 00304 printf("safety zone\r\n"); 00305 Ultra=0; 00306 wait(1); 00307 left1 = 60; //左モーター50% 00308 right1 = 100;//右モーター50% 00309 printf("右折\r\n"); 00310 wait(3); 00311 left1 = 100; 00312 right1 = 100; 00313 wait(5); 00314 left1 = 0; 00315 right1 = 0; 00316 wait(5); 00317 } 00318 00319 } 00320 00321 } 00322 }
Generated on Mon Jul 18 2022 09:03:41 by
1.7.2