![](/media/cache/group/default_image.jpg.50x50_q85.jpg)
VertexのFMに使ったプログラムです.
Dependencies: MPU6050 MS5607 SDFileSystem mbed
Revision 0:8483298c086d, committed 2016-11-20
- Comitter:
- naoki_westwell
- Date:
- Sun Nov 20 05:06:08 2016 +0000
- Commit message:
- Vertex FM
Changed in this revision
diff -r 000000000000 -r 8483298c086d MPU6050.lib --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/MPU6050.lib Sun Nov 20 05:06:08 2016 +0000 @@ -0,0 +1,1 @@ +https://developer.mbed.org/users/naoki_westwell/code/MPU6050/#52305bcb5cda
diff -r 000000000000 -r 8483298c086d MS5607.lib --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/MS5607.lib Sun Nov 20 05:06:08 2016 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/yamaguch/code/MS5607/#5760862143d1
diff -r 000000000000 -r 8483298c086d SDFileSystem.lib --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/SDFileSystem.lib Sun Nov 20 05:06:08 2016 +0000 @@ -0,0 +1,1 @@ +http://developer.mbed.org/users/mbed_official/code/SDFileSystem/#7b35d1709458
diff -r 000000000000 -r 8483298c086d main.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main.cpp Sun Nov 20 05:06:08 2016 +0000 @@ -0,0 +1,430 @@ +#include "mbed.h" +#include "MPU6050.h" +#include "MS5607I2C.h" +#include "SDFileSystem.h" +#define MAX_RDATA 256 //最大受信コマンド +#define MAX_TOTAL_DATA 256 //最大データフレーム +#define DATA_BUNDLE 10 //SDへ1度に保存するデータフレーム +#define DEPLOY_TIME_1 18000 //発射からの1回目のサーボ駆動(ミリ秒) 18秒 +#define DEPLOY_TIME_2 65500 //2回目のサーボ駆動(ミリ秒) 83.5-18秒 +#define DEPLOY_ALT2 300 //パラ2開放高度(m) +#define DUTY_BASE 0.0975 //デューティー比 ベース 136*0.001/2 + 0.03 +#define DUTY_1 0.084 // 開放1 108*0.001/2 + 0.03 +#define DUTY_2 0.069 // 開放2 78*0.001/2 + 0.03 + +Serial sci(p9, p10); //通信線 +//Serial sci(USBTX, USBRX); +BusOut myleds(LED1, LED2, LED3, LED4); //LED +//Serial pc(USBTX, USBRX); //USB +SDFileSystem sd(p5, p6, p7, p8, "sd"); //MicroSDCard +Serial uar(p13,p14); //GPS +I2C i2c(p28, p27); //音声合成 +const int tlk_addr = 0x2E<<1; //音声合成LSIのアドレス +MPU6050 mpu; //MPU6050(加速度ジャイロ) +MS5607I2C ms5607(p28, p27, false); //MS5607(気圧) +Ticker ticker; //タイマー割り込み +extern "C" void mbed_reset(); //リセット関数の宣言 +Timer timer; //タイマ +FILE *fp; //SDカード +PwmOut servo(p21); //サーボモータ +DigitalOut f_pin(p20); //フライトピン +Timer xtimer; //打上時間通達用のタイマー +Timer t; //デバッグ用 + +unsigned int pre_cnt=0; //スタンバイモードのカウント +unsigned int cnt=0; //カウント(ループ回数) +int32_t press, temp; //気圧, 気温 +float altitude; //高度 +int16_t ax, ay, az; //3軸加速度 +int16_t acc; //合成加速度 +int16_t gx, gy, gz; //3軸ジャイロ +unsigned int ms_time; //タイマー時間 +//unsigned int voice_state; //喋っている状況 +unsigned int mode=0; //モード 0:スタンバイモード 1:フライトモード 2:発射 3or4:開放 5:着地 + +//RTC +struct tm rtc; +time_t seconds; +int time_h=0,time_m=0,time_s=0; +int xtime_h=0,xtime_m=0,xtime_s=0; +char time_buf[32]; + +//gps +int gps_i, rlock, stn; +char gps_data[256]; +char ns,ew; +float gps_time,hokui,tokei; +unsigned int g_hokui, g_tokei; +unsigned int htime, mtime, stime; + +const float R = 287.052; // specific gas constant R*/M0 +const float g = 9.80665; // standard gravity +const float t_grad = 0.0065; // gradient of temperature +const float t0 = 273.15 + 15; // temperature at 0 altitude +const float p0 = 101325; // pressure at 0 altitude + +unsigned int servo_duty; //サーボデューティー比 +unsigned int launch_time; //発射した時間 +unsigned int deploy_time_1; //開放1した時間 +unsigned int deploy_time_2; //開放2した時間 +float gnd_alt, gnd_temp; //地上の高度 +float vertex=0.0; //最高高度 +int pre_acc, acc_sw; //前の合成加速度 +int dep_sw; //開放タイミング +int silent=0; //サイレントモード +int servo_sw=0; //サーボ保持やめるタイミング +int servo_sw2=0; //開放2後一定間隔でサーボを動かすタイミング + +char total_data[256]; //データフレーム +char data_buffer[DATA_BUNDLE][256]; //データバッファ(SDへ一度に保存する) +char Rdata[MAX_RDATA]; //受信データfrom通信線 + +char spk_line[][256] = { + "suta-toa,pu.\r", + "konnnichiwa.watashiwa,ti'-mu,;ba-te,ku'suno,ro/ke',toni,tousaisareteiru,jinnkouti'nou,ri',tu/de_su.;yorosiku.\r", + "ze'nnkaino,ri/se',to/kara <NUMK VAL=8 COUNTER=funn> <NUMK VAL=10 COUNTER=byo-> keikasimasi'ta.\r", + ";uchiageyotei,ji'kokumade,a'to <NUMK VAL=8 COUNTER=funn> <NUMK VAL=59 COUNTER=byo-> de_su.\r", + "rise,to.\r", + "esudhi-ka-/do,rise,to.\r", + "tijoukou'do/wo,sokuteityuude_su,sibara+ku,omachikudasa'i.\r", + "tijoukou'dono,so/kuteiga,kanryousimasi'ta.\r", + ";huraitomo-'do,;o'nn.\r", + ";de'-tano/kirokuto,keisokuga,kaisisarema'sita.\r", + ";rihuto,;o'hu rokextuxtu'toga,uchiagerarema'sita.\r", + "parashu-'to,ichi,hou+shutu.\r", + "parashu-'to,ni',hou+shutu.\r", + "sa-bomo-'ta-,kudo-.\r", + "rokextuxtu'towa,rakkatyuude_su.\r", + "rokextuxtu'tono,tyakutiwo,kakuninnsima'sita.\r", + ";uchiageyotei,ji'koku/wo,setteisima'sita.\r", + ";genzaiji'koku/wo,setteisima'sita.\r", + "huraito'pin,go'-.\r", + "huraito'pin,no-go'-.setuzokuwo/kakuninnsitekudasa'i.\r", + "sairento/mo'-do,o'hu.\r", + "ge'nzai,kaihousagyoutyuude'_su.\r", + "sintyokudo'ude_suka?\r", + "#J", + "#K", + }; + +//プロトタイプ宣言 +void tick(); //データ取得割り込み +void sci_rx(); //通信線コマンド割り込み +void gps_rx(); //GPS通信線コマンド割り込み +void deployment(); //開放アルゴリズム +void talk_line(int num); //指定したセリフを喋る +void random_talk(); //ランダムで喋る + +//////////// +//メイン関数// +//////////// +int main() { + talk_line(0); + //Real Time Clock + rtc.tm_sec = 00; // 0-59 + rtc.tm_min = 00; // 0-59 + rtc.tm_hour = 00; // 0-23 + rtc.tm_year = 115; // year since 1900 + seconds = mktime(&rtc); //時刻生成 + set_time(seconds); //時刻設定 + + servo.period_ms(20); //サーボ波長 + servo = DUTY_BASE; //サーボをベース状態 + wait(1); servo.pulsewidth_us(0); //保持やめる + mpu.initialize(); //MPU6050(加速度ジャイロ)初期 + sci.attach(sci_rx, Serial::RxIrq); //シリアル受信割り込み + uar.baud(4800); //GPS + uar.attach(gps_rx, Serial::RxIrq); //GPS割り込み + timer.start(); //タイマ開始 + timer.reset(); //タイマ初期化 + xtimer.start(); //タイマ開始 + xtimer.reset(); //タイマ初期化 + for(int i=0;i<4;i++){myleds=myleds|1<<i;wait(0.4);} myleds=0x06; + sci.baud(9600); + sci.printf("\n\n******************************\n"); + sci.printf("**********START UP!!**********\n"); + sci.printf("******************************\n"); + talk_line(1); + + while(1){ + //RTC + seconds = time(NULL); + strftime(time_buf,sizeof(time_buf),"%H,%M,%S",localtime(&seconds)); + sscanf(time_buf,"%d,%d,%d",&time_h,&time_m,&time_s); + if(mode==0){ + pre_cnt++; + random_talk(); + myleds=myleds^0xff; wait(0.1); + } + else + if(mode){ + + //SDへの記録 + + if(cnt%DATA_BUNDLE==DATA_BUNDLE-1){ + fp = fopen("/sd/data.csv", "a"); + if(fp==NULL){ + sci.printf("<!>Error<!> ---> Could not open file\r\n"); + }else{ + for(int i=0; i<DATA_BUNDLE; i++){ //DATA_BUDLE分のフレームを一度に保存 + fprintf(fp, "%s", data_buffer[i]); + sprintf(data_buffer[i], ""); + } + fclose(fp); + } + } + } + } +} +////////////////// +//データ取得割り込み// +////////////////// +void tick(){ + t.start(); + t.reset(); + ms_time = timer.read_ms(); //タイマー(ミリ秒) + + press = ms5607.getPressure(); //気圧取得 + temp = ms5607.getTemperature()*100; //気温取得 + altitude = t0/t_grad*(1-exp((t_grad*R/g)*log(press/p0)))-gnd_alt; //高度取得 + if(altitude<2000 && altitude>vertex) vertex=altitude; + if(vertex-altitude>2){dep_sw++;}else{dep_sw=0;} + deployment(); + + mpu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz); //加速度ジャイロ取 + + ax=float(ax); ay=float(ay); az=float(az); + gx=float(gx); gy=float(gy); gz=float(gz); + ax=float(ax)/2048*10000; ay=float(ay)/2048*10000; az=float(az)/2048*10000; + gx=float(gx)/65.5*100; gy=float(gy)/65.5*100; gz=float(gz)/65.5*100; + pre_acc = acc; + acc = int(sqrt(double(ax*ax+ay*ay+az*az))); //合成加速度 + + //データフレームへ格納 + sprintf(total_data,"%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%2d%2d%2d,%1d,%d,%2d%2d%2d,%d\r\n", + press,temp,ax,ay,az,gx,gy,gz, + g_hokui, g_tokei, htime, mtime, stime, + mode, ms_time, time_h, time_m, time_s, cnt + ); + sprintf(data_buffer[cnt%DATA_BUNDLE],"%s",total_data); //データバッファへ格納 + //sci.printf("%s",total_data); //通信線へ + //pc.printf("%s",total_data); //PCへ + //LED Flip-flops + if(cnt%6<4) myleds = 1 << cnt%6; + else myleds = 4 >> (cnt%6-4); + cnt++; //カウント + + //sci.printf("%f\r\n",t.read()); +} +//////////////////// +//通信コマンド割り込み// +//////////////////// +unsigned short sci_i=0; //コマンドN文字目 +char cmd[3]; //コマンド命令 +char val[MAX_RDATA-3]; //コマンド引数 +void sci_rx () { + //受信データが大きすぎる場合 + if(sci_i>MAX_RDATA-1){ + sci_i = MAX_RDATA-1; + Rdata[sci_i]='\n'; + } + Rdata[sci_i]=sci.getc(); //受信データの格納 + if(Rdata[sci_i]=='\n'){ //受信データ区切り→コマンドごとの処理 + Rdata[sci_i-1]='\0'; + sci_i=0; + + sci.printf("\nReceved data: \"%s\" \r\n", Rdata); + sscanf(Rdata,"%3s %s", cmd, val); //コマンドを命令と引数に分割 + //リセット・コマンド + if(strcmp(cmd, "RST")==0){ + sci.printf("---> Mbed reset \r\n"); talk_line(4); + wait(0.1); mbed_reset(); + }else + //フライトモード・コマンド + if(strcmp(cmd, "FLT")==0){ + sci.printf("---> Flight-Mode ON! \r\n"); talk_line(8); + mode=1; //フライトモードオン!! + ticker.attach(&tick, 0.1); //タイマー割り込み + timer.start(); //タイマ開始 + timer.reset(); //タイマ初期化 + }else + //SDリセット・コマンド + if(strcmp(cmd, "SDR")==0){ + fp = fopen("/sd/data.csv", "w"); + if(fp==NULL){ sci.printf("<!>Error<!> ---> Could not open file\r\n");} + else{ fprintf(fp, ""); fclose(fp); sci.printf("---> SDcard Reset \r\n"); talk_line(5);} + wait(0.1); + }else + //地上高度測定コマンド + if(strcmp(cmd, "ALT")==0){ + sci.printf("---> Measureing state of rest\r\n"); talk_line(6); + for(int i=0;i<100;i++){ + gnd_alt += t0/t_grad*(1-exp((t_grad*R/g)*log(ms5607.getPressure()/p0))); + if(i%10==9){ sci.printf("@"); myleds=myleds^0xff; } + wait(0.1); + } + gnd_alt/=100; + sci.printf("\n---> Measurement Complete\r\n"); talk_line(7); + }else + //現在時刻設定 + if(strcmp(cmd, "SCT")==0){ + sci.printf("---> Set Current Time\r\n"); talk_line(17); + rtc.tm_hour = int(atof(val)/10000); // 0-23 + rtc.tm_min = int((atof(val)-rtc.tm_hour*10000)/100); // 0-59 + rtc.tm_sec = int(atof(val)-rtc.tm_hour*10000-rtc.tm_min*100); // 0-59 + seconds = mktime(&rtc); + set_time(seconds); + }else + //打上時刻設定設定 + if(strcmp(cmd, "SXT")==0){ + sci.printf("---> Set Launch Time\r\n"); talk_line(16); + xtime_h = int(atof(val)/10000); // 0-23 + xtime_m = int((atof(val)-xtime_h*10000)/100); // 0-59 + xtime_s = int(atof(val)-xtime_h*10000-xtime_m*100); // 0-59 + }else + //パルス設定プログラム + if(strcmp(cmd, "PWM")==0){ + servo = atof(val)*0.001/2 + 0.03; + wait(1); servo.pulsewidth_us(0); + sci.printf("\n---> Set Pulse Width\r\n"); talk_line(13); + }else + //フライトピンチェック + if(strcmp(cmd, "FPC")==0){ + if(f_pin){sci.printf("---> Flight Pin Go !\r\n");talk_line(18);} + else{sci.printf("<!><!><!> Flight Pin No Go <!><!><!>\r\n");talk_line(19);} + }else + //サイレントモード + if(strcmp(cmd, "SLT")==0){ + if(silent==0){sci.printf("---> Silent Mode On !\r\n");silent=1;} + else{sci.printf("---> Silent Mode OFF \r\n");silent=0;talk_line(20);} + }else + //スピーク・コマンド + if(strcmp(cmd, "SPK")==0){ + int line_num = atoi(val); + sci.printf("---> Now talking \"%s\" \r\n", spk_line[line_num]); + talk_line(line_num); + }else + //トーク・コマンド + if(strcmp(cmd, "TLK")==0){ + if(strlen(val)+4 > MAX_RDATA-3){ + sci.printf("<!>Error<!> ---> Exceeds size limit\r\n"); + }else{ + sci.printf("---> Now talking \"%s\" \r\n", val); + strcat(val, ".\r"); + i2c.write(tlk_addr, val, strlen(val)); + } + }else{ + //エラー + sci.printf("<!>Error<!> ---> Unknown command \r\n"); + } + strcpy(cmd, ""); strcpy(val, ""); + }else{ //受信データ区切りでないなら格納続ける + sci_i++; + } +} + +//番号を指定してセリフを喋るよ +void talk_line(int num){ + if(silent==0) i2c.write(tlk_addr, spk_line[num], strlen(spk_line[num])); +} + +void gps_rx () {//I2C Interrupt RX + if( (gps_data[gps_i]=uar.getc()) != '$'){ + gps_i++; + }else{ + gps_data[gps_i]='\0'; + if ( sscanf(gps_data, "GPGGA,%f,%f,%c,%f,%c,%d,%d",&gps_time,&hokui,&ns,&tokei,&ew,&rlock,&stn) >= 1) { + g_hokui=(int(hokui/100)+(hokui-int(hokui/100))/60)*1000000; + g_tokei=(int(tokei/100)+(tokei-int(tokei/100))/60)*1000000; + + //time set + htime=int(gps_time/10000); mtime=int((gps_time-htime*10000)/100); stime=int(gps_time-htime*10000-mtime*100); + htime=htime+9;//UTC =>JST + } + gps_i=0; + sprintf(gps_data,""); + } +} + +void random_talk(){ + int rnd = timer.read_us()%6; + char msg[128]; + if(silent==0 && pre_cnt%600==0){ + if(rnd>=0 && rnd<=1){ + float nishitime=((xtime_h*3600+xtime_m*60+xtime_s)-(time_h*3600+time_m*60+time_s)); + int ato_m = int(nishitime/60); int ato_s = int(nishitime-ato_m*60); + sprintf(msg,";uchiageyotei,ji'kokumade,a'to <NUMK VAL=%d COUNTER=funn> <NUMK VAL=%d COUNTER=byo-> de_su.\r", + ato_m, ato_s); + }else + if(rnd>=2 && rnd<=3){ + sprintf(msg,";uchiageyotei,ji'kokuwa <NUMK VAL=%d COUNTER=ji> <NUMK VAL=%d COUNTER=funn> <NUMK VAL=%d COUNTER=byo-> de_su.\r", + xtime_h, xtime_m, xtime_s); + }else + if(rnd==4){ + sprintf(msg,"sintyokuwa,;do'ude_suka?ganbattekudasai\r"); + }else + if(rnd==5){ + sprintf(msg,"watasini'wa,joukuude,hutatu'no,parashu-'towo,houshutusuru'toiu,sekininngaarima_su.mina'sann,o-enn,yorosikuonegaisima_su.\r"); + } + i2c.write(tlk_addr, msg, strlen(msg)); + } +} + +void deployment(){ + //サーボ保持解除 + if(servo_sw){ + servo_sw++; + if(servo_sw>10) { + servo.pulsewidth_us(0); + servo_sw=0; + } + } + //発射判定 + if(mode==1){ + if(acc > 30000){ + mode=2; talk_line(10); sci.printf("Lift off!! (accel)\r\n"); timer.reset(); + launch_time = timer.read_ms(); + }else + if(f_pin==0){ + mode=3; talk_line(10); sci.printf("Lift off!! (flight pin)\r\n"); timer.reset(); + launch_time = timer.read_ms(); + } + }else + //開放判定1 + if(mode==2 || mode==3){ + if(ms_time-launch_time > DEPLOY_TIME_1){ //タイマー開放 + mode=4; servo = DUTY_1; servo_sw++; + sci.printf("Parachute1 deployed (timer)\r\n"); + deploy_time_1 = timer.read_ms(); talk_line(11); + }else + if(ms_time-launch_time>8000 && dep_sw==2){ //高度開放 + mode=5; servo = DUTY_1; servo_sw++; + sci.printf("Parachute1 deployed (altitude)\r\n"); + deploy_time_1 = timer.read_ms(); talk_line(11); + } + }else + //開放判定2 + if(mode==4 || mode==5){ + if(ms_time-deploy_time_1 > DEPLOY_TIME_2){ //タイマー開放 + mode=6; servo = DUTY_2; servo_sw++; + sci.printf("Parachute2 deployed (timer)\r\n"); + deploy_time_2 = timer.read_ms(); talk_line(12); + }else + if(ms_time-deploy_time_1>8000 && altitude<DEPLOY_ALT2){ + mode=7; servo = DUTY_2; servo_sw++; + sci.printf("Parachute2 deployed (altitude)\r\n"); + deploy_time_2 = timer.read_ms(); talk_line(12); + } + }else + //着地判定 + if(mode==6 || mode==7){ + servo_sw2++; + if(servo_sw2%10==0) { servo = DUTY_2; servo_sw++; } + if(abs(acc-pre_acc)<1000){ + acc_sw++; + }else{acc_sw=0;} + if(acc_sw==10){ + sci.printf("Rocket Landing! \r\n"); talk_line(15); mode=8; + } + } +} \ No newline at end of file
diff -r 000000000000 -r 8483298c086d mbed.bld --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/mbed.bld Sun Nov 20 05:06:08 2016 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/mbed_official/code/mbed/builds/9ad691361fac \ No newline at end of file