Dependencies: mbed
Diff: main.cpp
- Revision:
- 10:6e02396abaf5
- Parent:
- 9:435ce4946f6c
- Child:
- 11:6f553aa95d08
--- a/main.cpp Fri Dec 06 09:09:42 2019 +0000
+++ b/main.cpp Fri Dec 13 11:25:52 2019 +0000
@@ -28,248 +28,287 @@
TB6612 left1(p25,p17,p16); //モーターピン
TB6612 right1(p26,p19,p18); //モーターピン
Serial xbee(p13,p14); //xbee
-
-
-
+DigitalIn flight(p23); //フライトピン(in)
+DigitalOut SW(p24); //フライトピン（out）
int main()
{
- Sb612switch=0; //焦電off
+ /* Sb612switch=0; //焦電off
wait(1);
Ultra=0;//超音波off
-
+ SW=1;
+ flight==1;//flight pin ついてる
+ FET=1;//FET off
printf("CanSat-B_Start!\r\n");
-
+
//FET
-
- FET=1;
- wait(10);
- FET=0;
- wait(10);
-
+ while(1){
+ if(flight==1) {
+ wait(10);
+ printf("mada\r\n");
+ }
+ else{
+ if(flight==1) {
+ wait(10);
+ printf("madamada\r\n");
+ }
+ else{
+ printf("yattar\r\n");
+ FET=0;
+ wait(10);
+ FET=1;
+ wait(10);
+ printf("FET End!\r\n");
+ SW=0;
+
+ break;
+ }
+ }
+ }
//以下GPS
double a;
double b;
double distance;
-
- pc.printf("GPS Start\r\n");
-
- while(1)
- {
- if(gps.getgps())
- {
+ int k = 0;
+ int j = 0;
+
+ pc.printf("GPS Start\n");
+ xbee.printf("s\n");
+ while(1){
+ if(gps.getgps()){
+
+ pc.printf("(%lf,%lf)\r\n",gps.latitude,gps.longitude);//緯度と経度を表示
+ k ++;
+ if(k<59){
+ }else{
a = gps.latitude;
b = gps.longitude;
- pc.printf("位置情報取得成功！\r\n");
- pc.printf("(%lf,%lf)\r\n",gps.latitude,gps.longitude);//緯度と経度を表示
-
+ printf("(a =%lf,b =%lf)\r\n",gps.latitude,gps.longitude);//a,bを表示
break;
-
+ }
}else{
- pc.printf("NO DATA\r\n");//データ取得失敗
- wait(1);
- }
- }
-
- while(1)
- { printf("移動距離測定開始\r\n");
- left1=100;
- right1=100;
- wait(5);
- left1=0;
- right1=0;
- wait(3);
- if(gps.getgps()) {
+ pc.printf("NO DATA\r\n");//データ取得失敗
+ wait(1);
+ }
+ }
+ printf("moter on");
+ left1 = 100; //左モーター１００％
+ right1 = 100;//右モーター１００％
+
+ wait(30);
+ printf("moter off");
+ left1 = 0; //左モーター０％
+ right1 = 0;//右モーター０％
+ printf("moter off");
+ wait(15);
+ printf("GPS restart\n");
+
+ 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;
+ pc.printf("(%lf,%lf)\r\n",gps.latitude,gps.longitude);//緯度と経度を表示
+ j ++;
+ if(j<29){
+ }else{
+ // 球面三角法により、大円距離(メートル)を求める
+ double c;
+ double d;
+ c = gps.latitude;
+ d = gps.longitude;
- const double pi = 3.14159265359; // 円周率
+ pc.printf("(%lf,%lf)\n\r",gps.latitude,gps.longitude);//c,dを表示
+ 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);
+ if (distance<5){
}else{
+ pc.printf("(c =%lf,d =%lf)\n\r",gps.latitude,gps.longitude);//c,dを表示
pc.printf("5m clear!");
- break;
- }
-
- }else{
- pc.printf("NO DATA\r\n");//データ取得失敗
- wait(1);
- }
- }
- //GPS End
+ xbee.printf("5m clear!");
+ break;
+ }
+ }
+ }else{
+ printf("No data");
+ }
+ } //GPS End */
int i=1;
- float th;
- Timer tm;
- for(i=0;i<3;i++){
- pc.printf("start\r\n");
+ float th;
+ Timer tm;
+ for(i=0;i<3;i++){
+ pc.printf("start\r\n");
- left1 = 100; //左モーター１００％
- right1 = 100;//右モーター１００％
- printf("Restart\r\n" );
- wait(4);
- left1=50;
- right1=50;
+ 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
+ wait(1);
+
+ while(1) {
+ left1 = 0;
+ right1 = 0;
+
+ printf("超音波on\r\n 焦電off\r\n" ) ;
+
+ hs.TrigerOut();
+ wait(1);
+ int distance;
+ distance = hs.GetDistance();
+
+ 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[25];
+ snprintf(filename,25,"%s%03d%s","/local/pict",r,".jpg");
+ printf("Picture: %s ", filename);
+ if (camera.takePicture(filename)) {
+ while (camera.isProcessing()) {
+ camera.processPicture();
+
+ printf("take pictuer!");
+
+ xbee.printf("xbee connected!!\r\n");
+
+ bs = new base64();
+ bs->Encode(filename,"/local/data.txt");
+
+ printf("time = %f\n", timer.read());
+
+
+ if((fp=fopen("/local/d.txt","r"))!=NULL){
+ pc.printf("ok\r\n");
+ while((c=fgetc(fp))!=EOF){
+ xbee.printf("%c",c);
+ }
+ fclose(fp);
+ }
+ }
+ }else{
+ printf("take picture failed\r\n");
+ }
+ }else{
+ printf("camera is not ready\r\n");
+ }
+ while(1){
+
+ int received_data = xbee.getc();
+
+ if (received_data == 82 || received_data == 114){ //Rまたはｒ
+
+ xbee.printf("_________________________________________________________________________________________________________________________________\r\n");
+ if((fp=fopen("/local/d.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; //左モーター-50％
+ right1 = -100;//右モーター-50％
+ wait(2.0);
+ left1=-50;
+ right1=-50;
wait(1);
left1=0;
right1=0;
wait(1);
- printf("停止\n\r");
- Sb612switch=0; //焦電off
- wait(1);
- Ultra=1;//超音波on
- wait(1);
-
- while(1) {
- printf("超音波on\r\n 焦電off\r\n" ) ;
- hs.TrigerOut();
- wait(1);
- int distance;
- distance = hs.GetDistance();
- 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[25];
- snprintf(filename,25,"%s%03d%s", "/local/pict",r,".jpg");
- printf("Picture: %s ", filename);
- if (camera.takePicture(filename)) {
- while (camera.isProcessing()) {
- camera.processPicture();
- printf("take picture!");
-
- xbee.printf("xbee connnected!\r\n");
- bs = new base64();
- bs->Encode(filename,"/local/data.txt");
- }
- }else{
- printf("take picture failed\r\n");
- }
- }else{
- printf("camera is not ready\r\n");
- }
-
- printf("time = %f\n", timer.read());
-
-
- if((fp=fopen("/local/data.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");
- }
+ printf("右折\n\r");
+ left1 = 60; //左モーター100％
+ right1 = 100;//右モーター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");
+ wait(3);
+ left1 = 100;
+ right1 = 100;
+ wait(5);
+ left1 = 0;
+ right1 = 0;
+ wait(5);
+ }
}
+ }
}
-}