cansat_B 2019
/
CanSatB2019_main_1208
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
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 00020 DigitalOut FET(p21); //FET 00021 DigitalOut myled(LED1); 00022 US015 hs(p12,p11); 00023 DigitalIn thermo(p20); 00024 DigitalOut Sb612switch(p15); //焦電スイッチ 00025 DigitalOut Ultra(p12); 00026 Serial pc(USBTX,USBRX); // tx, rx 00027 JPEGCamera camera(p9, p10); // TX, RX 00028 TB6612 left1(p25,p17,p16); //モーターピン 00029 TB6612 right1(p26,p19,p18); //モーターピン 00030 Serial xbee(p13,p14); //xbee 00031 00032 00033 00034 00035 int main() 00036 { 00037 00038 00039 Sb612switch=0; //焦電off 00040 wait(1); 00041 Ultra=0;//超音波off 00042 00043 printf("CanSat-B_Start!\r\n"); 00044 00045 //FET 00046 00047 FET=1; 00048 wait(10); 00049 FET=0; 00050 wait(10); 00051 00052 00053 00054 //以下GPS 00055 double a; 00056 double b; 00057 double distance; 00058 int k = 0; 00059 int j = 0; 00060 00061 pc.printf("GPS Start\r\n"); 00062 xbee.printf("s\n"); 00063 if(gps.getgps()){ 00064 00065 pc.printf("(%lf,%lf)\r\n",gps.latitude,gps.longitude);//緯度と経度を表示 00066 i ++; 00067 if(k<59){ 00068 }else{ 00069 a = gps.latitude; 00070 b = gps.longitude; 00071 printf("(a =%lf,b =%lf)\r\n",gps.latitude,gps.longitude);//a,bを表示 00072 break; 00073 } 00074 }else{ 00075 pc.printf("NO DATA\r\n");//データ取得失敗 00076 wait(1); 00077 } 00078 } 00079 printf("moter on"); 00080 left = 100; //左モーター１００％ 00081 right = 100;//右モーター１００％ 00082 00083 wait(30); 00084 printf("moter off"); 00085 left = 0; //左モーター１０％ 00086 right = 0;//右モーター１０％(左折) 00087 printf("moter off"); 00088 wait(15); 00089 printf("GPS restart\n"); 00090 00091 while(1) { 00092 if(gps.getgps()){ 00093 00094 pc.printf("(%lf,%lf)\r\n",gps.latitude,gps.longitude);//緯度と経度を表示 00095 j ++; 00096 if(j<29){ 00097 }else{ 00098 // 球面三角法により、大円距離(メートル)を求める 00099 double c; 00100 double d; 00101 c = gps.latitude; 00102 d = gps.longitude; 00103 00104 pc.printf("(%lf,%lf)\n\r",gps.latitude,gps.longitude);//c,dを表示 00105 const double pi = 3.14159265359; // 円周率 00106 00107 double ra = a * pi / 180; 00108 double rb = b * pi / 180; // 緯度経度をラジアンに変換 00109 double rc = c * pi / 180; 00110 double rd = d * pi / 180; 00111 00112 double e = sin(ra) * sin(rc) + cos(ra) * cos(rc) * cos(rb - rd); // 2点の中心角(ラジアン)を求める 00113 double rr = acos(e); 00114 00115 const double earth_radius = 6378140; // 地球赤道半径(m) 00116 00117 distance = earth_radius * rr; // 2点間の距離(m) 00118 00119 if (distance<5){ 00120 }else{ 00121 pc.printf("(c =%lf,d =%lf)\n\r",gps.latitude,gps.longitude);//c,dを表示 00122 pc.printf("5m clear!"); 00123 xbee.printf("5m clear!"); 00124 break; 00125 } 00126 } 00127 }else{ 00128 printf("No data"); 00129 } 00130 } //GPS End 00131 00132 int i=1; 00133 float th; 00134 Timer tm; 00135 for(i=0;i<3;i++){ 00136 pc.printf("start\r\n"); 00137 00138 left1 = 100; //左モーター１００％ 00139 right1 = 100;//右モーター１００％ 00140 printf("Restart\r\n" ); 00141 wait(4); 00142 left1=50; 00143 right1=50; 00144 wait(1); 00145 left1=0; 00146 right1=0; 00147 wait(1); 00148 printf("停止\n\r"); 00149 Sb612switch=0; //焦電off 00150 wait(1); 00151 Ultra=1;//超音波on 00152 wait(1); 00153 00154 while(1) { 00155 printf("超音波on\r\n 焦電off\r\n" ) ; 00156 hs.TrigerOut(); 00157 wait(1); 00158 int distance; 00159 distance = hs.GetDistance(); 00160 printf("distance=%dmm\r\n",distance);//距離出力 00161 00162 if(distance<=2000){//超音波反応 00163 00164 Ultra=0;//超音波off 00165 wait(1); 00166 Sb612switch=1; //焦電on 00167 wait(1); 00168 printf("焦電On!\r\n "); 00169 bool detected=false; 00170 th = thermo; 00171 if(th==1 && !detected) {//焦電反応ありの場合 00172 i++; 00173 detected=true; 00174 pc.printf("human\r\n"); 00175 tm.reset(); 00176 tm.start(); 00177 00178 LocalFileSystem local("local"); 00179 Timer timer; 00180 timer.start(); 00181 camera.setPictureSize(JPEGCamera::SIZE320x240); 00182 00183 FILE *fp; 00184 base64 *bs; 00185 int c; 00186 00187 for (int r = 0; r < 1; r++) { 00188 if (camera.isReady()) { 00189 char filename[32]; 00190 sprintf(filename, "/local/pict%03d.jpg",r); 00191 printf("Picture: %s ", filename); 00192 if (camera.takePicture(filename)) { 00193 while (camera.isProcessing()) { 00194 camera.processPicture(); 00195 printf("take pictuer!"); 00196 } 00197 }else{ 00198 printf("take picture failed\r\n"); 00199 } 00200 }else{ 00201 printf("camera is not ready\r\n"); 00202 } 00203 00204 printf("time = %f\n", timer.read()); 00205 00206 00207 00208 xbee.printf("xbee connected!\r\n"); 00209 bs = new base64(); 00210 bs->Encode("/local/pict000.jpg","/local/data000.txt"); 00211 00212 if((fp=fopen("/local/data000.txt","r"))!=NULL) 00213 { 00214 pc.printf("ok\r\n"); 00215 while((c=fgetc(fp))!=EOF){ 00216 xbee.printf("%c",c); 00217 } 00218 fclose(fp); 00219 } 00220 } 00221 while(1){ 00222 00223 int received_data = xbee.getc(); 00224 00225 if (received_data == 82 || received_data == 114){ //Rまたはｒ 00226 xbee.printf("_________________________________________________________________________________________________________________________________\r\n"); 00227 if((fp=fopen("/local/data000.txt","r"))!=NULL) 00228 { 00229 while ((c=fgetc(fp))!=EOF){ 00230 xbee.printf("%c",c); //再送 00231 } 00232 fclose(fp); 00233 } 00234 } 00235 else{ 00236 break; 00237 } 00238 } 00239 00240 Sb612switch=0; //焦電off 00241 wait(1); 00242 }else{//焦電反応なしの場合 00243 printf("not found!\r\n"); 00244 00245 00246 Sb612switch=0; 00247 wait(1); 00248 Ultra=0; 00249 wait(1); 00250 detected=false; 00251 printf("後退\r\n"); 00252 left1 = -100; //左モーター-5０％ 00253 right1 = -100;//右モーター-5０％ 00254 wait(2.0); 00255 left1=-50; 00256 right1=-50; 00257 wait(1); 00258 left1=0; 00259 right1=0; 00260 wait(1); 00261 00262 printf("右折\n\r"); 00263 00264 left1 = 60; //左モーター１００％ 00265 right1 = 100;//右モーター１００％ 00266 wait(2.0); 00267 } 00268 }else{//超音波distance>2000 00269 printf("safety zone\r\n"); 00270 Ultra=0; 00271 wait(1); 00272 left1 = 60; //左モーター50％ 00273 right1 = 100;//右モーター50％ 00274 printf("右折\r\n"); 00275 } 00276 00277 } 00278 00279 } 00280 }
Generated on Tue Jul 12 2022 23:41:47 by
1.7.2