Revision 4:1354e56c7dd3, committed 2019-11-20

Comitter:

YUPPY

Date:

Wed Nov 20 08:06:46 2019 +0000

Parent:

3:5d0c4b13f4e8

Commit message:

class_before_loading;

Changed in this revision

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/CameraUS015sb612-3.lib	Wed Nov 20 08:06:46 2019 +0000
@@ -0,0 +1,1 @@
+https://os.mbed.com/users/YUPPY/code/CameraUS015sb612-3/#5f9c4d16acf6

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/JPEGCamera.cpp	Wed Nov 20 08:06:46 2019 +0000
@@ -0,0 +1,144 @@
+/* Arduino JPEGCamera Library
+ * Copyright 2010 SparkFun Electronic
+ * Written by Ryan Owens
+ * Modified by arms22
+ * Ported to mbed by yamaguch
+ */
+
+#include "JPEGCamera.h"
+
+#define min(x, y) ((x) < (y)) ? (x) : (y)
+
+const int RESPONSE_TIMEOUT = 500;
+const int DATA_TIMEOUT = 1000;
+
+JPEGCamera::JPEGCamera(PinName tx, PinName rx) : Serial(tx, rx) {
+    printf("AA\r\n");
+    baud(38400);
+    state = READY;
+}
+
+bool JPEGCamera::setPictureSize(JPEGCamera::PictureSize size, bool doReset) {
+    char buf[9] = {0x56, 0x00, 0x31, 0x05, 0x04, 0x01, 0x00, 0x19, (char) size};
+    int ret = sendReceive(buf, sizeof buf, 5);
+
+    if (ret == 5 && buf[0] == 0x76) {
+        if (doReset)
+            reset();
+        return true;
+    } else
+        return false;
+}
+
+bool JPEGCamera::isReady() {
+    return state == READY;
+}
+
+bool JPEGCamera::isProcessing() {
+    return state == PROCESSING;
+}
+
+bool JPEGCamera::takePicture(char *filename) {
+    if (state == READY) {
+        fp = fopen(filename, "wb");
+        if (fp != 0) {
+            if (takePicture()) {
+                imageSize = getImageSize();
+                address = 0;
+                state = PROCESSING;
+            } else {
+                fclose(fp);
+                printf("takePicture(%s) failed", filename);
+                state = ERROR;
+            }
+        } else {
+            printf("fopen() failed");
+            state = ERROR;
+        }
+    }
+    return state != ERROR;
+}
+
+bool JPEGCamera::processPicture() {
+    if (state == PROCESSING) {
+        if (address < imageSize) {
+            char data[1024];
+            int size = readData(data, min(sizeof(data), imageSize - address), address);
+            int ret = fwrite(data, size, 1, fp);
+            if (ret > 0)
+                address += size;
+            if (ret == 0 || address >= imageSize) {
+                stopPictures();
+                fclose(fp);
+                wait(0.1); // ????
+                state = ret > 0 ? READY : ERROR;
+            }
+        }
+    }
+
+    return state == PROCESSING || state == READY;
+}
+
+bool JPEGCamera::reset() {
+    char buf[4] = {0x56, 0x00, 0x26, 0x00};
+    int ret = sendReceive(buf, sizeof buf, 4);
+    if (ret == 4 && buf[0] == 0x76) {
+        wait(4.0);
+        state = READY;
+    } else {
+        state = ERROR;
+    }
+    return state == READY;
+}
+
+bool JPEGCamera::takePicture() {
+    char buf[5] = {0x56, 0x00, 0x36, 0x01, 0x00};
+    int ret = sendReceive(buf, sizeof buf, 5);
+
+    return ret == 5 && buf[0] == 0x76;
+}
+
+bool JPEGCamera::stopPictures() {
+    char buf[5] = {0x56, 0x00, 0x36, 0x01, 0x03};
+    int ret = sendReceive(buf, sizeof buf, 5);
+
+    return ret == 4 && buf[0] == 0x76;
+}
+
+int JPEGCamera::getImageSize() {
+    char buf[9] = {0x56, 0x00, 0x34, 0x01, 0x00};
+    int ret = sendReceive(buf, sizeof buf, 9);
+
+    //The size is in the last 2 characters of the response.
+    return (ret == 9 && buf[0] == 0x76) ? (buf[7] << 8 | buf[8]) : 0;
+}
+
+int JPEGCamera::readData(char *dataBuf, int size, int address) {
+    char buf[16] = {0x56, 0x00, 0x32, 0x0C, 0x00, 0x0A, 0x00, 0x00,
+                    address >> 8, address & 255, 0x00, 0x00, size >> 8, size & 255, 0x00, 0x0A
+                   };
+    int ret = sendReceive(buf, sizeof buf, 5);
+
+    return (ret == 5 && buf[0] == 0x76) ? receive(dataBuf, size, DATA_TIMEOUT) : 0;
+}
+
+int JPEGCamera::sendReceive(char *buf, int sendSize, int receiveSize) {
+    while (readable()) getc();
+
+    for (int i = 0; i < sendSize; i++) putc(buf[i]);
+
+    return receive(buf, receiveSize, RESPONSE_TIMEOUT);
+}
+
+int JPEGCamera::receive(char *buf, int size, int timeout) {
+    timer.start();
+    timer.reset();
+
+    int i = 0;
+    while (i < size && timer.read_ms() < timeout) {
+        if (readable())
+            buf[i++] = getc();
+    }
+
+    return i;
+}
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/JPEGCamera.h	Wed Nov 20 08:06:46 2019 +0000
@@ -0,0 +1,122 @@
+
+/* Arduino JPEGCamera Library
+ * Copyright 2010 SparkFun Electronics
+ * Written by Ryan Owens
+ * Modified by arms22
+ * Ported to mbed by yamaguch
+ */
+
+#ifndef JPEG_CAMERA_H
+#define JPEG_CAMERA_H
+
+#include "mbed.h"
+
+/**
+ * Interface for LinkSprite JPEG Camera module LS-Y201
+ */
+class JPEGCamera : public Serial {
+public:
+    /***/
+    enum PictureSize {
+        SIZE160x120 = 0x22,
+        SIZE320x240 = 0x11,
+        SIZE640x480 = 0x00,
+    };
+
+    /**
+     * Create JPEG Camera
+     *
+     * @param tx tx pin
+     * @param rx rx pin
+     */
+    JPEGCamera(PinName tx, PinName rx);
+
+    /**
+     * Set picture size
+     *
+     * @param size picture size (available sizes are SIZE160x120, SIZE320x240, SIZE640x480)
+     * @param doReset flag to perform reset operation after changing size
+     *
+     * @returns true if succeeded, false otherwise
+     */
+    bool setPictureSize(JPEGCamera::PictureSize size, bool doReset = true);
+
+    /**
+     * Return whether camera is ready or not
+     *
+     * @returns true if ready, false otherwise
+     */
+    bool isReady();
+
+    /**
+     * Return whether camera is processing the taken picture or not
+     *
+     * @returns true if the camera is in processing, false otherwise
+     */
+    bool isProcessing();
+
+    /**
+     * Take a picture
+     *
+     * @param filename filename to store the picture data
+     * @returns true if succeeded, false otherwise
+     */
+    bool takePicture(char *filename);
+    /**
+     * Process picture (writing the file)
+     *
+     * @returns true if no error in processing, false otherwise
+     */
+    bool processPicture();
+
+    /**
+     * Perform reset oepration (it takes 4 seconds)
+     *
+     * @returns true if succeeded, false otherwise
+     */
+    bool reset();
+
+    /**
+     * Send a picture command to the camera module
+     *
+     *  @returns true if succeeded, false otherwise
+     */
+    bool takePicture(void);
+
+    /**
+     * Send a stop pictures command to the camera module
+     *
+     *  @returns true if succeeded, false otherwise
+     */
+    bool stopPictures(void);
+
+    /**
+     * Get the picture image size
+     *
+     * @returns the actual image size in bytes
+     */
+    int getImageSize();
+
+    /**
+     * Read the picture data to the buffer
+     *
+     * @param dataBuf data buffer address to store the received data
+     * @param size data size to read
+     * @param address the address of the picture data to read
+     *
+     * @returns the size of the data read
+     */
+    int readData(char *dataBuf, int size, int address);
+
+//private:
+    Timer timer;
+    FILE *fp;
+    int imageSize;
+    int address;
+    enum State {UNKNOWN, READY, PROCESSING, ERROR = -1} state;
+    
+    int sendReceive(char *buf, int sendSize, int receiveSize);
+    int receive(char *buf, int size, int timeout);
+};
+
+#endif

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/TB6612.cpp	Wed Nov 20 08:06:46 2019 +0000
@@ -0,0 +1,57 @@
+/**
+ * Motor Driver TB6612 Control Library
+ *
+ * -- TB6612 is a device of the TOSHIBA. 
+ *
+ * Copyright (C) 2012 Junichi Katsu (JKSOFT) 
+ */
+ 
+ 
+#include "TB6612.h"
+ 
+// TB6612 Class Constructor
+TB6612::TB6612(PinName pwm, PinName fwd, PinName rev):
+        _pwm(pwm), _fwd(fwd), _rev(rev) {
+ 
+    _fwd = 0;
+    _rev = 0;
+    _pwm = 0.0;
+    _pwm.period(0.001);
+}
+ 
+// Speed Control
+//  arg
+//   int speed -100 -- 0 -- 100
+void TB6612::speed(int speed) {
+        
+    if( speed > 0 )
+    {
+        _pwm = ((float)speed) / 100.0;
+        _fwd = 1;
+        _rev = 0;
+    }
+    else if( speed < 0 )
+    {
+        _pwm = -((float)speed) / 100.0;
+        _fwd = 0;
+        _rev = 1;
+    }
+    else
+    {
+        _fwd = 1;
+        _rev = 1;
+    }
+}
+ 
+ 
+// Speed Control with time-out
+//  arg
+//   int speed -100 -- 0 -- 100
+//   int time  0
+void TB6612::move(int sspeed , int time)
+{
+    speed(sspeed);
+    wait_ms(time);
+}
+ 
+            
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/base64.cpp	Wed Nov 20 08:06:46 2019 +0000
@@ -0,0 +1,180 @@
+#include "base64.h"
+
+const char *base64::szB64 = "ABCDEFGHIJKLMNOPQRSTUVWXYZabcdefghijklmnopqrstuvwxyz0123456789+/=";
+
+base64::base64() : lpszOutputString(NULL)
+{
+}
+
+base64::~base64()
+{
+    if(lpszOutputString)
+        delete[] lpszOutputString;
+}
+
+const char *base64::Encode(const char *szStr, int iLens)
+{
+    int i,j;
+
+    if(lpszOutputString)
+        delete[] lpszOutputString;
+
+    if(iLens == -1)
+        iLen = strlen(szStr);
+    else
+        iLen = iLens;
+
+    lpszOutputString =
+        new char[(int)((double)(iLen)*1.5)+10];
+    for(i = 0,j = 0; i < (iLen - (iLen % 3)); i+=3)
+    {
+        lpszOutputString[j] = szB64[(szStr[i] & 0xfc) >> 2];
+        lpszOutputString[j+1] = szB64[((szStr[i] &0x03) << 4) |
+                                    ((szStr[i+1] & 0xf0) >> 4)];
+        lpszOutputString[j+2] = szB64[((szStr[i+1] & 0x0f) <<2 ) |
+                                    ((szStr[i+2] & 0xc0) >> 6)];
+        lpszOutputString[j+3] = szB64[(szStr[i+2] & 0x3f)];
+        j += 4;
+    }
+    i = iLen-(iLen % 3);    // 残りのサイズを計算
+    switch(iLen % 3)
+    {
+    case 2: // 1文字分パディングが必要
+    {
+        lpszOutputString[j] = szB64[(szStr[i] & 0xfc) >> 2];
+        lpszOutputString[j+1] = szB64[((szStr[i] &0x03) << 4) |
+                                    ((szStr[i+1] & 0xf0) >> 4)];
+        lpszOutputString[j+2] = szB64[((szStr[i+1] & 0x0f) <<2 )];
+        lpszOutputString[j+3] = szB64[64];  // Pad
+        lpszOutputString[j+4] = '\0';
+    }
+    break;
+    case 1: // 2文字分パディングが必要
+    {
+        lpszOutputString[j] = szB64[(szStr[i] & 0xfc) >> 2];
+        lpszOutputString[j+1] = szB64[((szStr[i] &0x03) << 4)];
+        lpszOutputString[j+2] = szB64[64];  // Pad
+        lpszOutputString[j+3] = szB64[64];  // Pad
+        lpszOutputString[j+4] = '\0';
+    }
+    break;
+    }
+    lpszOutputString[j+4] = '\0';
+
+    return lpszOutputString;
+}
+
+void base64::Encode(istream& istr, ostream& ostr, int iRet)
+{
+    int i;
+    char c[3];
+
+    i = 0;
+    while(!istr.eof())
+    {
+        c[0] = c[1] = c[2] = '\0';
+        istr.read(c,3);
+
+        ostr << szB64[(c[0] & 0xfc) >> 2];
+        i++;    if(i >= iRet && iRet != -1){ ostr << endl; i = 0; }
+        ostr << szB64[((c[0] &0x03) << 4) | ((c[1] & 0xf0) >> 4)];
+        i++;    if(i >= iRet && iRet != -1){ ostr << endl; i = 0; }
+        if(istr.gcount() == 1)
+            ostr << szB64[64];
+        else
+            ostr << szB64[((c[1] & 0x0f) <<2 ) | ((c[2] & 0xc0) >> 6)];
+        i++;    if(i >= iRet && iRet != -1){ ostr << endl; i = 0; }
+        if(istr.gcount() == 3)
+            ostr << szB64[(c[2] & 0x3f)];
+        else
+            ostr << szB64[64];
+        i++;    if(i >= iRet && iRet != -1){ ostr << endl; i = 0; }
+    }
+    ostr.flush();
+}
+
+
+int base64::FindIndexInB64(char c)
+{
+    int index = 0;
+    while(szB64[index] != '\0' && szB64[index] != c)
+        index++;
+
+    if(szB64[index] == '\0')
+        return -1;  // 見つからず
+
+    return index;   // 見つかった。
+}
+
+const char *base64::Decode(const char *szStr)
+{
+    //lpszOutputString
+    int i, j, l,iWriteCount,len;
+    char c;
+    char buf[4];
+
+    len = iLen = strlen(szStr);
+    if(lpszOutputString)
+        delete[] lpszOutputString;
+
+    iLen = (int)(((double)(iLen)/4.0)*3.0) + 4;
+
+    lpszOutputString = new char[iLen];
+
+    for(i = 0, j = 0; i < len; i+=4)
+    {
+        iWriteCount = 3;
+        for(l = 0; l < 4 && i+l<len; l++)
+        {
+            c = szStr[i+l];
+            if(c == szB64[64])  // Pad
+                iWriteCount--;
+            else {
+                buf[l] = FindIndexInB64(c);
+                //if(buf[l] == -1) error!;
+            }
+        }
+        lpszOutputString[j] = ((buf[0] << 2) & 0xfc) | ((buf[1] >> 4) & 0x03);
+        if(iWriteCount >= 2)
+            lpszOutputString[j+1] = ((buf[1] << 4) & 0xf0) | ((buf[2] >> 2) & 0x0f);
+        if(iWriteCount == 3)
+            lpszOutputString[j+2] = ((buf[2] << 6) & 0xc0) | (buf[3] & 0x3f);
+        j+=iWriteCount;
+    }
+    iLen = j;
+    lpszOutputString[j] = '\0';
+
+    return lpszOutputString;
+}
+
+void base64::Decode(istream& istr, ostream& ostr)
+{
+    int i,iWriteCount;
+    char c;
+    char buf[4];
+    char out[3];
+
+    while(1)
+    {
+        iWriteCount = 3;
+        for(i = 0; i < 4; i++)
+        {
+            istr >> c;
+            if(istr.eof())
+            {
+                ostr.flush();
+                return;
+            }
+            if(c == szB64[64])  // Pad
+                iWriteCount--;
+            else {
+                buf[i] = FindIndexInB64(c);
+                //if(buf[i] == -1) error!;
+            }
+        }
+        out[0] = ((buf[0] << 2) & 0xfc) | ((buf[1] >> 4) & 0x03);
+        out[1] = ((buf[1] << 4) & 0xf0) | ((buf[2] >> 2) & 0x0f);
+        out[2] = ((buf[2] << 6) & 0xc0) | (buf[3] & 0x3f);
+        ostr.write(out, iWriteCount);
+    }
+}

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/base64.h	Wed Nov 20 08:06:46 2019 +0000
@@ -0,0 +1,45 @@
+#if !defined BASE64_H_INC_
+#define BASE64_H_INC_
+
+#include <string.h>
+#include <iostream>
+#include <fstream>
+
+using namespace std;
+
+// Base64 Encodeing
+class base64
+{
+    int iLen;
+    char *lpszOutputString; // 結果出力先
+    static const char *szB64; // Base64変換テーブル
+
+    static int FindIndexInB64(char c);//szB64の Base64変換テーブルのなかの、どれか、探し出す。ない場合、-1
+public:
+    base64();
+    ~base64();
+    const char *Encode(const char *szStr, int iLens = -1);  // エンコード(文字列用)。ただし、データの長さを指定すれば、バイナリデータが可能
+    const char *Decode(const char *szStr);  // デコード(文字列用)
+    int GetLenght() const { return iLen; }  // 出力された長さ
+    const char *Get() const { return lpszOutputString; }    // 文字列用の結果を取得
+
+    static void Encode(istream& istr, ostream& ostr, int iRet = 76);    // istrはバイナリストリーム推奨
+        // エンコード(ストリーム用) iRetは、何文字目で改行するか。-1なら、改行しない。
+    static void Decode(istream& istr, ostream& ostr);   // ostrはバイナリストリーム推奨
+        // デコード(ストリーム用)
+
+    static void Encode(const char *lpszImputFileName, const char *lpszOutputFileName, int iRet = 76)
+    {
+        ifstream f(lpszImputFileName,ios::in | ios::binary);
+        ofstream of(lpszOutputFileName);
+        Encode(f,of);
+    }
+    static void Decode(const char *lpszImputFileName, const char *lpszOutputFileName)
+    {
+        ifstream f(lpszImputFileName);
+        ofstream of(lpszOutputFileName,ios::out | ios::trunc | ios::binary);
+        Decode(f,of);
+    }
+};
+
+#endif // #if !defined BASE64_H_INC_

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/getGPS.cpp	Wed Nov 20 08:06:46 2019 +0000
@@ -0,0 +1,58 @@
+#include "mbed.h"
+#include "getGPS.h"
+ 
+GPS::GPS(PinName gpstx,PinName gpsrx): _gps(gpstx,gpsrx)
+{
+    latitude = 0;
+    longitude = 0;
+    _gps.baud(GPSBAUD);
+    _gps.printf("$PMTK314,0,0,0,1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0*29");
+}
+ 
+bool GPS::getgps()
+{
+    char gps_data[256];
+    int i;
+    
+    do {
+        while(_gps.getc() != '$'); //$マークまで読み飛ばし
+        i = 0;
+ 
+        /* gpa_data初期化 */
+        for(int j = 0; j < 256; j++)
+            gps_data[j] = '\0';
+ 
+        /* NMEAから一行読み込み */
+        while((gps_data[i] = _gps.getc()) != '\r') {
+            i++;
+            if(i == 256) {
+                i = 255;
+                break;
+            }
+        }
+    } while(strstr(gps_data, "GPGGA") == NULL); //GGAセンテンスまで一行ずつ読み込み続ける
+    
+    int rlock;
+    char ns,ew;
+    double w_time, raw_longitude, raw_latitude;
+    int satnum;
+    double hdop;
+ 
+    if(sscanf(gps_data, "GPGGA, %lf, %lf, %c, %lf, %c, %d, %d, %lf", &w_time, &raw_latitude, &ns, &raw_longitude, &ew, &rlock, &satnum, &hdop) > 1) {
+        /* 座標1（度部分） */
+        double latitude_dd = (double)(raw_latitude / 100);
+        double longitude_dd = (double)(raw_longitude / 100);
+ 
+        /* 座標2（分部分 → 度） */
+        double latitude_md = (raw_latitude - latitude_dd * 100) / 60;
+        double longitude_md = (raw_longitude - longitude_dd * 100) / 60;
+ 
+        /* 座標1 + 2 */
+        latitude = latitude_dd + latitude_md;
+        longitude = longitude_dd + longitude_md;
+ 
+        return true;
+    } else
+        return false; //GGAセンテンスの情報が欠けている時
+}
+ 
\ No newline at end of file

--- a/main.cpp	Fri Nov 08 19:48:16 2019 +0000
+++ b/main.cpp	Wed Nov 20 08:06:46 2019 +0000
@@ -1,28 +1,23 @@
 #define cansatB
+#define min(x, y) ((x) < (y)) ? (x) : (y)
 #include "mbed.h"       //mbed
 #include "getGPS.h"         //GPS
 #include "math.h"         //GPS
 #include "TB6612.h"      //motorDriver
-#include "XBee.h"        //XBee
-#include <SoftwareSerial.h>   //カメラ
-#include <SD.h>//SDカード
-#include <JPEGCamera.h>//カメラ
+#include "JPEGCamera.h"//カメラ
+#include "base64.h"//写真ーXBee
 #include "us015.h"  // 超音波センサ
 #include <stdio.h>
 US015 hs(p12,p11);                  //P12 :超音波センサ トリガ出力  //p11 :超音波センサ  エコー入力
-Serial pc(USBTX,USBRX);            //GPs
 GPS gps (p28,p27);                 //GPS
-PwmOut motorSpeedR(p26);  //モーター
-PwmOut motorSpeedL(p25);  //モーター
-DigitalIn flight(p22,p23);      //フライトピン
+
 DigitalOut FET(p21);                //FET
-XBee xbee(p13, p14);               // XBee
+DigitalOut Ultra(p12);
 DigitalIn thermo(p20);     //焦電センサ↓
 DigitalOut led(LED1);     
-Serial pc(USBTX, USBRX); // tx, rx  焦電センサ↑
-Serial pc (p9);     //カメラ
-TB6612 left(p25,p17,p16);
-TB6612 right(p26,p19,p18);
+Serial pc(USBTX,USBRX); // tx, rx  焦電センサ↑
+TB6612 left(p25,p17,p16);//motor
+TB6612 right(p26,p19,p18);//motor
 
 int main() {            //FET
     FET = 0;
@@ -52,7 +47,6 @@
     }
     }
     }
-
 int main() {       //以下GPS
      double a;
      double b;
@@ -104,7 +98,13 @@
 
          if (distance<5){
              printf("%lf\r\n",distance);   
+        left = 100; //左モーター100％
+        right = 100;//右モーター100％
+        printf("直進\n\r");
              }else{
+             left = 0; 
+             right = 0;
+             printf("停止\n\r");
              pc.printf("5m clear!");
              break;
               }
@@ -117,64 +117,215 @@
         return 0;          //注意！void()に変えること.このままだとここで終わる
      }
       
-int main() {
+    int main(){
+  float th;
+  Timer tm;
+  pc.printf("start\r\n");
+
+  bool detected=false;
+    thermo=0; //焦電off
+    Ultra=1;//超音波on
+   
     while(1) {
-        left = 100; //左モーター１００％
-        right = 100;//右モーター１００％
-        wait(1.0);
-        left = 30;//左３０％
-        right = 30;//右３０％
-        wait(1.0);
-        printf("OK");
+         hs.TrigerOut();
+         wait(1);
+         int distance;
+         distance = hs.GetDistance();
+         printf("%d\r\n",distance);//距離出力
+        
+        if(distance<2000){//超音波反応
+         Ultra=0;//超音波off
+         thermo=1;//焦電on
+         left = 0; //左モーター０％
+        right = 0;//右モーター０％
+        printf("停止\n\r");
+          if(true)
+             th = thermo;
+             if(th=1 && !detected) {//焦電反応ありの場合
+               detected=true;
+             pc.printf("human\r\n");
+             tm.reset();
+             tm.start();
+             thermo=0;
+       const int RESPONSE_TIMEOUT = 500;
+const int DATA_TIMEOUT = 1000;
+
+JPEGCamera::JPEGCamera(PinName tx, PinName rx) : Serial(tx, rx) {
+    printf("AA\r\n");
+    baud(38400);
+    state = READY;
+}
+
+bool JPEGCamera::setPictureSize(JPEGCamera::PictureSize size, bool doReset) {
+    char buf[9] = {0x56, 0x00, 0x31, 0x05, 0x04, 0x01, 0x00, 0x19, (char) size};
+    int ret = sendReceive(buf, sizeof buf, 5);
+
+    if (ret == 5 && buf[0] == 0x76) {
+        if (doReset)
+            reset();
+        return true;
+    } else
+        return false;
+}
+
+bool JPEGCamera::isReady() {
+    return state == READY;
+}
+
+bool JPEGCamera::isProcessing() {
+    return state == PROCESSING;
+}
+
+bool JPEGCamera::takePicture(char *filename) {
+    if (state == READY) {
+        fp = fopen(filename, "wb");
+        if (fp != 0) {
+            if (takePicture()) {
+                imageSize = getImageSize();
+                address = 0;
+                state = PROCESSING;
+            } else {
+                fclose(fp);
+                printf("takePicture(%s) failed", filename);
+                state = ERROR;
+            }
+        } else {
+            printf("fopen() failed");
+            state = ERROR;
+        }
     }
+    return state != ERROR;
+}
+
+bool JPEGCamera::processPicture() {
+    if (state == PROCESSING) {
+        if (address < imageSize) {
+            char data[1024];
+            int size = readData(data, min(sizeof(data), imageSize - address), address);
+            int ret = fwrite(data, size, 1, fp);
+            if (ret > 0)
+                address += size;
+            if (ret == 0 || address >= imageSize) {
+                stopPictures();
+                fclose(fp);
+                wait(0.1); // ????
+                state = ret > 0 ? READY : ERROR;
+            }
+        }
+    }
+
+    return state == PROCESSING || state == READY;
 }
 
-if(distance<2000){
-    motorStopR();
-    motorStopL();
-    stopUS015();
-    startsb612a();
+bool JPEGCamera::reset() {
+    char buf[4] = {0x56, 0x00, 0x26, 0x00};
+    int ret = sendReceive(buf, sizeof buf, 4);
+    if (ret == 4 && buf[0] == 0x76) {
+        wait(4.0);
+        state = READY;
+    } else {
+        state = ERROR;
+    }
+    return state == READY;
+}
+
+bool JPEGCamera::takePicture() {
+    char buf[5] = {0x56, 0x00, 0x36, 0x01, 0x00};
+    int ret = sendReceive(buf, sizeof buf, 5);
+
+    return ret == 5 && buf[0] == 0x76;
+}
+
+bool JPEGCamera::stopPictures() {
+    char buf[5] = {0x56, 0x00, 0x36, 0x01, 0x03};
+    int ret = sendReceive(buf, sizeof buf, 5);
+
+    return ret == 4 && buf[0] == 0x76;
+}
+
+int JPEGCamera::getImageSize() {
+    char buf[9] = {0x56, 0x00, 0x34, 0x01, 0x00};
+    int ret = sendReceive(buf, sizeof buf, 9);
+
+    //The size is in the last 2 characters of the response.
+    return (ret == 9 && buf[0] == 0x76) ? (buf[7] << 8 | buf[8]) : 0;
+}
+
+int JPEGCamera::readData(char *dataBuf, int size, int address) {
+    char buf[16] = {0x56, 0x00, 0x32, 0x0C, 0x00, 0x0A, 0x00, 0x00,
+                    address >> 8, address & 255, 0x00, 0x00, size >> 8, size & 255, 0x00, 0x0A
+                   };
+    int ret = sendReceive(buf, sizeof buf, 5);
+
+    return (ret == 5 && buf[0] == 0x76) ? receive(dataBuf, size, DATA_TIMEOUT) : 0;
+}
+
+int JPEGCamera::sendReceive(char *buf, int sendSize, int receiveSize) {
+    while (readable()) getc();
+
+    for (int i = 0; i < sendSize; i++) putc(buf[i]);
+
+    return receive(buf, receiveSize, RESPONSE_TIMEOUT);
+}
+
+int JPEGCamera::receive(char *buf, int size, int timeout) {
+    timer.start();
+    timer.reset();
+
+    int i = 0;
+    while (i < size && timer.read_ms() < timeout) {
+        if (readable())
+            buf[i++] = getc();
+    }
+
+    return i;
+}
+    LocalFileSystem local("local");
+
+Serial pc(USBTX,USBRX);
+Serial xbee(p13, p14);
+int main(void){
+    FILE *fp;
+    base64 *bs;
+    int c;
+    
+    xbee.printf("charizard!!!\r\n");
+    bs = new base64();
+    bs->Encode("/local/PICT000.jpg","/local/d.txt");
+    
+    
+   if((fp=fopen("/local/d.txt","r"))!=NULL)
+   {
+       while ((c=fgetc(fp))!=EOF){
+           xbee.printf("%c",c);
+       }
+       fclose(fp);
+   }
+   return 0;
+}
+
+            }else{//焦電反応なしの場合
+             detected=false;
+         thermo=0;
+         Ultra=1;
+             }
+           
+}
+  //while(true)
+  }
+  return 0;
+  }
+  
 //超音波センサー反応あり
 //停止
 //超音波センサーOFF
 //焦電センサーON
-  int main()
-{
-  float th;
-  Timer tm;
-  pc.printf("start\r\n");
-  led=0;
-  bool detected=false;
-  while(true)
-  {
-    th = thermo;
-    if(th==1 && !detected) {
-      led = 1;
-      detected=true;
-      pc.printf("human\r\n");
-      tm.reset();
-      tm.start();
-    }
-    if(tm.read_ms()>10000) {
-      printf("Time out!\r\n");
-      led = 0;
-      detected=false;
-    }
-  }
-}
-   if(detected=true){
-       stopsb612a();
-       startCamera();
-       stopCamera();
-       sendSD();
-       sendPC();
        
    //焦電センサー反応あり
    //焦電センサーOFF
    //カメラ起動
    //カメラOFF
-   //データをSDカードに保存
-   //保存データをXBeeによりPCへ送信
+   //XBeeによりPCへ送信
        if(receiveOK()=true){
            printf("Unknown Creature has been descovered!\n");
            }
@@ -185,12 +336,11 @@
        //"NO"受信、ミッション再開
 
     else if(detected=false){
-        stopsb612a();
-        motorForwardL();
-        motorStopL();
-        motorForwardR();
-        motorForwardL();
-        startUS015();
+        thermo=0;
+        left=100;
+        wait(3.0);
+        right=100;
+        Ultra=1;
         }
     } 
     //焦電センサー反応無し
@@ -200,12 +350,11 @@
     //直進
 }
 else if(distance>=2000){
-    motorStopL();
-    motorStopR();
-    motorForwardL();
-    motorStopL();
-    motorForwardR();
-    motorForwardL();
+    left=0;
+    right=0;
+    left=100;
+    wait(3.0);
+    right=100;
 }
 //超音波センサー反応無し
 //停止