kyunsat / Mbed 2 deprecated kk2

kk

Dependencies:   SDFileSystem mbed

Fork of cansat_kk by monoCanSat

main.cpp@4:cfaf33b2c97a, 2016-03-04 (annotated)

Committer:
mosukro
Date:
Fri Mar 04 03:26:39 2016 +0000
Revision:
4:cfaf33b2c97a
Parent:
3:efaa785283a6
Child:
5:4e79ad68f3e3
201603041226

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Nike3221 0:649fc30be6ec 1 #include "mbed.h"
Nike3221 0:649fc30be6ec 2 #include "SDFileSystem.h"
mosukro 4:cfaf33b2c97a 3 #include "math.h"
Nike3221 0:649fc30be6ec 4
Nike3221 0:649fc30be6ec 5 Ticker timer;
Nike3221 0:649fc30be6ec 6 SDFileSystem sd(p5, p6, p7, p8, "sd"); // the pinout on the mbed Cool Components workshop board
mosukro 4:cfaf33b2c97a 7 Serial gps(p13, p14); // tx, rx
Nike3221 0:649fc30be6ec 8 Serial pc(USBTX, USBRX); // tx, rx
Nike3221 0:649fc30be6ec 9 PwmOut moterl(p21);//左モーター
Nike3221 0:649fc30be6ec 10 PwmOut moterr(p22);//右モーター
Nike3221 0:649fc30be6ec 11 DigitalOut led1(LED1);
Nike3221 0:649fc30be6ec 12 DigitalOut fet3(p23);//ニクロム線
Nike3221 0:649fc30be6ec 13 DigitalOut led2(LED2);
Nike3221 0:649fc30be6ec 14 DigitalOut led3(LED3);
Nike3221 0:649fc30be6ec 15 DigitalOut led4(LED4);
Nike3221 0:649fc30be6ec 16 DigitalOut janpa1(p19);//パラシュートの開きを検知
mosukro 4:cfaf33b2c97a 17 DigitalIn janpa2(p20);//パラシュートの開きを検知
Nike3221 0:649fc30be6ec 18
mosukro 4:cfaf33b2c97a 19 float palse=0.02;
mosukro 1:fa44a6246bcc 20 char gps_data[256];
mosukro 1:fa44a6246bcc 21 float longitude,latitude,gpstime,knot,angle;
mosukro 4:cfaf33b2c97a 22 //緯度longitude　経度latitude,
Nike3221 0:649fc30be6ec 23 int i=0;
mosukro 4:cfaf33b2c97a 24 char *gps_target = "$GPRMC,230412.000,A,3022.5297,N,13057.6164,E,0.00,168.46,030316,,,A*6D";
mosukro 4:cfaf33b2c97a 25 float high_target = 7.0;
mosukro 4:cfaf33b2c97a 26 float longitude_target,latitude_target,gpstime_target,knot_target,angle_target,high;
mosukro 3:efaa785283a6 27 int count=0;
mosukro 4:cfaf33b2c97a 28 int gomidata1,gomidata2,gomidata3,gomidata4,gomidata5,gomidata6;
mosukro 4:cfaf33b2c97a 29 float longitudegosa;
mosukro 4:cfaf33b2c97a 30 float latitudegosa;
mosukro 4:cfaf33b2c97a 31 int angle_targetmath = 0;
Nike3221 0:649fc30be6ec 32
mosukro 1:fa44a6246bcc 33 void gps_rx()//GPSデータ受信割り込み
Nike3221 0:649fc30be6ec 34 {
mosukro 4:cfaf33b2c97a 35 //pc.printf("gps_rx");
mosukro 1:fa44a6246bcc 36 gps_data[i] = gps.getc();
mosukro 4:cfaf33b2c97a 37 //gps_data[i] = pc.getc();
mosukro 1:fa44a6246bcc 38 if( gps_data[i] == '$' )
mosukro 2:be9046fb5859 39 {
mosukro 1:fa44a6246bcc 40 //$から受信データを保持する
mosukro 1:fa44a6246bcc 41 gps_data[0] = '$';
mosukro 1:fa44a6246bcc 42 i = 1;
mosukro 1:fa44a6246bcc 43 }
mosukro 1:fa44a6246bcc 44 else if( gps_data[i-1] == '\r' && gps_data[i] == '\n' )
mosukro 1:fa44a6246bcc 45 {
mosukro 1:fa44a6246bcc 46 // 改行コードまでのデータを解析する
mosukro 1:fa44a6246bcc 47 gps_data[i+1] = '\0';
mosukro 1:fa44a6246bcc 48 if(memcmp(gps_data, "$GPRMC",6) == 0)
mosukro 1:fa44a6246bcc 49 {
mosukro 1:fa44a6246bcc 50 // $GPRMCで始ってればデータを分けて格納
mosukro 1:fa44a6246bcc 51 sscanf(gps_data,"$GPRMC,%f,A,%f,N,%f,E,%f,%f",&gpstime,&longitude,&latitude,&knot,&angle);
mosukro 4:cfaf33b2c97a 52 pc.printf("GPRMC %f %f %f %f %f \n",gpstime,longitude,latitude,knot,angle);
mosukro 4:cfaf33b2c97a 53 }
mosukro 4:cfaf33b2c97a 54 if(memcmp(gps_data, "$GPGGA",6) == 0)
mosukro 4:cfaf33b2c97a 55 {
mosukro 4:cfaf33b2c97a 56 // $GPGGAで始ってればデータを分けて格納
mosukro 4:cfaf33b2c97a 57 sscanf(gps_data,"$GPGGA,%d,%d,N,%d,E,%d,%d,%d,%f,M",&gomidata1,&gomidata2,&gomidata3,&gomidata4,&gomidata5,&gomidata6,&high);
mosukro 4:cfaf33b2c97a 58 pc.printf("high %f",high);
mosukro 1:fa44a6246bcc 59 }
mosukro 1:fa44a6246bcc 60 //データをSDに書き込み
mosukro 4:cfaf33b2c97a 61 FILE *fp = fopen("/sd/gpsdata.txt", "a");
mosukro 1:fa44a6246bcc 62 if(fp == NULL)
mosukro 1:fa44a6246bcc 63 {
mosukro 1:fa44a6246bcc 64 error("Could not open file for write\n");
mosukro 1:fa44a6246bcc 65 }
mosukro 1:fa44a6246bcc 66 fprintf(fp,"%s \n",gps_data);
mosukro 1:fa44a6246bcc 67 fclose(fp);
mosukro 4:cfaf33b2c97a 68 //free(fp);
Nike3221 0:649fc30be6ec 69
mosukro 1:fa44a6246bcc 70 //PCにデータ送信
mosukro 1:fa44a6246bcc 71 pc.printf("%s \n",gps_data);
mosukro 1:fa44a6246bcc 72 i = 0;
mosukro 1:fa44a6246bcc 73 }
mosukro 1:fa44a6246bcc 74 else
mosukro 1:fa44a6246bcc 75 {
mosukro 1:fa44a6246bcc 76 //改行コードが来るまでカウントを続ける
mosukro 1:fa44a6246bcc 77 i++;
mosukro 2:be9046fb5859 78 if(i==255)
mosukro 2:be9046fb5859 79 {
mosukro 4:cfaf33b2c97a 80 printf("*** Error! ***\n");
mosukro 1:fa44a6246bcc 81 }
mosukro 1:fa44a6246bcc 82 }
Nike3221 0:649fc30be6ec 83
Nike3221 0:649fc30be6ec 84 }
Nike3221 0:649fc30be6ec 85
mosukro 1:fa44a6246bcc 86 void move()
Nike3221 0:649fc30be6ec 87 {
mosukro 1:fa44a6246bcc 88 //動作
mosukro 2:be9046fb5859 89 switch (count)
mosukro 2:be9046fb5859 90 {
mosukro 2:be9046fb5859 91 case 0:
mosukro 3:efaa785283a6 92 //パラシュートを焼き切るまで待つ
mosukro 3:efaa785283a6 93 //焼き切ったらcount+1
mosukro 4:cfaf33b2c97a 94 //緯度longitude　経度latitude,
mosukro 4:cfaf33b2c97a 95 //pc.printf("case0");
mosukro 4:cfaf33b2c97a 96 if( (high_target+1.0)>= high )
mosukro 4:cfaf33b2c97a 97 {
mosukro 4:cfaf33b2c97a 98 pc.printf("hight_START");
mosukro 4:cfaf33b2c97a 99 //左右のモーターを動かす
mosukro 4:cfaf33b2c97a 100 moterl.pulsewidth(palse); //パルス幅
mosukro 4:cfaf33b2c97a 101 moterr.pulsewidth(palse);
mosukro 4:cfaf33b2c97a 102 //焼き切り開始
mosukro 4:cfaf33b2c97a 103 led1=1;
mosukro 4:cfaf33b2c97a 104 //fet3=1;
mosukro 4:cfaf33b2c97a 105 wait(80);//焼き切り時間1分
mosukro 4:cfaf33b2c97a 106 led1=0;
mosukro 4:cfaf33b2c97a 107 angle_target = 90-atan2(sin(latitude_target-latitude),cos(longitude)*tan(longitude_target)-sin(longitude)*cos(latitude_target-latitude));
mosukro 4:cfaf33b2c97a 108 if((angle_target+angle)>360){angle_targetmath = (int(angle_target)+int(angle))%360;}
mosukro 4:cfaf33b2c97a 109 else{angle_targetmath = angle_target+angle;}
mosukro 4:cfaf33b2c97a 110 pc.printf("%f %d \n",angle_target,angle_targetmath);
mosukro 4:cfaf33b2c97a 111 count++;
mosukro 4:cfaf33b2c97a 112 }
mosukro 2:be9046fb5859 113 break;
mosukro 2:be9046fb5859 114 case 1:
mosukro 3:efaa785283a6 115 //落ちてパラシュートを焼き切った後
mosukro 4:cfaf33b2c97a 116 pc.printf("case1");
mosukro 4:cfaf33b2c97a 117 if( (angle_targetmath+5)>=int(angle) && (angle_targetmath-5)<=int(angle) )//3は適当な数字。誤差範囲
mosukro 3:efaa785283a6 118 {
mosukro 3:efaa785283a6 119 //角度が合ったらcount+1
mosukro 4:cfaf33b2c97a 120 count++;
mosukro 4:cfaf33b2c97a 121
mosukro 3:efaa785283a6 122 }
mosukro 3:efaa785283a6 123 else
mosukro 3:efaa785283a6 124 {
mosukro 3:efaa785283a6 125 //角度が合うまで回転
mosukro 4:cfaf33b2c97a 126 if(angle_targetmath>=angle)
mosukro 3:efaa785283a6 127 {
mosukro 3:efaa785283a6 128 //differ_angle=angle_target-angle;
mosukro 3:efaa785283a6 129 //moterlを動かす
mosukro 4:cfaf33b2c97a 130 moterl.pulsewidth(palse);
mosukro 4:cfaf33b2c97a 131 pc.printf("L \n");
mosukro 3:efaa785283a6 132 }
mosukro 4:cfaf33b2c97a 133 else if(angle>=angle_targetmath)
mosukro 3:efaa785283a6 134 {
mosukro 3:efaa785283a6 135 //differ_angle=angle-angle_target;
mosukro 3:efaa785283a6 136 //moterrを動かす
mosukro 4:cfaf33b2c97a 137 moterr.pulsewidth(palse);
mosukro 4:cfaf33b2c97a 138 pc.printf("R \n");
mosukro 3:efaa785283a6 139 }
mosukro 3:efaa785283a6 140 }
mosukro 2:be9046fb5859 141 break;
mosukro 2:be9046fb5859 142 case 2:
mosukro 4:cfaf33b2c97a 143 //pc.printf("case2");
mosukro 3:efaa785283a6 144 //目標地点の近くまで走行
mosukro 3:efaa785283a6 145 //目標地点に近づいたらcount+1
mosukro 4:cfaf33b2c97a 146 //緯度longitude　経度latitude,
mosukro 4:cfaf33b2c97a 147 longitudegosa = longitude_target-longitude;
mosukro 4:cfaf33b2c97a 148 latitudegosa = latitude_target-latitude;
mosukro 4:cfaf33b2c97a 149
mosukro 4:cfaf33b2c97a 150 if( abs(longitudegosa)<=50 && abs(latitudegosa)<=50 )
mosukro 4:cfaf33b2c97a 151 {
mosukro 4:cfaf33b2c97a 152 //longitude =;
mosukro 4:cfaf33b2c97a 153 //latitude =;
mosukro 4:cfaf33b2c97a 154 count++;
mosukro 4:cfaf33b2c97a 155 }
mosukro 4:cfaf33b2c97a 156 else
mosukro 4:cfaf33b2c97a 157 {
mosukro 4:cfaf33b2c97a 158 pc.printf("front \n");
mosukro 4:cfaf33b2c97a 159 moterl.pulsewidth(palse); //パルス幅
mosukro 4:cfaf33b2c97a 160 moterr.pulsewidth(palse);
mosukro 4:cfaf33b2c97a 161 }
mosukro 3:efaa785283a6 162 break;
mosukro 3:efaa785283a6 163 case 3:
mosukro 4:cfaf33b2c97a 164 pc.printf("case3");
mosukro 3:efaa785283a6 165 //動作を停止。割り込みも停止させ、回収待ち
mosukro 3:efaa785283a6 166 //moterl=moterr=0;
mosukro 3:efaa785283a6 167 //ngps.detach();
mosukro 4:cfaf33b2c97a 168 count++;
mosukro 2:be9046fb5859 169 break;
mosukro 2:be9046fb5859 170 default:
mosukro 4:cfaf33b2c97a 171 moterl.pulsewidth(0.0);
mosukro 4:cfaf33b2c97a 172 moterr.pulsewidth(0.0);
mosukro 2:be9046fb5859 173 break;
mosukro 3:efaa785283a6 174 }
Nike3221 0:649fc30be6ec 175 }
mosukro 1:fa44a6246bcc 176
mosukro 1:fa44a6246bcc 177 int main()
mosukro 1:fa44a6246bcc 178 {
mosukro 4:cfaf33b2c97a 179 pc.printf("START! \n");
mosukro 1:fa44a6246bcc 180 gps.baud(9600);
mosukro 1:fa44a6246bcc 181 gps.attach(gps_rx, Serial::RxIrq);//ＧＰＳ割り込み
mosukro 4:cfaf33b2c97a 182 //pc.attach(gps_rx, Serial::RxIrq);
mosukro 2:be9046fb5859 183 sscanf(gps_target,"$GPRMC,%f,A,%f,N,%f,E,%f,%f",&gpstime_target,&longitude_target,&latitude_target,&knot_target,&angle_target);
mosukro 4:cfaf33b2c97a 184 pc.printf("%f %f %f %f %f \n",gpstime_target,longitude_target,latitude_target,knot_target,angle_target);
mosukro 4:cfaf33b2c97a 185 janpa1=1;
mosukro 4:cfaf33b2c97a 186 while(janpa2!=0);
mosukro 4:cfaf33b2c97a 187 pc.printf("PINOK");
mosukro 4:cfaf33b2c97a 188 moterl.period(0.02); // 周期
mosukro 4:cfaf33b2c97a 189 moterr.period(0.02);
Nike3221 0:649fc30be6ec 190 while(1)
Nike3221 0:649fc30be6ec 191 {
mosukro 4:cfaf33b2c97a 192 //pc.putc(gps.getc());
mosukro 4:cfaf33b2c97a 193 //pc.printf(",");
mosukro 4:cfaf33b2c97a 194 //moterl.pulsewidth(0.0020); //パルス幅
mosukro 4:cfaf33b2c97a 195 //moterr.pulsewidth(0.0020);
mosukro 1:fa44a6246bcc 196 move();
Nike3221 0:649fc30be6ec 197 }
mosukro 1:fa44a6246bcc 198
Nike3221 0:649fc30be6ec 199 }