Dependencies: mbed
Diff: main.cpp
- Revision:
- 7:fae874e898b3
- Parent:
- 6:db1a62608047
- Child:
- 8:765a73e21907
--- a/main.cpp Fri Nov 22 08:52:55 2019 +0000
+++ b/main.cpp Wed Dec 04 11:49:44 2019 +0000
@@ -1,38 +1,56 @@
#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 "mbed.h"
+//mbed
+#include "getGPS.h"
+#include "math.h"
+//GPS
+#include "TB6612.h"
+ //motorDriver
+ #include "JPEGCamera.h"
+//カメラ
#include <stdio.h>
-US015 hs(p12,p11); //P12 :超音波センサ トリガ出力 //p11 :超音波センサ エコー入力
+#include <base64.h>
+//XBee
+#include "us015.h"
+// 超音波センサ
+
+
GPS gps (p28,p27); //GPS
DigitalOut FET(p21); //FET
+DigitalOut myled(LED1);
+US015 hs(p12,p11);
+DigitalIn thermo(p20);
+DigitalOut Sb612switch(p15); //焦電スイッチ
DigitalOut Ultra(p12);
-DigitalOut 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
+Serial pc(USBTX,USBRX); // tx, rx
+JPEGCamera camera(p9, p10); // TX, RX
+TB6612 left1(p25,p17,p16); //モーターピン
+TB6612 right1(p26,p19,p18); //モーターピン
+Serial xbee(p13,p14); //xbee
-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() {
+int main()
+{
+
+
+ Sb612switch=0; //焦電off
+ wait(1);
+ Ultra=0;//超音波off
+
printf("CanSat-B_Start!\r\n");
- printf("FETのコピーはここ");
+ //FET
+
+ FET=1;
+ wait(10);
+ FET=0;
+ wait(10);
+
+
+
//以下GPS
double a;
double b;
@@ -40,8 +58,10 @@
pc.printf("GPS Start\r\n");
- while(1) {
- if(gps.getgps()){
+ while(1)
+ {
+ if(gps.getgps())
+ {
a = gps.latitude;
b = gps.longitude;
@@ -50,304 +70,203 @@
break;
}else{
- pc.printf("NO DATA\r\n");//データ取得失敗
- wait(1);
- }
- }
- while(1){
+ 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;
+ // 球面三角法により、大円距離(メートル)を求める
+ double c;
+ double d;
+ c = gps.latitude;
+ d = gps.longitude;
- const double pi = 3.14159265359; // 円周率
+ 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 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);
+ double e = sin(ra) * sin(rc) + cos(ra) * cos(rc) * cos(rb - rd); // 2点の中心角(ラジアン)を求める
+ double rr = acos(e);
- const double earth_radius = 6378140; // 地球赤道半径(m)
+ const double earth_radius = 6378140; // 地球赤道半径(m)
- distance = earth_radius * rr; // 2点間の距離(m)
+ distance = earth_radius * rr; // 2点間の距離(m)
if (distance<5){
printf("%lf\r\n",distance);
- motorFoward();
+ right1=100;
+ left1=100;
}else{
- motorStop();
+ right1=0;
+ left1=0;
pc.printf("5m clear!");
break;
- }
+ }
- }else{
+ }else{
pc.printf("NO DATA\r\n");//データ取得失敗
wait(1);
}
}
//GPS End
+ int i=1;
float th;
Timer tm;
+ for(i=0;i<3;i++){
pc.printf("start\r\n");
- bool detected=false;
- thermo=0; //焦電off
+ left1 = 100; //左モーター１００％
+ right1 = 100;//右モーター１００％
+ printf("Restart\r\n" );
+ wait(4);
+ left1=50;
+ right1=50;
+ wait(1);
+ left1=0;
+ right1=0;
+ wait(1);
+ printf("停止\n\r");
+ Sb612switch=0; //焦電off
+ wait(1);
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へ送信
- printf("OK1はここに\r\n");
- //"OK"受信、ミッション終了
- printf("OK2はここに\r\n");
- //"NO"受信、ミッション再開
-
- printf("NOはここに");
+ wait(1);
- //焦電センサー反応無し
- //焦電センサー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();
+ while(1) {
+ printf("超音波on\r\n 焦電off\r\n" ) ;
+ hs.TrigerOut();
wait(1);
int distance;
distance = hs.GetDistance();
- printf("%d\r\n",distance);//距離出力
+ printf("distance=%dmm\r\n",distance);//距離出力
+
+ if(distance<=2000){//超音波反応
+
+ Ultra=0;//超音波off
+ wait(1);
+ Sb612switch=1; //焦電on
+ wait(1);
+ printf("焦電On!\r\n ");
+ bool detected=false;
+ th = thermo;
+ if(th==1 && !detected) {//焦電反応ありの場合
+ i++;
+ detected=true;
+ pc.printf("human\r\n");
+ tm.reset();
+ tm.start();
+
+ LocalFileSystem local("local");
+ Timer timer;
+ timer.start();
+ camera.setPictureSize(JPEGCamera::SIZE320x240);
+
+ FILE *fp;
+ base64 *bs;
+ int c;
+
+ for (int r = 0; r < 1; r++) {
+ if (camera.isReady()) {
+ char filename[32];
+ sprintf(filename, "/local/pict%03d.jpg",r);
+ printf("Picture: %s ", filename);
+ if (camera.takePicture(filename)) {
+ while (camera.isProcessing()) {
+ camera.processPicture();
+ printf("take pictuer!");
+ }
+ }else{
+ printf("take picture failed\r\n");
+ }
+ }else{
+ printf("camera is not ready\r\n");
+ }
+
+ printf("time = %f\n", timer.read());
+
+
+
+ xbee.printf("xbee connected!\r\n");
+ bs = new base64();
+ bs->Encode("/local/pict000.jpg","/local/data000.txt");
+
+ if((fp=fopen("/local/data000.txt","r"))!=NULL)
+ {
+ pc.printf("ok\r\n");
+ while((c=fgetc(fp))!=EOF){
+ xbee.printf("%c",c);
+ }
+ fclose(fp);
+ }
+ }
+ while(1){
+
+ int received_data = xbee.getc();
+
+ if (received_data == 82 || received_data == 114){ //Rまたはｒ
+ xbee.printf("_________________________________________________________________________________________________________________________________\r\n");
+ if((fp=fopen("/local/data000.txt","r"))!=NULL)
+ {
+ while ((c=fgetc(fp))!=EOF){
+ xbee.printf("%c",c); //再送
+ }
+ fclose(fp);
+ }
+ }
+ else{
+ break;
+ }
+ }
+
+ Sb612switch=0; //焦電off
+ wait(1);
+ }else{//焦電反応なしの場合
+ printf("not found!\r\n");
+
+
+ Sb612switch=0;
+ wait(1);
+ Ultra=0;
+ wait(1);
+ detected=false;
+ printf("後退\r\n");
+ left1 = -100; //左モーター-5０％
+ right1 = -100;//右モーター-5０％
+ wait(2.0);
+ left1=-50;
+ right1=-50;
+ wait(1);
+ left1=0;
+ right1=0;
+ wait(1);
+
+ printf("右折\n\r");
+
+ left1 = 60; //左モーター１００％
+ right1 = 100;//右モーター１００％
+ wait(2.0);
+ }
+ }else{//超音波distance>2000
+ printf("safety zone\r\n");
+ Ultra=0;
+ wait(1);
+ left1 = 60; //左モーター50％
+ right1 = 100;//右モーター50％
+ printf("右折\r\n");
}
-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");
- }
\ No newline at end of file
+
+ }
+}