mainまでいけました カメラのとこからコンパイルできません
Dependencies: mbed CameraUS015sb612-3
main.cpp
- Committer:
- YUPPY
- Date:
- 2019-11-22
- Revision:
- 5:5aa7223226df
- Parent:
- 4:1354e56c7dd3
- Child:
- 6:db1a62608047
File content as of revision 5:5aa7223226df:
#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 "JPEGCamera.h"//カメラ #include "base64.h"//写真ーXBee #include "us015.h" // 超音波センサ #include <stdio.h> US015 hs(p12,p11); //P12 :超音波センサ トリガ出力 //p11 :超音波センサ エコー入力 GPS gps (p28,p27); //GPS DigitalOut FET(p21); //FET DigitalOut Ultra(p12); DigitalIn thermo(p20); //焦電センサ↓ DigitalOut led(LED1); Serial pc(USBTX,USBRX); // tx, rx 焦電センサ↑ Serial xbee(p13, p14); TB6612 left(p25,p17,p16);//motor TB6612 right(p26,p19,p18);//motor void JPEGCamera(void); void us015(void); void motorFoward(void); void motorFowardR(void); void motorForwardL(void); void motorStop(void); void motorReverse(void); LocalFileSystem local("local"); int main() { printf("CanSat-B_Start!\r\n"); //FET FET = 0; SW = 1; //p21をhigh(3.3V)にする。 while(1) { if(flight==1) { wait(10); } else{ if(flight==1) { wait(10); } else{ FET = 0; //FET、ニクロム線切断 wait(20); FET = 1; wait(12); FET = 0; wait(10); FET = 1; wait(12); FET = 0; SW = 0; //p21をlow(0V)にする。 break; } } } //以下GPS double a; double b; double distance; pc.printf("GPS Start\r\n"); while(1) { if(gps.getgps()){ a = gps.latitude; b = gps.longitude; pc.printf("(%lf,%lf)\r\n",gps.latitude,gps.longitude);//緯度と経度を表示 break; }else{ pc.printf("NO DATA\r\n");//データ取得失敗 wait(1); } } while(1){ if(gps.getgps()) { pc.printf("(%lf,%lf)\n\r",gps.latitude,gps.longitude);//緯度と経度を表示 // 球面三角法により、大円距離(メートル)を求める double c; double d; c = gps.latitude; d = gps.longitude; const double pi = 3.14159265359; // 円周率 double ra = a * pi / 180; double rb = b * pi / 180; // 緯度経度をラジアンに変換 double rc = c * pi / 180; double rd = d * pi / 180; double e = sin(ra) * sin(rc) + cos(ra) * cos(rc) * cos(rb - rd); // 2点の中心角(ラジアン)を求める double rr = acos(e); const double earth_radius = 6378140; // 地球赤道半径(m) distance = earth_radius * rr; // 2点間の距離(m) if (distance<5){ printf("%lf\r\n",distance); motorFoward(); }else{ motorStop(); pc.printf("5m clear!"); break; } }else{ pc.printf("NO DATA\r\n");//データ取得失敗 wait(1); } } //GPS End float th; Timer tm; pc.printf("start\r\n"); bool detected=false; thermo=0; //焦電off Ultra=1;//超音波on us015(); if(distance<2000){//超音波反応 Ultra=0;//超音波off thermo=1;//焦電on motorStop(); if(true) th = thermo; if(th=1 && !detected) {//焦電反応ありの場合 detected=true; pc.printf("human\r\n"); tm.reset(); tm.start(); thermo=0; JPEGCamera(); 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); } }else{//焦電反応なしの場合 detected=false; thermo=0; Ultra=1; } //while(true) //超音波センサー反応あり //停止 //超音波センサーOFF //焦電センサーON //焦電センサー反応あり //焦電センサーOFF //カメラ起動 //カメラOFF //XBeeによりPCへ送信 if(receiveOK()=true){ printf("Unknown Creature has been descovered!\n"); } //"OK"受信、ミッション終了 else if(receiveOK()=false){ printf("Continue!\n"); } //"NO"受信、ミッション再開 else if(detected=false){ thermo=0; motorFowardR(); wait(3); Ultra=1; motorFoward(); } //焦電センサー反応無し //焦電センサーOFF //方向転換 //超音波センサーON //直進 } else if(distance>=2000){ motorStop(); wait(2); motorFowardR(); wait(3); } //超音波センサー反応無し //停止 //方向転換 //直進 } void JPEGCamera(){ 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; } void us015(){ hs.TrigerOut(); wait(1); int distance; distance = hs.GetDistance(); printf("%d\r\n",distance);//距離出力 } void motorFoward(){ left = 100; right = 100; printf("直進\n\r"); } void motorFowardR(){ left = 100; right = 20; printf("右折\n\r"); } void motorForwardL(){ left = 20; right = 100; printf("左折\n\r");} void motorStop(){ left = 0; right = 0; printf("停止\n\r");} void motorReverse(){ left = -50; right = -50; printf("バック\n\r"); }