cansat_B 2019 / Mbed 2 deprecated CanSatB2019_main_1213

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@12:e9d22bda3be6, 2019-12-13 (annotated)

Committer:
YUPPY
Date:
Fri Dec 13 10:29:24 2019 +0000
Revision:
12:e9d22bda3be6
Parent:
11:8061348a85eb 
yeah

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 12:e9d22bda3be6 31 DigitalIn flight(p23); //フライトピン(in)
YUPPY 12:e9d22bda3be6 32 DigitalOut SW(p24); //フライトピン（out）
YUPPY 5:5aa7223226df 33
YUPPY 7:fae874e898b3 34 int main()
YUPPY 7:fae874e898b3 35 {
YUPPY 7:fae874e898b3 36
YUPPY 7:fae874e898b3 37
YUPPY 7:fae874e898b3 38 Sb612switch=0; //焦電off
YUPPY 7:fae874e898b3 39 wait(1);
YUPPY 7:fae874e898b3 40 Ultra=0;//超音波off
YUPPY 12:e9d22bda3be6 41 SW=1;
YUPPY 12:e9d22bda3be6 42 flight==1;//flight pin ついてる
YUPPY 11:8061348a85eb 43 FET=1;//FET off
YUPPY 5:5aa7223226df 44 printf("CanSat-B_Start!\r\n");
YUPPY 12:e9d22bda3be6 45
YUPPY 7:fae874e898b3 46 //FET
YUPPY 11:8061348a85eb 47 while(1){
YUPPY 11:8061348a85eb 48 if(flight==1) {
YUPPY 11:8061348a85eb 49 wait(10);
YUPPY 11:8061348a85eb 50 printf("mada\r\n");
YUPPY 11:8061348a85eb 51 }
YUPPY 11:8061348a85eb 52 else{
YUPPY 11:8061348a85eb 53 if(flight==1) {
YUPPY 11:8061348a85eb 54 wait(10);
YUPPY 11:8061348a85eb 55 printf("madamada\r\n");
YUPPY 11:8061348a85eb 56 }
YUPPY 11:8061348a85eb 57 else{
YUPPY 11:8061348a85eb 58 printf("yattar\r\n");
YUPPY 11:8061348a85eb 59 FET=0;
YUPPY 11:8061348a85eb 60 wait(10);
YUPPY 11:8061348a85eb 61 FET=1;
YUPPY 11:8061348a85eb 62 wait(10);
YUPPY 11:8061348a85eb 63 printf("FET End!\r\n");
YUPPY 12:e9d22bda3be6 64 SW=0;
YUPPY 11:8061348a85eb 65
YUPPY 11:8061348a85eb 66 break;
YUPPY 11:8061348a85eb 67 }
YUPPY 11:8061348a85eb 68 }
YUPPY 11:8061348a85eb 69 }
YUPPY 7:fae874e898b3 70
YUPPY 7:fae874e898b3 71
YUPPY 5:5aa7223226df 72 //以下GPS
YUPPY 2:e2b803e3bcbc 73 double a;
YUPPY 2:e2b803e3bcbc 74 double b;
YUPPY 2:e2b803e3bcbc 75 double distance;
KINU 9:ec770f68440e 76 int k = 0;
KINU 9:ec770f68440e 77 int j = 0;
KINU 9:ec770f68440e 78
KINU 10:b190babac114 79 pc.printf("GPS Start\n");
KINU 10:b190babac114 80 xbee.printf("s\n");
YUPPY 11:8061348a85eb 81 while(1){
KINU 10:b190babac114 82 if(gps.getgps()){
KINU 9:ec770f68440e 83
KINU 9:ec770f68440e 84 pc.printf("(%lf,%lf)\r\n",gps.latitude,gps.longitude);//緯度と経度を表示
KINU 10:b190babac114 85 k ++;
KINU 9:ec770f68440e 86 if(k<59){
KINU 9:ec770f68440e 87 }else{
YUPPY 2:e2b803e3bcbc 88 a = gps.latitude;
YUPPY 2:e2b803e3bcbc 89 b = gps.longitude;
KINU 9:ec770f68440e 90 printf("(a =%lf,b =%lf)\r\n",gps.latitude,gps.longitude);//a,bを表示
YUPPY 2:e2b803e3bcbc 91 break;
KINU 9:ec770f68440e 92 }
YUPPY 2:e2b803e3bcbc 93 }else{
KINU 9:ec770f68440e 94 pc.printf("NO DATA\r\n");//データ取得失敗
KINU 9:ec770f68440e 95 wait(1);
KINU 9:ec770f68440e 96 }
KINU 9:ec770f68440e 97 }
KINU 9:ec770f68440e 98 printf("moter on");
KINU 10:b190babac114 99 left1 = 100; //左モーター１００％
KINU 10:b190babac114 100 right1 = 100;//右モーター１００％
KINU 9:ec770f68440e 101
KINU 9:ec770f68440e 102 wait(30);
KINU 9:ec770f68440e 103 printf("moter off");
KINU 10:b190babac114 104 left1 = 0; //左モーター０％
KINU 10:b190babac114 105 right1 = 0;//右モーター０％
KINU 9:ec770f68440e 106 printf("moter off");
KINU 9:ec770f68440e 107 wait(15);
KINU 9:ec770f68440e 108 printf("GPS restart\n");
KINU 9:ec770f68440e 109
KINU 9:ec770f68440e 110 while(1) {
KINU 9:ec770f68440e 111 if(gps.getgps()){
YUPPY 2:e2b803e3bcbc 112
KINU 9:ec770f68440e 113 pc.printf("(%lf,%lf)\r\n",gps.latitude,gps.longitude);//緯度と経度を表示
KINU 9:ec770f68440e 114 j ++;
KINU 9:ec770f68440e 115 if(j<29){
KINU 9:ec770f68440e 116 }else{
KINU 9:ec770f68440e 117 // 球面三角法により、大円距離(メートル)を求める
KINU 9:ec770f68440e 118 double c;
KINU 9:ec770f68440e 119 double d;
KINU 9:ec770f68440e 120 c = gps.latitude;
KINU 9:ec770f68440e 121 d = gps.longitude;
YUPPY 2:e2b803e3bcbc 122
KINU 9:ec770f68440e 123 pc.printf("(%lf,%lf)\n\r",gps.latitude,gps.longitude);//c,dを表示
KINU 9:ec770f68440e 124 const double pi = 3.14159265359; // 円周率
YUPPY 2:e2b803e3bcbc 125
KINU 9:ec770f68440e 126 double ra = a * pi / 180;
KINU 9:ec770f68440e 127 double rb = b * pi / 180; // 緯度経度をラジアンに変換
KINU 9:ec770f68440e 128 double rc = c * pi / 180;
KINU 9:ec770f68440e 129 double rd = d * pi / 180;
YUPPY 2:e2b803e3bcbc 130
KINU 9:ec770f68440e 131 double e = sin(ra) * sin(rc) + cos(ra) * cos(rc) * cos(rb - rd); // 2点の中心角(ラジアン)を求める
KINU 9:ec770f68440e 132 double rr = acos(e);
YUPPY 2:e2b803e3bcbc 133
KINU 9:ec770f68440e 134 const double earth_radius = 6378140; // 地球赤道半径(m)
YUPPY 2:e2b803e3bcbc 135
KINU 9:ec770f68440e 136 distance = earth_radius * rr; // 2点間の距離(m)
YUPPY 2:e2b803e3bcbc 137
KINU 9:ec770f68440e 138 if (distance<5){
YUPPY 2:e2b803e3bcbc 139 }else{
KINU 9:ec770f68440e 140 pc.printf("(c =%lf,d =%lf)\n\r",gps.latitude,gps.longitude);//c,dを表示
YUPPY 2:e2b803e3bcbc 141 pc.printf("5m clear!");
KINU 9:ec770f68440e 142 xbee.printf("5m clear!");
KINU 9:ec770f68440e 143 break;
KINU 9:ec770f68440e 144 }
KINU 9:ec770f68440e 145 }
KINU 9:ec770f68440e 146 }else{
KINU 9:ec770f68440e 147 printf("No data");
KINU 10:b190babac114 148 }
KINU 10:b190babac114 149 } //GPS End
YUPPY 5:5aa7223226df 150
YUPPY 7:fae874e898b3 151 int i=1;
YUPPY 4:1354e56c7dd3 152 float th;
YUPPY 4:1354e56c7dd3 153 Timer tm;
YUPPY 7:fae874e898b3 154 for(i=0;i<3;i++){
YUPPY 4:1354e56c7dd3 155 pc.printf("start\r\n");
YUPPY 6:db1a62608047 156
YUPPY 7:fae874e898b3 157 left1 = 100; //左モーター１００％
YUPPY 7:fae874e898b3 158 right1 = 100;//右モーター１００％
YUPPY 7:fae874e898b3 159 printf("Restart\r\n" );
YUPPY 7:fae874e898b3 160 wait(4);
YUPPY 7:fae874e898b3 161 left1=50;
YUPPY 7:fae874e898b3 162 right1=50;
YUPPY 7:fae874e898b3 163 wait(1);
YUPPY 7:fae874e898b3 164 left1=0;
YUPPY 7:fae874e898b3 165 right1=0;
YUPPY 7:fae874e898b3 166 wait(1);
YUPPY 7:fae874e898b3 167 printf("停止\n\r");
YUPPY 7:fae874e898b3 168 Sb612switch=0; //焦電off
YUPPY 7:fae874e898b3 169 wait(1);
YUPPY 4:1354e56c7dd3 170 Ultra=1;//超音波on
YUPPY 7:fae874e898b3 171 wait(1);
YUPPY 5:5aa7223226df 172
YUPPY 7:fae874e898b3 173 while(1) {
YUPPY 7:fae874e898b3 174 printf("超音波on\r\n 焦電off\r\n" ) ;
YUPPY 7:fae874e898b3 175 hs.TrigerOut();
YUPPY 5:5aa7223226df 176 wait(1);
YUPPY 5:5aa7223226df 177 int distance;
YUPPY 5:5aa7223226df 178 distance = hs.GetDistance();
YUPPY 7:fae874e898b3 179 printf("distance=%dmm\r\n",distance);//距離出力
YUPPY 7:fae874e898b3 180
YUPPY 7:fae874e898b3 181 if(distance<=2000){//超音波反応
YUPPY 7:fae874e898b3 182
YUPPY 7:fae874e898b3 183 Ultra=0;//超音波off
YUPPY 7:fae874e898b3 184 wait(1);
YUPPY 7:fae874e898b3 185 Sb612switch=1; //焦電on
YUPPY 7:fae874e898b3 186 wait(1);
YUPPY 7:fae874e898b3 187 printf("焦電On!\r\n ");
YUPPY 7:fae874e898b3 188 bool detected=false;
YUPPY 7:fae874e898b3 189 th = thermo;
YUPPY 7:fae874e898b3 190 if(th==1 && !detected) {//焦電反応ありの場合
YUPPY 7:fae874e898b3 191 i++;
YUPPY 7:fae874e898b3 192 detected=true;
YUPPY 7:fae874e898b3 193 pc.printf("human\r\n");
YUPPY 7:fae874e898b3 194 tm.reset();
YUPPY 7:fae874e898b3 195 tm.start();
YUPPY 7:fae874e898b3 196
YUPPY 7:fae874e898b3 197 LocalFileSystem local("local");
YUPPY 7:fae874e898b3 198 Timer timer;
YUPPY 7:fae874e898b3 199 timer.start();
YUPPY 7:fae874e898b3 200 camera.setPictureSize(JPEGCamera::SIZE320x240);
YUPPY 7:fae874e898b3 201
YUPPY 7:fae874e898b3 202 FILE *fp;
YUPPY 7:fae874e898b3 203 base64 *bs;
YUPPY 7:fae874e898b3 204 int c;
YUPPY 7:fae874e898b3 205
YUPPY 7:fae874e898b3 206 for (int r = 0; r < 1; r++) {
YUPPY 7:fae874e898b3 207 if (camera.isReady()) {
YUPPY 7:fae874e898b3 208 char filename[32];
YUPPY 7:fae874e898b3 209 sprintf(filename, "/local/pict%03d.jpg",r);
YUPPY 7:fae874e898b3 210 printf("Picture: %s ", filename);
YUPPY 7:fae874e898b3 211 if (camera.takePicture(filename)) {
YUPPY 7:fae874e898b3 212 while (camera.isProcessing()) {
YUPPY 7:fae874e898b3 213 camera.processPicture();
YUPPY 7:fae874e898b3 214 printf("take pictuer!");
YUPPY 7:fae874e898b3 215 }
YUPPY 7:fae874e898b3 216 }else{
YUPPY 7:fae874e898b3 217 printf("take picture failed\r\n");
YUPPY 7:fae874e898b3 218 }
YUPPY 7:fae874e898b3 219 }else{
YUPPY 7:fae874e898b3 220 printf("camera is not ready\r\n");
YUPPY 7:fae874e898b3 221 }
YUPPY 7:fae874e898b3 222
YUPPY 7:fae874e898b3 223 printf("time = %f\n", timer.read());
YUPPY 7:fae874e898b3 224
YUPPY 7:fae874e898b3 225
YUPPY 7:fae874e898b3 226
YUPPY 7:fae874e898b3 227 xbee.printf("xbee connected!\r\n");
YUPPY 7:fae874e898b3 228 bs = new base64();
YUPPY 7:fae874e898b3 229 bs->Encode("/local/pict000.jpg","/local/data000.txt");
YUPPY 7:fae874e898b3 230
YUPPY 7:fae874e898b3 231 if((fp=fopen("/local/data000.txt","r"))!=NULL)
YUPPY 7:fae874e898b3 232 {
YUPPY 7:fae874e898b3 233 pc.printf("ok\r\n");
YUPPY 7:fae874e898b3 234 while((c=fgetc(fp))!=EOF){
YUPPY 7:fae874e898b3 235 xbee.printf("%c",c);
YUPPY 7:fae874e898b3 236 }
YUPPY 7:fae874e898b3 237 fclose(fp);
YUPPY 7:fae874e898b3 238 }
YUPPY 7:fae874e898b3 239 }
YUPPY 7:fae874e898b3 240 while(1){
YUPPY 7:fae874e898b3 241
YUPPY 7:fae874e898b3 242 int received_data = xbee.getc();
YUPPY 7:fae874e898b3 243
YUPPY 7:fae874e898b3 244 if (received_data == 82 || received_data == 114){ //Rまたはｒ
YUPPY 7:fae874e898b3 245 xbee.printf("_________________________________________________________________________________________________________________________________\r\n");
YUPPY 7:fae874e898b3 246 if((fp=fopen("/local/data000.txt","r"))!=NULL)
YUPPY 7:fae874e898b3 247 {
YUPPY 7:fae874e898b3 248 while ((c=fgetc(fp))!=EOF){
YUPPY 7:fae874e898b3 249 xbee.printf("%c",c); //再送
YUPPY 7:fae874e898b3 250 }
YUPPY 7:fae874e898b3 251 fclose(fp);
YUPPY 7:fae874e898b3 252 }
YUPPY 7:fae874e898b3 253 }
YUPPY 7:fae874e898b3 254 else{
YUPPY 7:fae874e898b3 255 break;
YUPPY 7:fae874e898b3 256 }
YUPPY 7:fae874e898b3 257 }
YUPPY 7:fae874e898b3 258
YUPPY 7:fae874e898b3 259 Sb612switch=0; //焦電off
YUPPY 7:fae874e898b3 260 wait(1);
YUPPY 7:fae874e898b3 261 }else{//焦電反応なしの場合
YUPPY 7:fae874e898b3 262 printf("not found!\r\n");
YUPPY 7:fae874e898b3 263
YUPPY 7:fae874e898b3 264
YUPPY 7:fae874e898b3 265 Sb612switch=0;
YUPPY 7:fae874e898b3 266 wait(1);
YUPPY 7:fae874e898b3 267 Ultra=0;
YUPPY 7:fae874e898b3 268 wait(1);
YUPPY 7:fae874e898b3 269 detected=false;
YUPPY 7:fae874e898b3 270 printf("後退\r\n");
YUPPY 7:fae874e898b3 271 left1 = -100; //左モーター-5０％
YUPPY 7:fae874e898b3 272 right1 = -100;//右モーター-5０％
YUPPY 7:fae874e898b3 273 wait(2.0);
YUPPY 7:fae874e898b3 274 left1=-50;
YUPPY 7:fae874e898b3 275 right1=-50;
YUPPY 7:fae874e898b3 276 wait(1);
YUPPY 7:fae874e898b3 277 left1=0;
YUPPY 7:fae874e898b3 278 right1=0;
YUPPY 7:fae874e898b3 279 wait(1);
YUPPY 7:fae874e898b3 280
YUPPY 7:fae874e898b3 281 printf("右折\n\r");
YUPPY 7:fae874e898b3 282
YUPPY 7:fae874e898b3 283 left1 = 60; //左モーター１００％
YUPPY 7:fae874e898b3 284 right1 = 100;//右モーター１００％
YUPPY 7:fae874e898b3 285 wait(2.0);
YUPPY 7:fae874e898b3 286 }
YUPPY 7:fae874e898b3 287 }else{//超音波distance>2000
YUPPY 7:fae874e898b3 288 printf("safety zone\r\n");
YUPPY 7:fae874e898b3 289 Ultra=0;
YUPPY 7:fae874e898b3 290 wait(1);
YUPPY 7:fae874e898b3 291 left1 = 60; //左モーター50％
YUPPY 7:fae874e898b3 292 right1 = 100;//右モーター50％
YUPPY 7:fae874e898b3 293 printf("右折\r\n");
YUPPY 5:5aa7223226df 294 }
YUPPY 4:1354e56c7dd3 295
YUPPY 5:5aa7223226df 296 }
YUPPY 7:fae874e898b3 297
YUPPY 7:fae874e898b3 298 }
YUPPY 7:fae874e898b3 299 }