main.cpp

00001 #define cansatB
00002 #define min(x, y) ((x) < (y)) ? (x) : (y)
00003 #include "mbed.h"       //mbed
00004 #include "getGPS.h"         //GPS
00005 #include "math.h"         //GPS
00006 #include "TB6612.h"      //motorDriver
00007 #include "JPEGCamera.h"//カメラ
00008 #include "base64.h"//写真ーXBee
00009 #include "us015.h"  // 超音波センサ
00010 #include <stdio.h>
00011 US015 hs(p12,p11);                  //P12 :超音波センサ トリガ出力  //p11 :超音波センサ  エコー入力
00012 GPS gps (p28,p27);                 //GPS
00013 
00014 DigitalOut FET(p21);                //FET
00015 DigitalOut Ultra(p12);
00016 DigitalIn thermo(p20);     //焦電センサ↓
00017 DigitalOut led(LED1);     
00018 Serial pc(USBTX,USBRX); // tx, rx  焦電センサ↑
00019 TB6612 left(p25,p17,p16);//motor
00020 TB6612 right(p26,p19,p18);//motor
00021 
00022 int main() {            //FET
00023     FET = 0;
00024     SW = 1;  //p21をhigh(3.3V)にする。 
00025      while(1) {
00026     if(flight==1) {
00027         wait(10);
00028         }
00029         
00030     else{
00031         if(flight==1) {
00032         wait(10);
00033         }
00034         else{
00035         FET = 0;                       //FET、ニクロム線切断
00036         wait(20);
00037         FET = 1;
00038         wait(12);
00039         FET = 0;
00040         wait(10);
00041         FET = 1;
00042         wait(12);
00043         FET = 0; 
00044         SW = 0;  //p21をlow(0V)にする。 
00045     break;
00046             }
00047     }
00048     }
00049     }
00050 int main() {       //以下GPS
00051      double a;
00052      double b;
00053      double distance;
00054     
00055     pc.printf("GPS Start\r\n");
00056     
00057      while(1) {
00058          if(gps.getgps()){
00059            a = gps.latitude;
00060            b = gps.longitude;
00061            
00062           pc.printf("(%lf,%lf)\r\n",gps.latitude,gps.longitude);//緯度と経度を表示
00063            
00064            break;
00065            
00066          }else{
00067           pc.printf("NO DATA\r\n");//データ取得失敗
00068           wait(1);
00069             }
00070        }
00071       while(1){
00072          if(gps.getgps()) {
00073            
00074           pc.printf("(%lf,%lf)\n\r",gps.latitude,gps.longitude);//緯度と経度を表示   
00075           
00076        // 球面三角法により、大円距離(メートル)を求める
00077        double c;
00078        double d; 
00079         c = gps.latitude;
00080         d = gps.longitude;
00081 
00082          const double pi = 3.14159265359; // 円周率
00083                            
00084             double ra = a * pi / 180;
00085             double rb = b * pi / 180;     // 緯度経度をラジアンに変換
00086             double rc = c * pi / 180;
00087             double rd = d * pi / 180;
00088         
00089             double e = sin(ra) * sin(rc) +  cos(ra) * cos(rc) * cos(rb - rd);  // 2点の中心角(ラジアン)を求める
00090             double rr = acos(e);
00091 
00092             const double earth_radius = 6378140;   // 地球赤道半径(m)
00093 
00094             distance = earth_radius * rr; // 2点間の距離(m)
00095             
00096            
00097              
00098 
00099          if (distance<5){
00100              printf("%lf\r\n",distance);   
00101         left = 100; //左モーター100％
00102         right = 100;//右モーター100％
00103         printf("直進\n\r");
00104              }else{
00105              left = 0; 
00106              right = 0;
00107              printf("停止\n\r");
00108              pc.printf("5m clear!");
00109              break;
00110               }
00111           
00112           }else{
00113            pc.printf("NO DATA\r\n");//データ取得失敗
00114            wait(1);
00115            }
00116         }
00117         return 0;          //注意！void()に変えること.このままだとここで終わる
00118      }
00119       
00120     int main(){
00121   float th;
00122   Timer tm;
00123   pc.printf("start\r\n");
00124 
00125   bool detected=false;
00126     thermo=0; //焦電off
00127     Ultra=1;//超音波on
00128    
00129     while(1) {
00130          hs.TrigerOut();
00131          wait(1);
00132          int distance;
00133          distance = hs.GetDistance();
00134          printf("%d\r\n",distance);//距離出力
00135         
00136         if(distance<2000){//超音波反応
00137          Ultra=0;//超音波off
00138          thermo=1;//焦電on
00139          left = 0; //左モーター０％
00140         right = 0;//右モーター０％
00141         printf("停止\n\r");
00142           if(true)
00143              th = thermo;
00144              if(th=1 && !detected) {//焦電反応ありの場合
00145                detected=true;
00146              pc.printf("human\r\n");
00147              tm.reset();
00148              tm.start();
00149              thermo=0;
00150        const int RESPONSE_TIMEOUT = 500;
00151 const int DATA_TIMEOUT = 1000;
00152 
00153 JPEGCamera::JPEGCamera(PinName tx, PinName rx) : Serial(tx, rx) {
00154     printf("AA\r\n");
00155     baud(38400);
00156     state = READY;
00157 }
00158 
00159 bool JPEGCamera::setPictureSize(JPEGCamera::PictureSize size, bool doReset) {
00160     char buf[9] = {0x56, 0x00, 0x31, 0x05, 0x04, 0x01, 0x00, 0x19, (char) size};
00161     int ret = sendReceive(buf, sizeof buf, 5);
00162 
00163     if (ret == 5 && buf[0] == 0x76) {
00164         if (doReset)
00165             reset();
00166         return true;
00167     } else
00168         return false;
00169 }
00170 
00171 bool JPEGCamera::isReady() {
00172     return state == READY;
00173 }
00174 
00175 bool JPEGCamera::isProcessing() {
00176     return state == PROCESSING;
00177 }
00178 
00179 bool JPEGCamera::takePicture(char *filename) {
00180     if (state == READY) {
00181         fp = fopen(filename, "wb");
00182         if (fp != 0) {
00183             if (takePicture()) {
00184                 imageSize = getImageSize();
00185                 address = 0;
00186                 state = PROCESSING;
00187             } else {
00188                 fclose(fp);
00189                 printf("takePicture(%s) failed", filename);
00190                 state = ERROR;
00191             }
00192         } else {
00193             printf("fopen() failed");
00194             state = ERROR;
00195         }
00196     }
00197     return state != ERROR;
00198 }
00199 
00200 bool JPEGCamera::processPicture() {
00201     if (state == PROCESSING) {
00202         if (address < imageSize) {
00203             char data[1024];
00204             int size = readData(data, min(sizeof(data), imageSize - address), address);
00205             int ret = fwrite(data, size, 1, fp);
00206             if (ret > 0)
00207                 address += size;
00208             if (ret == 0 || address >= imageSize) {
00209                 stopPictures();
00210                 fclose(fp);
00211                 wait(0.1); // ????
00212                 state = ret > 0 ? READY : ERROR;
00213             }
00214         }
00215     }
00216 
00217     return state == PROCESSING || state == READY;
00218 }
00219 
00220 bool JPEGCamera::reset() {
00221     char buf[4] = {0x56, 0x00, 0x26, 0x00};
00222     int ret = sendReceive(buf, sizeof buf, 4);
00223     if (ret == 4 && buf[0] == 0x76) {
00224         wait(4.0);
00225         state = READY;
00226     } else {
00227         state = ERROR;
00228     }
00229     return state == READY;
00230 }
00231 
00232 bool JPEGCamera::takePicture() {
00233     char buf[5] = {0x56, 0x00, 0x36, 0x01, 0x00};
00234     int ret = sendReceive(buf, sizeof buf, 5);
00235 
00236     return ret == 5 && buf[0] == 0x76;
00237 }
00238 
00239 bool JPEGCamera::stopPictures() {
00240     char buf[5] = {0x56, 0x00, 0x36, 0x01, 0x03};
00241     int ret = sendReceive(buf, sizeof buf, 5);
00242 
00243     return ret == 4 && buf[0] == 0x76;
00244 }
00245 
00246 int JPEGCamera::getImageSize() {
00247     char buf[9] = {0x56, 0x00, 0x34, 0x01, 0x00};
00248     int ret = sendReceive(buf, sizeof buf, 9);
00249 
00250     //The size is in the last 2 characters of the response.
00251     return (ret == 9 && buf[0] == 0x76) ? (buf[7] << 8 | buf[8]) : 0;
00252 }
00253 
00254 int JPEGCamera::readData(char *dataBuf, int size, int address) {
00255     char buf[16] = {0x56, 0x00, 0x32, 0x0C, 0x00, 0x0A, 0x00, 0x00,
00256                     address >> 8, address & 255, 0x00, 0x00, size >> 8, size & 255, 0x00, 0x0A
00257                    };
00258     int ret = sendReceive(buf, sizeof buf, 5);
00259 
00260     return (ret == 5 && buf[0] == 0x76) ? receive(dataBuf, size, DATA_TIMEOUT) : 0;
00261 }
00262 
00263 int JPEGCamera::sendReceive(char *buf, int sendSize, int receiveSize) {
00264     while (readable()) getc();
00265 
00266     for (int i = 0; i < sendSize; i++) putc(buf[i]);
00267 
00268     return receive(buf, receiveSize, RESPONSE_TIMEOUT);
00269 }
00270 
00271 int JPEGCamera::receive(char *buf, int size, int timeout) {
00272     timer.start();
00273     timer.reset();
00274 
00275     int i = 0;
00276     while (i < size && timer.read_ms() < timeout) {
00277         if (readable())
00278             buf[i++] = getc();
00279     }
00280 
00281     return i;
00282 }
00283     LocalFileSystem local("local");
00284 
00285 Serial pc(USBTX,USBRX);
00286 Serial xbee(p13, p14);
00287 int main(void){
00288     FILE *fp;
00289     base64 *bs;
00290     int c;
00291     
00292     xbee.printf("charizard!!!\r\n");
00293     bs = new base64();
00294     bs->Encode("/local/PICT000.jpg","/local/d.txt");
00295     
00296     
00297    if((fp=fopen("/local/d.txt","r"))!=NULL)
00298    {
00299        while ((c=fgetc(fp))!=EOF){
00300            xbee.printf("%c",c);
00301        }
00302        fclose(fp);
00303    }
00304    return 0;
00305 }
00306 
00307             }else{//焦電反応なしの場合
00308              detected=false;
00309          thermo=0;
00310          Ultra=1;
00311              }
00312            
00313 }
00314   //while(true)
00315   }
00316   return 0;
00317   }
00318   
00319 //超音波センサー反応あり
00320 //停止
00321 //超音波センサーOFF
00322 //焦電センサーON
00323        
00324    //焦電センサー反応あり
00325    //焦電センサーOFF
00326    //カメラ起動
00327    //カメラOFF
00328    //XBeeによりPCへ送信
00329        if(receiveOK()=true){
00330            printf("Unknown Creature has been descovered!\n");
00331            }
00332        //"OK"受信、ミッション終了
00333        else if(receiveOK()=false){
00334            printf("Continue!\n");
00335            }
00336        //"NO"受信、ミッション再開
00337 
00338     else if(detected=false){
00339         thermo=0;
00340         left=100;
00341         wait(3.0);
00342         right=100;
00343         Ultra=1;
00344         }
00345     } 
00346     //焦電センサー反応無し
00347     //焦電センサーOFF
00348     //方向転換
00349     //超音波センサーON
00350     //直進
00351 }
00352 else if(distance>=2000){
00353     left=0;
00354     right=0;
00355     left=100;
00356     wait(3.0);
00357     right=100;
00358 }
00359 //超音波センサー反応無し
00360 //停止
00361 //方向転換
00362 //直進