cansat_B 2019 / Mbed 2 deprecated GPStougou1

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   mbed

main.cpp@6:586f877b5aa9, 2019-11-30 (annotated)

Committer:
KINU
Date:
Sat Nov 30 10:31:43 2019 +0000
Revision:
6:586f877b5aa9
Parent:
5:9b5899b06e2e 
GPStougou1

Who changed what in which revision?

UserRevisionLine numberNew contents of line
KINU 0:f728b8e6bdf2 1 #include "mbed.h"
KINU 0:f728b8e6bdf2 2 #include "JPEGCamera.h"
KINU 0:f728b8e6bdf2 3 #include "us015.h"
KINU 3:f70ecf1e21d5 4 #include "TB6612.h"
saeichi 5:9b5899b06e2e 5 #include <stdio.h>
saeichi 5:9b5899b06e2e 6 #include <base64.h>
KINU 6:586f877b5aa9 7 #include "getGPS.h"
KINU 6:586f877b5aa9 8 #include "math.h"
saeichi 5:9b5899b06e2e 9
KINU 0:f728b8e6bdf2 10 DigitalOut myled(LED1);
KINU 0:f728b8e6bdf2 11 US015 hs(p12,p11);
YUPPY 4:75090ff74dd0 12 DigitalIn thermo(p20);
saeichi 5:9b5899b06e2e 13 DigitalOut Sb612switch(p15); //焦電スイッチ
KINU 0:f728b8e6bdf2 14 DigitalOut Ultra(p12);
saeichi 5:9b5899b06e2e 15 Serial pc(USBTX,USBRX); // tx, rx
saeichi 5:9b5899b06e2e 16 JPEGCamera camera(p9, p10); // TX, RX
saeichi 5:9b5899b06e2e 17 TB6612 left1(p25,p17,p16); //モーターピン
saeichi 5:9b5899b06e2e 18 TB6612 right1(p26,p19,p18); //モーターピン
saeichi 5:9b5899b06e2e 19 Serial xbee(p13,p14); //xbee
saeichi 5:9b5899b06e2e 20
KINU 6:586f877b5aa9 21 GPS gps (p28,p27); //GPS
KINU 6:586f877b5aa9 22
KINU 6:586f877b5aa9 23
KINU 0:f728b8e6bdf2 24 int main(){
seijakunouenimutou 2:c799e2dde99f 25 int i=1;
KINU 0:f728b8e6bdf2 26 float th;
KINU 0:f728b8e6bdf2 27 Timer tm;
KINU 6:586f877b5aa9 28 double a;
KINU 6:586f877b5aa9 29 double b;
KINU 6:586f877b5aa9 30 double distance;
KINU 6:586f877b5aa9 31
KINU 6:586f877b5aa9 32 pc.printf("GPS Start\n");
KINU 6:586f877b5aa9 33
KINU 6:586f877b5aa9 34 while(1) {
KINU 6:586f877b5aa9 35 if(gps.getgps()){
KINU 6:586f877b5aa9 36 a = gps.latitude;
KINU 6:586f877b5aa9 37 b = gps.longitude;
KINU 6:586f877b5aa9 38
KINU 6:586f877b5aa9 39 pc.printf("(%lf,%lf)\r\n",gps.latitude,gps.longitude);//緯度と経度を表示
KINU 6:586f877b5aa9 40 break;
KINU 6:586f877b5aa9 41
KINU 6:586f877b5aa9 42 }else{
KINU 6:586f877b5aa9 43 pc.printf("NO DATA\r\n");//データ取得失敗
KINU 6:586f877b5aa9 44 wait(1);
KINU 6:586f877b5aa9 45 }
KINU 6:586f877b5aa9 46 }
KINU 6:586f877b5aa9 47 while(1){
KINU 6:586f877b5aa9 48 if(gps.getgps()) {
KINU 6:586f877b5aa9 49
KINU 6:586f877b5aa9 50 pc.printf("(%lf,%lf)\n\r",gps.latitude,gps.longitude);//緯度と経度を表示
KINU 6:586f877b5aa9 51
KINU 6:586f877b5aa9 52 // 球面三角法により、大円距離(メートル)を求める
KINU 6:586f877b5aa9 53 double c;
KINU 6:586f877b5aa9 54 double d;
KINU 6:586f877b5aa9 55 c = gps.latitude;
KINU 6:586f877b5aa9 56 d = gps.longitude;
KINU 6:586f877b5aa9 57
KINU 6:586f877b5aa9 58 const double pi = 3.14159265359; // 円周率
KINU 6:586f877b5aa9 59
KINU 6:586f877b5aa9 60 double ra = a * pi / 180;
KINU 6:586f877b5aa9 61 double rb = b * pi / 180; // 緯度経度をラジアンに変換
KINU 6:586f877b5aa9 62 double rc = c * pi / 180;
KINU 6:586f877b5aa9 63 double rd = d * pi / 180;
KINU 6:586f877b5aa9 64
KINU 6:586f877b5aa9 65 double e = sin(ra) * sin(rc) + cos(ra) * cos(rc) * cos(rb - rd); // 2点の中心角(ラジアン)を求める
KINU 6:586f877b5aa9 66 double rr = acos(e);
KINU 6:586f877b5aa9 67
KINU 6:586f877b5aa9 68 const double earth_radius = 6378140; // 地球赤道半径(m)
KINU 6:586f877b5aa9 69
KINU 6:586f877b5aa9 70 distance = earth_radius * rr; // 2点間の距離(m)
KINU 6:586f877b5aa9 71
KINU 6:586f877b5aa9 72
KINU 6:586f877b5aa9 73 if (distance<5){
KINU 6:586f877b5aa9 74 }else{
KINU 6:586f877b5aa9 75 pc.printf("5m clear!");
KINU 6:586f877b5aa9 76 xbee.printf("5m clear!");
KINU 6:586f877b5aa9 77 break;
KINU 6:586f877b5aa9 78 }
KINU 6:586f877b5aa9 79
KINU 6:586f877b5aa9 80 }else{
KINU 6:586f877b5aa9 81 xbee.printf("NO DATA\r\n");//データ取得失
KINU 6:586f877b5aa9 82 pc.printf("NO DATA\r\n");
KINU 6:586f877b5aa9 83 wait(1);
KINU 6:586f877b5aa9 84 }
KINU 6:586f877b5aa9 85 }
KINU 6:586f877b5aa9 86
seijakunouenimutou 2:c799e2dde99f 87 for(i=0;i<3;i++){
KINU 0:f728b8e6bdf2 88 pc.printf("start\r\n");
saeichi 5:9b5899b06e2e 89
saeichi 5:9b5899b06e2e 90 left1 = 100; //左モーター１００％
saeichi 5:9b5899b06e2e 91 right1 = 100;//右モーター１００％
YUPPY 4:75090ff74dd0 92
YUPPY 4:75090ff74dd0 93 Sb612switch=0; //焦電off
KINU 0:f728b8e6bdf2 94 Ultra=1;//超音波on
KINU 3:f70ecf1e21d5 95
KINU 0:f728b8e6bdf2 96 while(1) {
YUPPY 4:75090ff74dd0 97 printf("超音波on\r\n 焦電off\r\n" ) ;
KINU 0:f728b8e6bdf2 98 hs.TrigerOut();
KINU 0:f728b8e6bdf2 99 wait(1);
KINU 0:f728b8e6bdf2 100 int distance;
KINU 0:f728b8e6bdf2 101 distance = hs.GetDistance();
YUPPY 4:75090ff74dd0 102 printf("distance=%d\r\n",distance);//距離出力
KINU 0:f728b8e6bdf2 103
YUPPY 4:75090ff74dd0 104 if(distance<=2000){//超音波反応
saeichi 5:9b5899b06e2e 105 left1 = 0; //左モーター０％
saeichi 5:9b5899b06e2e 106 right1 = 0;//右モーター０％
KINU 3:f70ecf1e21d5 107 printf("停止\n\r");
YUPPY 4:75090ff74dd0 108 wait(5.0);
KINU 3:f70ecf1e21d5 109
KINU 0:f728b8e6bdf2 110 Ultra=0;//超音波off
YUPPY 4:75090ff74dd0 111 Sb612switch=1; //焦電on
YUPPY 4:75090ff74dd0 112 printf("超音波off\r\n焦電on\r\n ");
YUPPY 4:75090ff74dd0 113 bool detected=false;
KINU 0:f728b8e6bdf2 114 th = thermo;
YUPPY 4:75090ff74dd0 115 if(th==1 && !detected) {//焦電反応ありの場合
KINU 3:f70ecf1e21d5 116 i++;
KINU 0:f728b8e6bdf2 117 detected=true;
KINU 3:f70ecf1e21d5 118 pc.printf("human\r\n");
KINU 3:f70ecf1e21d5 119 tm.reset();
KINU 3:f70ecf1e21d5 120 tm.start();
KINU 0:f728b8e6bdf2 121
KINU 3:f70ecf1e21d5 122 LocalFileSystem local("local");
KINU 3:f70ecf1e21d5 123 Timer timer;
KINU 3:f70ecf1e21d5 124 timer.start();
KINU 3:f70ecf1e21d5 125 camera.setPictureSize(JPEGCamera::SIZE320x240);
saeichi 5:9b5899b06e2e 126
saeichi 5:9b5899b06e2e 127 FILE *fp;
saeichi 5:9b5899b06e2e 128 base64 *bs;
saeichi 5:9b5899b06e2e 129 int c;
KINU 0:f728b8e6bdf2 130
saeichi 5:9b5899b06e2e 131 for (int r = 0; r < 1; r++) {
KINU 3:f70ecf1e21d5 132 if (camera.isReady()) {
KINU 3:f70ecf1e21d5 133 char filename[32];
saeichi 5:9b5899b06e2e 134 sprintf(filename, "/local/pict%03d.jpg",r);
KINU 3:f70ecf1e21d5 135 printf("Picture: %s ", filename);
KINU 3:f70ecf1e21d5 136 if (camera.takePicture(filename)) {
KINU 3:f70ecf1e21d5 137 while (camera.isProcessing()) {
KINU 0:f728b8e6bdf2 138 camera.processPicture();
KINU 0:f728b8e6bdf2 139 printf("take pictuer!");
KINU 3:f70ecf1e21d5 140 }
KINU 3:f70ecf1e21d5 141 }else{
KINU 0:f728b8e6bdf2 142 printf("take picture failed\r\n");
KINU 3:f70ecf1e21d5 143 }
KINU 3:f70ecf1e21d5 144 }else{
KINU 3:f70ecf1e21d5 145 printf("camera is not ready\r\n");
KINU 0:f728b8e6bdf2 146 }
KINU 3:f70ecf1e21d5 147
KINU 3:f70ecf1e21d5 148 printf("time = %f\n", timer.read());
seijakunouenimutou 2:c799e2dde99f 149
saeichi 5:9b5899b06e2e 150
saeichi 5:9b5899b06e2e 151
saeichi 5:9b5899b06e2e 152 xbee.printf("xbee connected!\r\n");
saeichi 5:9b5899b06e2e 153 bs = new base64();
saeichi 5:9b5899b06e2e 154 bs->Encode("/local/pict000.jpg","/local/data000.txt");
saeichi 5:9b5899b06e2e 155
saeichi 5:9b5899b06e2e 156 if((fp=fopen("/local/data000.txt","r"))!=NULL)
saeichi 5:9b5899b06e2e 157 {
saeichi 5:9b5899b06e2e 158 pc.printf("ok\r\n");
saeichi 5:9b5899b06e2e 159 while((c=fgetc(fp))!=EOF){
saeichi 5:9b5899b06e2e 160 xbee.printf("%c",c);
saeichi 5:9b5899b06e2e 161 }
saeichi 5:9b5899b06e2e 162 fclose(fp);
saeichi 5:9b5899b06e2e 163 }
saeichi 5:9b5899b06e2e 164 }
saeichi 5:9b5899b06e2e 165
KINU 3:f70ecf1e21d5 166 }else{//焦電反応なしの場合
YUPPY 4:75090ff74dd0 167 printf("not found!\r\n");
YUPPY 4:75090ff74dd0 168
saeichi 5:9b5899b06e2e 169 left1 = 70; //左モーター１０％
saeichi 5:9b5899b06e2e 170 right1 = 100;//右モーター１００％(右折)
YUPPY 4:75090ff74dd0 171 Sb612switch=0;
YUPPY 4:75090ff74dd0 172 Ultra=1;
KINU 3:f70ecf1e21d5 173 detected=false;
KINU 3:f70ecf1e21d5 174 printf("右折\n\r");
KINU 3:f70ecf1e21d5 175 wait(3.0);
saeichi 5:9b5899b06e2e 176 left1 = 100; //左モーター１００％
saeichi 5:9b5899b06e2e 177 right1 = 100;//右モーター１００％
KINU 3:f70ecf1e21d5 178 }
KINU 3:f70ecf1e21d5 179 }else{//超音波distance>2000
YUPPY 4:75090ff74dd0 180 printf("safety zone\r\n");
saeichi 5:9b5899b06e2e 181 left1 = 100; //左モーター100％
saeichi 5:9b5899b06e2e 182 right1 = 70;//右モーター70％(左折)
YUPPY 4:75090ff74dd0 183 printf("左折\r\n");
KINU 3:f70ecf1e21d5 184 }
KINU 3:f70ecf1e21d5 185
KINU 3:f70ecf1e21d5 186 }
KINU 3:f70ecf1e21d5 187
KINU 0:f728b8e6bdf2 188 }
KINU 0:f728b8e6bdf2 189 }
saeichi 5:9b5899b06e2e 190
YUPPY 4:75090ff74dd0 191