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