cansat_B 2019 / Mbed 2 deprecated CanSatB2019_main_1214_essence_8_6

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@9:435ce4946f6c, 2019-12-06 (annotated)

Committer:
saeichi
Date:
Fri Dec 06 09:09:42 2019 +0000
Revision:
9:435ce4946f6c
Parent:
8:765a73e21907
Child:
10:6e02396abaf5
main;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
YUPPY 0:6212b283430c 1 #define cansatB
YUPPY 7:fae874e898b3 2 #include "mbed.h"
YUPPY 7:fae874e898b3 3 //mbed
YUPPY 7:fae874e898b3 4 #include "getGPS.h"
YUPPY 7:fae874e898b3 5 #include "math.h"
YUPPY 7:fae874e898b3 6 //GPS
YUPPY 7:fae874e898b3 7 #include "TB6612.h"
YUPPY 7:fae874e898b3 8 //motorDriver
YUPPY 7:fae874e898b3 9 #include "JPEGCamera.h"
YUPPY 7:fae874e898b3 10 //カメラ
YUPPY 0:6212b283430c 11 #include <stdio.h>
YUPPY 7:fae874e898b3 12 #include <base64.h>
YUPPY 7:fae874e898b3 13 //XBee
YUPPY 7:fae874e898b3 14 #include "us015.h"
YUPPY 7:fae874e898b3 15 // 超音波センサ
YUPPY 7:fae874e898b3 16
YUPPY 7:fae874e898b3 17
YUPPY 2:e2b803e3bcbc 18 GPS gps (p28,p27); //GPS
YUPPY 4:1354e56c7dd3 19
YUPPY 2:e2b803e3bcbc 20 DigitalOut FET(p21); //FET
YUPPY 7:fae874e898b3 21 DigitalOut myled(LED1);
YUPPY 7:fae874e898b3 22 US015 hs(p12,p11);
YUPPY 7:fae874e898b3 23 DigitalIn thermo(p20);
YUPPY 7:fae874e898b3 24 DigitalOut Sb612switch(p15); //焦電スイッチ
YUPPY 4:1354e56c7dd3 25 DigitalOut Ultra(p12);
YUPPY 7:fae874e898b3 26 Serial pc(USBTX,USBRX); // tx, rx
YUPPY 7:fae874e898b3 27 JPEGCamera camera(p9, p10); // TX, RX
YUPPY 7:fae874e898b3 28 TB6612 left1(p25,p17,p16); //モーターピン
YUPPY 7:fae874e898b3 29 TB6612 right1(p26,p19,p18); //モーターピン
YUPPY 7:fae874e898b3 30 Serial xbee(p13,p14); //xbee
YUPPY 2:e2b803e3bcbc 31
YUPPY 7:fae874e898b3 32
YUPPY 7:fae874e898b3 33
YUPPY 5:5aa7223226df 34
YUPPY 7:fae874e898b3 35 int main()
YUPPY 7:fae874e898b3 36 {
YUPPY 7:fae874e898b3 37
YUPPY 7:fae874e898b3 38
YUPPY 7:fae874e898b3 39 Sb612switch=0; //焦電off
YUPPY 7:fae874e898b3 40 wait(1);
YUPPY 7:fae874e898b3 41 Ultra=0;//超音波off
YUPPY 7:fae874e898b3 42
YUPPY 5:5aa7223226df 43 printf("CanSat-B_Start!\r\n");
YUPPY 5:5aa7223226df 44
YUPPY 7:fae874e898b3 45 //FET
YUPPY 7:fae874e898b3 46
YUPPY 7:fae874e898b3 47 FET=1;
YUPPY 7:fae874e898b3 48 wait(10);
YUPPY 7:fae874e898b3 49 FET=0;
YUPPY 7:fae874e898b3 50 wait(10);
YUPPY 7:fae874e898b3 51
YUPPY 7:fae874e898b3 52
YUPPY 7:fae874e898b3 53
YUPPY 5:5aa7223226df 54 //以下GPS
YUPPY 2:e2b803e3bcbc 55 double a;
YUPPY 2:e2b803e3bcbc 56 double b;
YUPPY 2:e2b803e3bcbc 57 double distance;
YUPPY 2:e2b803e3bcbc 58
YUPPY 2:e2b803e3bcbc 59 pc.printf("GPS Start\r\n");
YUPPY 2:e2b803e3bcbc 60
YUPPY 7:fae874e898b3 61 while(1)
YUPPY 7:fae874e898b3 62 {
YUPPY 7:fae874e898b3 63 if(gps.getgps())
YUPPY 7:fae874e898b3 64 {
YUPPY 2:e2b803e3bcbc 65 a = gps.latitude;
YUPPY 2:e2b803e3bcbc 66 b = gps.longitude;
YUPPY 8:765a73e21907 67 pc.printf("位置情報取得成功！\r\n");
YUPPY 2:e2b803e3bcbc 68 pc.printf("(%lf,%lf)\r\n",gps.latitude,gps.longitude);//緯度と経度を表示
YUPPY 8:765a73e21907 69
YUPPY 2:e2b803e3bcbc 70 break;
YUPPY 2:e2b803e3bcbc 71
YUPPY 2:e2b803e3bcbc 72 }else{
YUPPY 7:fae874e898b3 73 pc.printf("NO DATA\r\n");//データ取得失敗
YUPPY 7:fae874e898b3 74 wait(1);
YUPPY 7:fae874e898b3 75 }
YUPPY 7:fae874e898b3 76 }
YUPPY 8:765a73e21907 77
YUPPY 7:fae874e898b3 78 while(1)
YUPPY 8:765a73e21907 79 { printf("移動距離測定開始\r\n");
YUPPY 8:765a73e21907 80 left1=100;
YUPPY 8:765a73e21907 81 right1=100;
YUPPY 8:765a73e21907 82 wait(5);
YUPPY 8:765a73e21907 83 left1=0;
YUPPY 8:765a73e21907 84 right1=0;
YUPPY 8:765a73e21907 85 wait(3);
YUPPY 2:e2b803e3bcbc 86 if(gps.getgps()) {
YUPPY 2:e2b803e3bcbc 87
YUPPY 2:e2b803e3bcbc 88 pc.printf("(%lf,%lf)\n\r",gps.latitude,gps.longitude);//緯度と経度を表示
YUPPY 2:e2b803e3bcbc 89
YUPPY 7:fae874e898b3 90 // 球面三角法により、大円距離(メートル)を求める
YUPPY 7:fae874e898b3 91 double c;
YUPPY 7:fae874e898b3 92 double d;
YUPPY 7:fae874e898b3 93 c = gps.latitude;
YUPPY 7:fae874e898b3 94 d = gps.longitude;
YUPPY 2:e2b803e3bcbc 95
YUPPY 7:fae874e898b3 96 const double pi = 3.14159265359; // 円周率
YUPPY 2:e2b803e3bcbc 97
YUPPY 7:fae874e898b3 98 double ra = a * pi / 180;
YUPPY 7:fae874e898b3 99 double rb = b * pi / 180; // 緯度経度をラジアンに変換
YUPPY 7:fae874e898b3 100 double rc = c * pi / 180;
YUPPY 7:fae874e898b3 101 double rd = d * pi / 180;
YUPPY 2:e2b803e3bcbc 102
YUPPY 7:fae874e898b3 103 double e = sin(ra) * sin(rc) + cos(ra) * cos(rc) * cos(rb - rd); // 2点の中心角(ラジアン)を求める
YUPPY 7:fae874e898b3 104 double rr = acos(e);
YUPPY 2:e2b803e3bcbc 105
YUPPY 7:fae874e898b3 106 const double earth_radius = 6378140; // 地球赤道半径(m)
YUPPY 2:e2b803e3bcbc 107
YUPPY 7:fae874e898b3 108 distance = earth_radius * rr; // 2点間の距離(m)
YUPPY 2:e2b803e3bcbc 109
YUPPY 2:e2b803e3bcbc 110
YUPPY 2:e2b803e3bcbc 111
YUPPY 2:e2b803e3bcbc 112
YUPPY 2:e2b803e3bcbc 113 if (distance<5){
YUPPY 8:765a73e21907 114 printf("走行距離=%lf\r\n",distance);
YUPPY 2:e2b803e3bcbc 115 }else{
YUPPY 2:e2b803e3bcbc 116 pc.printf("5m clear!");
YUPPY 2:e2b803e3bcbc 117 break;
YUPPY 7:fae874e898b3 118 }
YUPPY 2:e2b803e3bcbc 119
YUPPY 7:fae874e898b3 120 }else{
YUPPY 2:e2b803e3bcbc 121 pc.printf("NO DATA\r\n");//データ取得失敗
YUPPY 2:e2b803e3bcbc 122 wait(1);
YUPPY 2:e2b803e3bcbc 123 }
YUPPY 2:e2b803e3bcbc 124 }
YUPPY 5:5aa7223226df 125 //GPS End
YUPPY 5:5aa7223226df 126
YUPPY 7:fae874e898b3 127 int i=1;
YUPPY 4:1354e56c7dd3 128 float th;
YUPPY 4:1354e56c7dd3 129 Timer tm;
YUPPY 7:fae874e898b3 130 for(i=0;i<3;i++){
YUPPY 4:1354e56c7dd3 131 pc.printf("start\r\n");
YUPPY 6:db1a62608047 132
YUPPY 7:fae874e898b3 133 left1 = 100; //左モーター１００％
YUPPY 7:fae874e898b3 134 right1 = 100;//右モーター１００％
YUPPY 7:fae874e898b3 135 printf("Restart\r\n" );
YUPPY 7:fae874e898b3 136 wait(4);
YUPPY 7:fae874e898b3 137 left1=50;
YUPPY 7:fae874e898b3 138 right1=50;
YUPPY 7:fae874e898b3 139 wait(1);
YUPPY 7:fae874e898b3 140 left1=0;
YUPPY 7:fae874e898b3 141 right1=0;
YUPPY 7:fae874e898b3 142 wait(1);
YUPPY 7:fae874e898b3 143 printf("停止\n\r");
YUPPY 7:fae874e898b3 144 Sb612switch=0; //焦電off
YUPPY 7:fae874e898b3 145 wait(1);
YUPPY 4:1354e56c7dd3 146 Ultra=1;//超音波on
YUPPY 7:fae874e898b3 147 wait(1);
YUPPY 5:5aa7223226df 148
YUPPY 7:fae874e898b3 149 while(1) {
YUPPY 7:fae874e898b3 150 printf("超音波on\r\n 焦電off\r\n" ) ;
YUPPY 7:fae874e898b3 151 hs.TrigerOut();
YUPPY 5:5aa7223226df 152 wait(1);
YUPPY 5:5aa7223226df 153 int distance;
YUPPY 5:5aa7223226df 154 distance = hs.GetDistance();
YUPPY 7:fae874e898b3 155 printf("distance=%dmm\r\n",distance);//距離出力
YUPPY 7:fae874e898b3 156
YUPPY 7:fae874e898b3 157 if(distance<=2000){//超音波反応
YUPPY 7:fae874e898b3 158
YUPPY 7:fae874e898b3 159 Ultra=0;//超音波off
YUPPY 7:fae874e898b3 160 wait(1);
YUPPY 7:fae874e898b3 161 Sb612switch=1; //焦電on
YUPPY 7:fae874e898b3 162 wait(1);
YUPPY 7:fae874e898b3 163 printf("焦電On!\r\n ");
YUPPY 7:fae874e898b3 164 bool detected=false;
YUPPY 7:fae874e898b3 165 th = thermo;
YUPPY 7:fae874e898b3 166 if(th==1 && !detected) {//焦電反応ありの場合
YUPPY 7:fae874e898b3 167 i++;
YUPPY 7:fae874e898b3 168 detected=true;
YUPPY 7:fae874e898b3 169 pc.printf("human\r\n");
YUPPY 7:fae874e898b3 170 tm.reset();
YUPPY 7:fae874e898b3 171 tm.start();
YUPPY 7:fae874e898b3 172
YUPPY 7:fae874e898b3 173 LocalFileSystem local("local");
YUPPY 7:fae874e898b3 174 Timer timer;
YUPPY 7:fae874e898b3 175 timer.start();
YUPPY 7:fae874e898b3 176 camera.setPictureSize(JPEGCamera::SIZE320x240);
YUPPY 7:fae874e898b3 177
YUPPY 7:fae874e898b3 178 FILE *fp;
YUPPY 7:fae874e898b3 179 base64 *bs;
YUPPY 7:fae874e898b3 180 int c;
YUPPY 7:fae874e898b3 181
YUPPY 7:fae874e898b3 182 for (int r = 0; r < 1; r++) {
YUPPY 7:fae874e898b3 183 if (camera.isReady()) {
saeichi 9:435ce4946f6c 184
saeichi 9:435ce4946f6c 185 char filename[25];
saeichi 9:435ce4946f6c 186 snprintf(filename,25,"%s%03d%s", "/local/pict",r,".jpg");
YUPPY 7:fae874e898b3 187 printf("Picture: %s ", filename);
YUPPY 7:fae874e898b3 188 if (camera.takePicture(filename)) {
YUPPY 7:fae874e898b3 189 while (camera.isProcessing()) {
YUPPY 7:fae874e898b3 190 camera.processPicture();
saeichi 9:435ce4946f6c 191 printf("take picture!");
saeichi 9:435ce4946f6c 192
saeichi 9:435ce4946f6c 193 xbee.printf("xbee connnected!\r\n");
saeichi 9:435ce4946f6c 194 bs = new base64();
saeichi 9:435ce4946f6c 195 bs->Encode(filename,"/local/data.txt");
YUPPY 7:fae874e898b3 196 }
YUPPY 7:fae874e898b3 197 }else{
YUPPY 7:fae874e898b3 198 printf("take picture failed\r\n");
YUPPY 7:fae874e898b3 199 }
YUPPY 7:fae874e898b3 200 }else{
YUPPY 7:fae874e898b3 201 printf("camera is not ready\r\n");
YUPPY 7:fae874e898b3 202 }
YUPPY 7:fae874e898b3 203
YUPPY 7:fae874e898b3 204 printf("time = %f\n", timer.read());
saeichi 9:435ce4946f6c 205
YUPPY 7:fae874e898b3 206
saeichi 9:435ce4946f6c 207 if((fp=fopen("/local/data.txt","r"))!=NULL)
YUPPY 7:fae874e898b3 208 {
YUPPY 7:fae874e898b3 209 pc.printf("ok\r\n");
YUPPY 7:fae874e898b3 210 while((c=fgetc(fp))!=EOF){
YUPPY 7:fae874e898b3 211 xbee.printf("%c",c);
YUPPY 7:fae874e898b3 212 }
YUPPY 7:fae874e898b3 213 fclose(fp);
YUPPY 7:fae874e898b3 214 }
YUPPY 7:fae874e898b3 215 }
YUPPY 7:fae874e898b3 216 while(1){
YUPPY 7:fae874e898b3 217
YUPPY 7:fae874e898b3 218 int received_data = xbee.getc();
YUPPY 7:fae874e898b3 219
YUPPY 7:fae874e898b3 220 if (received_data == 82 || received_data == 114){ //Rまたはｒ
YUPPY 7:fae874e898b3 221 xbee.printf("_________________________________________________________________________________________________________________________________\r\n");
YUPPY 7:fae874e898b3 222 if((fp=fopen("/local/data000.txt","r"))!=NULL)
YUPPY 7:fae874e898b3 223 {
YUPPY 7:fae874e898b3 224 while ((c=fgetc(fp))!=EOF){
YUPPY 7:fae874e898b3 225 xbee.printf("%c",c); //再送
YUPPY 7:fae874e898b3 226 }
YUPPY 7:fae874e898b3 227 fclose(fp);
YUPPY 7:fae874e898b3 228 }
YUPPY 7:fae874e898b3 229 }
YUPPY 7:fae874e898b3 230 else{
YUPPY 7:fae874e898b3 231 break;
YUPPY 7:fae874e898b3 232 }
YUPPY 7:fae874e898b3 233 }
YUPPY 7:fae874e898b3 234
YUPPY 7:fae874e898b3 235 Sb612switch=0; //焦電off
YUPPY 7:fae874e898b3 236 wait(1);
YUPPY 7:fae874e898b3 237 }else{//焦電反応なしの場合
YUPPY 7:fae874e898b3 238 printf("not found!\r\n");
YUPPY 7:fae874e898b3 239
YUPPY 7:fae874e898b3 240
YUPPY 7:fae874e898b3 241 Sb612switch=0;
YUPPY 7:fae874e898b3 242 wait(1);
YUPPY 7:fae874e898b3 243 Ultra=0;
YUPPY 7:fae874e898b3 244 wait(1);
YUPPY 7:fae874e898b3 245 detected=false;
YUPPY 7:fae874e898b3 246 printf("後退\r\n");
YUPPY 7:fae874e898b3 247 left1 = -100; //左モーター-5０％
YUPPY 7:fae874e898b3 248 right1 = -100;//右モーター-5０％
YUPPY 7:fae874e898b3 249 wait(2.0);
YUPPY 7:fae874e898b3 250 left1=-50;
YUPPY 7:fae874e898b3 251 right1=-50;
YUPPY 7:fae874e898b3 252 wait(1);
YUPPY 7:fae874e898b3 253 left1=0;
YUPPY 7:fae874e898b3 254 right1=0;
YUPPY 7:fae874e898b3 255 wait(1);
YUPPY 7:fae874e898b3 256
YUPPY 7:fae874e898b3 257 printf("右折\n\r");
YUPPY 7:fae874e898b3 258
YUPPY 7:fae874e898b3 259 left1 = 60; //左モーター１００％
YUPPY 7:fae874e898b3 260 right1 = 100;//右モーター１００％
YUPPY 7:fae874e898b3 261 wait(2.0);
YUPPY 7:fae874e898b3 262 }
YUPPY 7:fae874e898b3 263 }else{//超音波distance>2000
YUPPY 7:fae874e898b3 264 printf("safety zone\r\n");
YUPPY 7:fae874e898b3 265 Ultra=0;
YUPPY 7:fae874e898b3 266 wait(1);
YUPPY 7:fae874e898b3 267 left1 = 60; //左モーター50％
YUPPY 7:fae874e898b3 268 right1 = 100;//右モーター50％
YUPPY 7:fae874e898b3 269 printf("右折\r\n");
YUPPY 5:5aa7223226df 270 }
YUPPY 4:1354e56c7dd3 271
YUPPY 5:5aa7223226df 272 }
YUPPY 7:fae874e898b3 273
YUPPY 7:fae874e898b3 274 }
YUPPY 7:fae874e898b3 275 }