CORE
VertexのFMに使ったプログラムです．
Dependencies: MPU6050 MS5607 SDFileSystem mbed
Diff: main.cpp
- Revision:
- 0:8483298c086d
--- /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