VertexのFMに使ったプログラムです.
Dependencies: MPU6050 MS5607 SDFileSystem mbed
main.cpp@0:8483298c086d, 2016-11-20 (annotated)
- Committer:
- naoki_westwell
- Date:
- Sun Nov 20 05:06:08 2016 +0000
- Revision:
- 0:8483298c086d
Vertex FM
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
naoki_westwell | 0:8483298c086d | 1 | #include "mbed.h" |
naoki_westwell | 0:8483298c086d | 2 | #include "MPU6050.h" |
naoki_westwell | 0:8483298c086d | 3 | #include "MS5607I2C.h" |
naoki_westwell | 0:8483298c086d | 4 | #include "SDFileSystem.h" |
naoki_westwell | 0:8483298c086d | 5 | #define MAX_RDATA 256 //最大受信コマンド |
naoki_westwell | 0:8483298c086d | 6 | #define MAX_TOTAL_DATA 256 //最大データフレーム |
naoki_westwell | 0:8483298c086d | 7 | #define DATA_BUNDLE 10 //SDへ1度に保存するデータフレーム |
naoki_westwell | 0:8483298c086d | 8 | #define DEPLOY_TIME_1 18000 //発射からの1回目のサーボ駆動(ミリ秒) 18秒 |
naoki_westwell | 0:8483298c086d | 9 | #define DEPLOY_TIME_2 65500 //2回目のサーボ駆動(ミリ秒) 83.5-18秒 |
naoki_westwell | 0:8483298c086d | 10 | #define DEPLOY_ALT2 300 //パラ2開放高度(m) |
naoki_westwell | 0:8483298c086d | 11 | #define DUTY_BASE 0.0975 //デューティー比 ベース 136*0.001/2 + 0.03 |
naoki_westwell | 0:8483298c086d | 12 | #define DUTY_1 0.084 // 開放1 108*0.001/2 + 0.03 |
naoki_westwell | 0:8483298c086d | 13 | #define DUTY_2 0.069 // 開放2 78*0.001/2 + 0.03 |
naoki_westwell | 0:8483298c086d | 14 | |
naoki_westwell | 0:8483298c086d | 15 | Serial sci(p9, p10); //通信線 |
naoki_westwell | 0:8483298c086d | 16 | //Serial sci(USBTX, USBRX); |
naoki_westwell | 0:8483298c086d | 17 | BusOut myleds(LED1, LED2, LED3, LED4); //LED |
naoki_westwell | 0:8483298c086d | 18 | //Serial pc(USBTX, USBRX); //USB |
naoki_westwell | 0:8483298c086d | 19 | SDFileSystem sd(p5, p6, p7, p8, "sd"); //MicroSDCard |
naoki_westwell | 0:8483298c086d | 20 | Serial uar(p13,p14); //GPS |
naoki_westwell | 0:8483298c086d | 21 | I2C i2c(p28, p27); //音声合成 |
naoki_westwell | 0:8483298c086d | 22 | const int tlk_addr = 0x2E<<1; //音声合成LSIのアドレス |
naoki_westwell | 0:8483298c086d | 23 | MPU6050 mpu; //MPU6050(加速度ジャイロ) |
naoki_westwell | 0:8483298c086d | 24 | MS5607I2C ms5607(p28, p27, false); //MS5607(気圧) |
naoki_westwell | 0:8483298c086d | 25 | Ticker ticker; //タイマー割り込み |
naoki_westwell | 0:8483298c086d | 26 | extern "C" void mbed_reset(); //リセット関数の宣言 |
naoki_westwell | 0:8483298c086d | 27 | Timer timer; //タイマ |
naoki_westwell | 0:8483298c086d | 28 | FILE *fp; //SDカード |
naoki_westwell | 0:8483298c086d | 29 | PwmOut servo(p21); //サーボモータ |
naoki_westwell | 0:8483298c086d | 30 | DigitalOut f_pin(p20); //フライトピン |
naoki_westwell | 0:8483298c086d | 31 | Timer xtimer; //打上時間通達用のタイマー |
naoki_westwell | 0:8483298c086d | 32 | Timer t; //デバッグ用 |
naoki_westwell | 0:8483298c086d | 33 | |
naoki_westwell | 0:8483298c086d | 34 | unsigned int pre_cnt=0; //スタンバイモードのカウント |
naoki_westwell | 0:8483298c086d | 35 | unsigned int cnt=0; //カウント(ループ回数) |
naoki_westwell | 0:8483298c086d | 36 | int32_t press, temp; //気圧, 気温 |
naoki_westwell | 0:8483298c086d | 37 | float altitude; //高度 |
naoki_westwell | 0:8483298c086d | 38 | int16_t ax, ay, az; //3軸加速度 |
naoki_westwell | 0:8483298c086d | 39 | int16_t acc; //合成加速度 |
naoki_westwell | 0:8483298c086d | 40 | int16_t gx, gy, gz; //3軸ジャイロ |
naoki_westwell | 0:8483298c086d | 41 | unsigned int ms_time; //タイマー時間 |
naoki_westwell | 0:8483298c086d | 42 | //unsigned int voice_state; //喋っている状況 |
naoki_westwell | 0:8483298c086d | 43 | unsigned int mode=0; //モード 0:スタンバイモード 1:フライトモード 2:発射 3or4:開放 5:着地 |
naoki_westwell | 0:8483298c086d | 44 | |
naoki_westwell | 0:8483298c086d | 45 | //RTC |
naoki_westwell | 0:8483298c086d | 46 | struct tm rtc; |
naoki_westwell | 0:8483298c086d | 47 | time_t seconds; |
naoki_westwell | 0:8483298c086d | 48 | int time_h=0,time_m=0,time_s=0; |
naoki_westwell | 0:8483298c086d | 49 | int xtime_h=0,xtime_m=0,xtime_s=0; |
naoki_westwell | 0:8483298c086d | 50 | char time_buf[32]; |
naoki_westwell | 0:8483298c086d | 51 | |
naoki_westwell | 0:8483298c086d | 52 | //gps |
naoki_westwell | 0:8483298c086d | 53 | int gps_i, rlock, stn; |
naoki_westwell | 0:8483298c086d | 54 | char gps_data[256]; |
naoki_westwell | 0:8483298c086d | 55 | char ns,ew; |
naoki_westwell | 0:8483298c086d | 56 | float gps_time,hokui,tokei; |
naoki_westwell | 0:8483298c086d | 57 | unsigned int g_hokui, g_tokei; |
naoki_westwell | 0:8483298c086d | 58 | unsigned int htime, mtime, stime; |
naoki_westwell | 0:8483298c086d | 59 | |
naoki_westwell | 0:8483298c086d | 60 | const float R = 287.052; // specific gas constant R*/M0 |
naoki_westwell | 0:8483298c086d | 61 | const float g = 9.80665; // standard gravity |
naoki_westwell | 0:8483298c086d | 62 | const float t_grad = 0.0065; // gradient of temperature |
naoki_westwell | 0:8483298c086d | 63 | const float t0 = 273.15 + 15; // temperature at 0 altitude |
naoki_westwell | 0:8483298c086d | 64 | const float p0 = 101325; // pressure at 0 altitude |
naoki_westwell | 0:8483298c086d | 65 | |
naoki_westwell | 0:8483298c086d | 66 | unsigned int servo_duty; //サーボデューティー比 |
naoki_westwell | 0:8483298c086d | 67 | unsigned int launch_time; //発射した時間 |
naoki_westwell | 0:8483298c086d | 68 | unsigned int deploy_time_1; //開放1した時間 |
naoki_westwell | 0:8483298c086d | 69 | unsigned int deploy_time_2; //開放2した時間 |
naoki_westwell | 0:8483298c086d | 70 | float gnd_alt, gnd_temp; //地上の高度 |
naoki_westwell | 0:8483298c086d | 71 | float vertex=0.0; //最高高度 |
naoki_westwell | 0:8483298c086d | 72 | int pre_acc, acc_sw; //前の合成加速度 |
naoki_westwell | 0:8483298c086d | 73 | int dep_sw; //開放タイミング |
naoki_westwell | 0:8483298c086d | 74 | int silent=0; //サイレントモード |
naoki_westwell | 0:8483298c086d | 75 | int servo_sw=0; //サーボ保持やめるタイミング |
naoki_westwell | 0:8483298c086d | 76 | int servo_sw2=0; //開放2後一定間隔でサーボを動かすタイミング |
naoki_westwell | 0:8483298c086d | 77 | |
naoki_westwell | 0:8483298c086d | 78 | char total_data[256]; //データフレーム |
naoki_westwell | 0:8483298c086d | 79 | char data_buffer[DATA_BUNDLE][256]; //データバッファ(SDへ一度に保存する) |
naoki_westwell | 0:8483298c086d | 80 | char Rdata[MAX_RDATA]; //受信データfrom通信線 |
naoki_westwell | 0:8483298c086d | 81 | |
naoki_westwell | 0:8483298c086d | 82 | char spk_line[][256] = { |
naoki_westwell | 0:8483298c086d | 83 | "suta-toa,pu.\r", |
naoki_westwell | 0:8483298c086d | 84 | "konnnichiwa.watashiwa,ti'-mu,;ba-te,ku'suno,ro/ke',toni,tousaisareteiru,jinnkouti'nou,ri',tu/de_su.;yorosiku.\r", |
naoki_westwell | 0:8483298c086d | 85 | "ze'nnkaino,ri/se',to/kara <NUMK VAL=8 COUNTER=funn> <NUMK VAL=10 COUNTER=byo-> keikasimasi'ta.\r", |
naoki_westwell | 0:8483298c086d | 86 | ";uchiageyotei,ji'kokumade,a'to <NUMK VAL=8 COUNTER=funn> <NUMK VAL=59 COUNTER=byo-> de_su.\r", |
naoki_westwell | 0:8483298c086d | 87 | "rise,to.\r", |
naoki_westwell | 0:8483298c086d | 88 | "esudhi-ka-/do,rise,to.\r", |
naoki_westwell | 0:8483298c086d | 89 | "tijoukou'do/wo,sokuteityuude_su,sibara+ku,omachikudasa'i.\r", |
naoki_westwell | 0:8483298c086d | 90 | "tijoukou'dono,so/kuteiga,kanryousimasi'ta.\r", |
naoki_westwell | 0:8483298c086d | 91 | ";huraitomo-'do,;o'nn.\r", |
naoki_westwell | 0:8483298c086d | 92 | ";de'-tano/kirokuto,keisokuga,kaisisarema'sita.\r", |
naoki_westwell | 0:8483298c086d | 93 | ";rihuto,;o'hu rokextuxtu'toga,uchiagerarema'sita.\r", |
naoki_westwell | 0:8483298c086d | 94 | "parashu-'to,ichi,hou+shutu.\r", |
naoki_westwell | 0:8483298c086d | 95 | "parashu-'to,ni',hou+shutu.\r", |
naoki_westwell | 0:8483298c086d | 96 | "sa-bomo-'ta-,kudo-.\r", |
naoki_westwell | 0:8483298c086d | 97 | "rokextuxtu'towa,rakkatyuude_su.\r", |
naoki_westwell | 0:8483298c086d | 98 | "rokextuxtu'tono,tyakutiwo,kakuninnsima'sita.\r", |
naoki_westwell | 0:8483298c086d | 99 | ";uchiageyotei,ji'koku/wo,setteisima'sita.\r", |
naoki_westwell | 0:8483298c086d | 100 | ";genzaiji'koku/wo,setteisima'sita.\r", |
naoki_westwell | 0:8483298c086d | 101 | "huraito'pin,go'-.\r", |
naoki_westwell | 0:8483298c086d | 102 | "huraito'pin,no-go'-.setuzokuwo/kakuninnsitekudasa'i.\r", |
naoki_westwell | 0:8483298c086d | 103 | "sairento/mo'-do,o'hu.\r", |
naoki_westwell | 0:8483298c086d | 104 | "ge'nzai,kaihousagyoutyuude'_su.\r", |
naoki_westwell | 0:8483298c086d | 105 | "sintyokudo'ude_suka?\r", |
naoki_westwell | 0:8483298c086d | 106 | "#J", |
naoki_westwell | 0:8483298c086d | 107 | "#K", |
naoki_westwell | 0:8483298c086d | 108 | }; |
naoki_westwell | 0:8483298c086d | 109 | |
naoki_westwell | 0:8483298c086d | 110 | //プロトタイプ宣言 |
naoki_westwell | 0:8483298c086d | 111 | void tick(); //データ取得割り込み |
naoki_westwell | 0:8483298c086d | 112 | void sci_rx(); //通信線コマンド割り込み |
naoki_westwell | 0:8483298c086d | 113 | void gps_rx(); //GPS通信線コマンド割り込み |
naoki_westwell | 0:8483298c086d | 114 | void deployment(); //開放アルゴリズム |
naoki_westwell | 0:8483298c086d | 115 | void talk_line(int num); //指定したセリフを喋る |
naoki_westwell | 0:8483298c086d | 116 | void random_talk(); //ランダムで喋る |
naoki_westwell | 0:8483298c086d | 117 | |
naoki_westwell | 0:8483298c086d | 118 | //////////// |
naoki_westwell | 0:8483298c086d | 119 | //メイン関数// |
naoki_westwell | 0:8483298c086d | 120 | //////////// |
naoki_westwell | 0:8483298c086d | 121 | int main() { |
naoki_westwell | 0:8483298c086d | 122 | talk_line(0); |
naoki_westwell | 0:8483298c086d | 123 | //Real Time Clock |
naoki_westwell | 0:8483298c086d | 124 | rtc.tm_sec = 00; // 0-59 |
naoki_westwell | 0:8483298c086d | 125 | rtc.tm_min = 00; // 0-59 |
naoki_westwell | 0:8483298c086d | 126 | rtc.tm_hour = 00; // 0-23 |
naoki_westwell | 0:8483298c086d | 127 | rtc.tm_year = 115; // year since 1900 |
naoki_westwell | 0:8483298c086d | 128 | seconds = mktime(&rtc); //時刻生成 |
naoki_westwell | 0:8483298c086d | 129 | set_time(seconds); //時刻設定 |
naoki_westwell | 0:8483298c086d | 130 | |
naoki_westwell | 0:8483298c086d | 131 | servo.period_ms(20); //サーボ波長 |
naoki_westwell | 0:8483298c086d | 132 | servo = DUTY_BASE; //サーボをベース状態 |
naoki_westwell | 0:8483298c086d | 133 | wait(1); servo.pulsewidth_us(0); //保持やめる |
naoki_westwell | 0:8483298c086d | 134 | mpu.initialize(); //MPU6050(加速度ジャイロ)初期 |
naoki_westwell | 0:8483298c086d | 135 | sci.attach(sci_rx, Serial::RxIrq); //シリアル受信割り込み |
naoki_westwell | 0:8483298c086d | 136 | uar.baud(4800); //GPS |
naoki_westwell | 0:8483298c086d | 137 | uar.attach(gps_rx, Serial::RxIrq); //GPS割り込み |
naoki_westwell | 0:8483298c086d | 138 | timer.start(); //タイマ開始 |
naoki_westwell | 0:8483298c086d | 139 | timer.reset(); //タイマ初期化 |
naoki_westwell | 0:8483298c086d | 140 | xtimer.start(); //タイマ開始 |
naoki_westwell | 0:8483298c086d | 141 | xtimer.reset(); //タイマ初期化 |
naoki_westwell | 0:8483298c086d | 142 | for(int i=0;i<4;i++){myleds=myleds|1<<i;wait(0.4);} myleds=0x06; |
naoki_westwell | 0:8483298c086d | 143 | sci.baud(9600); |
naoki_westwell | 0:8483298c086d | 144 | sci.printf("\n\n******************************\n"); |
naoki_westwell | 0:8483298c086d | 145 | sci.printf("**********START UP!!**********\n"); |
naoki_westwell | 0:8483298c086d | 146 | sci.printf("******************************\n"); |
naoki_westwell | 0:8483298c086d | 147 | talk_line(1); |
naoki_westwell | 0:8483298c086d | 148 | |
naoki_westwell | 0:8483298c086d | 149 | while(1){ |
naoki_westwell | 0:8483298c086d | 150 | //RTC |
naoki_westwell | 0:8483298c086d | 151 | seconds = time(NULL); |
naoki_westwell | 0:8483298c086d | 152 | strftime(time_buf,sizeof(time_buf),"%H,%M,%S",localtime(&seconds)); |
naoki_westwell | 0:8483298c086d | 153 | sscanf(time_buf,"%d,%d,%d",&time_h,&time_m,&time_s); |
naoki_westwell | 0:8483298c086d | 154 | if(mode==0){ |
naoki_westwell | 0:8483298c086d | 155 | pre_cnt++; |
naoki_westwell | 0:8483298c086d | 156 | random_talk(); |
naoki_westwell | 0:8483298c086d | 157 | myleds=myleds^0xff; wait(0.1); |
naoki_westwell | 0:8483298c086d | 158 | } |
naoki_westwell | 0:8483298c086d | 159 | else |
naoki_westwell | 0:8483298c086d | 160 | if(mode){ |
naoki_westwell | 0:8483298c086d | 161 | |
naoki_westwell | 0:8483298c086d | 162 | //SDへの記録 |
naoki_westwell | 0:8483298c086d | 163 | |
naoki_westwell | 0:8483298c086d | 164 | if(cnt%DATA_BUNDLE==DATA_BUNDLE-1){ |
naoki_westwell | 0:8483298c086d | 165 | fp = fopen("/sd/data.csv", "a"); |
naoki_westwell | 0:8483298c086d | 166 | if(fp==NULL){ |
naoki_westwell | 0:8483298c086d | 167 | sci.printf("<!>Error<!> ---> Could not open file\r\n"); |
naoki_westwell | 0:8483298c086d | 168 | }else{ |
naoki_westwell | 0:8483298c086d | 169 | for(int i=0; i<DATA_BUNDLE; i++){ //DATA_BUDLE分のフレームを一度に保存 |
naoki_westwell | 0:8483298c086d | 170 | fprintf(fp, "%s", data_buffer[i]); |
naoki_westwell | 0:8483298c086d | 171 | sprintf(data_buffer[i], ""); |
naoki_westwell | 0:8483298c086d | 172 | } |
naoki_westwell | 0:8483298c086d | 173 | fclose(fp); |
naoki_westwell | 0:8483298c086d | 174 | } |
naoki_westwell | 0:8483298c086d | 175 | } |
naoki_westwell | 0:8483298c086d | 176 | } |
naoki_westwell | 0:8483298c086d | 177 | } |
naoki_westwell | 0:8483298c086d | 178 | } |
naoki_westwell | 0:8483298c086d | 179 | ////////////////// |
naoki_westwell | 0:8483298c086d | 180 | //データ取得割り込み// |
naoki_westwell | 0:8483298c086d | 181 | ////////////////// |
naoki_westwell | 0:8483298c086d | 182 | void tick(){ |
naoki_westwell | 0:8483298c086d | 183 | t.start(); |
naoki_westwell | 0:8483298c086d | 184 | t.reset(); |
naoki_westwell | 0:8483298c086d | 185 | ms_time = timer.read_ms(); //タイマー(ミリ秒) |
naoki_westwell | 0:8483298c086d | 186 | |
naoki_westwell | 0:8483298c086d | 187 | press = ms5607.getPressure(); //気圧取得 |
naoki_westwell | 0:8483298c086d | 188 | temp = ms5607.getTemperature()*100; //気温取得 |
naoki_westwell | 0:8483298c086d | 189 | altitude = t0/t_grad*(1-exp((t_grad*R/g)*log(press/p0)))-gnd_alt; //高度取得 |
naoki_westwell | 0:8483298c086d | 190 | if(altitude<2000 && altitude>vertex) vertex=altitude; |
naoki_westwell | 0:8483298c086d | 191 | if(vertex-altitude>2){dep_sw++;}else{dep_sw=0;} |
naoki_westwell | 0:8483298c086d | 192 | deployment(); |
naoki_westwell | 0:8483298c086d | 193 | |
naoki_westwell | 0:8483298c086d | 194 | mpu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz); //加速度ジャイロ取 |
naoki_westwell | 0:8483298c086d | 195 | |
naoki_westwell | 0:8483298c086d | 196 | ax=float(ax); ay=float(ay); az=float(az); |
naoki_westwell | 0:8483298c086d | 197 | gx=float(gx); gy=float(gy); gz=float(gz); |
naoki_westwell | 0:8483298c086d | 198 | ax=float(ax)/2048*10000; ay=float(ay)/2048*10000; az=float(az)/2048*10000; |
naoki_westwell | 0:8483298c086d | 199 | gx=float(gx)/65.5*100; gy=float(gy)/65.5*100; gz=float(gz)/65.5*100; |
naoki_westwell | 0:8483298c086d | 200 | pre_acc = acc; |
naoki_westwell | 0:8483298c086d | 201 | acc = int(sqrt(double(ax*ax+ay*ay+az*az))); //合成加速度 |
naoki_westwell | 0:8483298c086d | 202 | |
naoki_westwell | 0:8483298c086d | 203 | //データフレームへ格納 |
naoki_westwell | 0:8483298c086d | 204 | sprintf(total_data,"%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%2d%2d%2d,%1d,%d,%2d%2d%2d,%d\r\n", |
naoki_westwell | 0:8483298c086d | 205 | press,temp,ax,ay,az,gx,gy,gz, |
naoki_westwell | 0:8483298c086d | 206 | g_hokui, g_tokei, htime, mtime, stime, |
naoki_westwell | 0:8483298c086d | 207 | mode, ms_time, time_h, time_m, time_s, cnt |
naoki_westwell | 0:8483298c086d | 208 | ); |
naoki_westwell | 0:8483298c086d | 209 | sprintf(data_buffer[cnt%DATA_BUNDLE],"%s",total_data); //データバッファへ格納 |
naoki_westwell | 0:8483298c086d | 210 | //sci.printf("%s",total_data); //通信線へ |
naoki_westwell | 0:8483298c086d | 211 | //pc.printf("%s",total_data); //PCへ |
naoki_westwell | 0:8483298c086d | 212 | //LED Flip-flops |
naoki_westwell | 0:8483298c086d | 213 | if(cnt%6<4) myleds = 1 << cnt%6; |
naoki_westwell | 0:8483298c086d | 214 | else myleds = 4 >> (cnt%6-4); |
naoki_westwell | 0:8483298c086d | 215 | cnt++; //カウント |
naoki_westwell | 0:8483298c086d | 216 | |
naoki_westwell | 0:8483298c086d | 217 | //sci.printf("%f\r\n",t.read()); |
naoki_westwell | 0:8483298c086d | 218 | } |
naoki_westwell | 0:8483298c086d | 219 | //////////////////// |
naoki_westwell | 0:8483298c086d | 220 | //通信コマンド割り込み// |
naoki_westwell | 0:8483298c086d | 221 | //////////////////// |
naoki_westwell | 0:8483298c086d | 222 | unsigned short sci_i=0; //コマンドN文字目 |
naoki_westwell | 0:8483298c086d | 223 | char cmd[3]; //コマンド命令 |
naoki_westwell | 0:8483298c086d | 224 | char val[MAX_RDATA-3]; //コマンド引数 |
naoki_westwell | 0:8483298c086d | 225 | void sci_rx () { |
naoki_westwell | 0:8483298c086d | 226 | //受信データが大きすぎる場合 |
naoki_westwell | 0:8483298c086d | 227 | if(sci_i>MAX_RDATA-1){ |
naoki_westwell | 0:8483298c086d | 228 | sci_i = MAX_RDATA-1; |
naoki_westwell | 0:8483298c086d | 229 | Rdata[sci_i]='\n'; |
naoki_westwell | 0:8483298c086d | 230 | } |
naoki_westwell | 0:8483298c086d | 231 | Rdata[sci_i]=sci.getc(); //受信データの格納 |
naoki_westwell | 0:8483298c086d | 232 | if(Rdata[sci_i]=='\n'){ //受信データ区切り→コマンドごとの処理 |
naoki_westwell | 0:8483298c086d | 233 | Rdata[sci_i-1]='\0'; |
naoki_westwell | 0:8483298c086d | 234 | sci_i=0; |
naoki_westwell | 0:8483298c086d | 235 | |
naoki_westwell | 0:8483298c086d | 236 | sci.printf("\nReceved data: \"%s\" \r\n", Rdata); |
naoki_westwell | 0:8483298c086d | 237 | sscanf(Rdata,"%3s %s", cmd, val); //コマンドを命令と引数に分割 |
naoki_westwell | 0:8483298c086d | 238 | //リセット・コマンド |
naoki_westwell | 0:8483298c086d | 239 | if(strcmp(cmd, "RST")==0){ |
naoki_westwell | 0:8483298c086d | 240 | sci.printf("---> Mbed reset \r\n"); talk_line(4); |
naoki_westwell | 0:8483298c086d | 241 | wait(0.1); mbed_reset(); |
naoki_westwell | 0:8483298c086d | 242 | }else |
naoki_westwell | 0:8483298c086d | 243 | //フライトモード・コマンド |
naoki_westwell | 0:8483298c086d | 244 | if(strcmp(cmd, "FLT")==0){ |
naoki_westwell | 0:8483298c086d | 245 | sci.printf("---> Flight-Mode ON! \r\n"); talk_line(8); |
naoki_westwell | 0:8483298c086d | 246 | mode=1; //フライトモードオン!! |
naoki_westwell | 0:8483298c086d | 247 | ticker.attach(&tick, 0.1); //タイマー割り込み |
naoki_westwell | 0:8483298c086d | 248 | timer.start(); //タイマ開始 |
naoki_westwell | 0:8483298c086d | 249 | timer.reset(); //タイマ初期化 |
naoki_westwell | 0:8483298c086d | 250 | }else |
naoki_westwell | 0:8483298c086d | 251 | //SDリセット・コマンド |
naoki_westwell | 0:8483298c086d | 252 | if(strcmp(cmd, "SDR")==0){ |
naoki_westwell | 0:8483298c086d | 253 | fp = fopen("/sd/data.csv", "w"); |
naoki_westwell | 0:8483298c086d | 254 | if(fp==NULL){ sci.printf("<!>Error<!> ---> Could not open file\r\n");} |
naoki_westwell | 0:8483298c086d | 255 | else{ fprintf(fp, ""); fclose(fp); sci.printf("---> SDcard Reset \r\n"); talk_line(5);} |
naoki_westwell | 0:8483298c086d | 256 | wait(0.1); |
naoki_westwell | 0:8483298c086d | 257 | }else |
naoki_westwell | 0:8483298c086d | 258 | //地上高度測定コマンド |
naoki_westwell | 0:8483298c086d | 259 | if(strcmp(cmd, "ALT")==0){ |
naoki_westwell | 0:8483298c086d | 260 | sci.printf("---> Measureing state of rest\r\n"); talk_line(6); |
naoki_westwell | 0:8483298c086d | 261 | for(int i=0;i<100;i++){ |
naoki_westwell | 0:8483298c086d | 262 | gnd_alt += t0/t_grad*(1-exp((t_grad*R/g)*log(ms5607.getPressure()/p0))); |
naoki_westwell | 0:8483298c086d | 263 | if(i%10==9){ sci.printf("@"); myleds=myleds^0xff; } |
naoki_westwell | 0:8483298c086d | 264 | wait(0.1); |
naoki_westwell | 0:8483298c086d | 265 | } |
naoki_westwell | 0:8483298c086d | 266 | gnd_alt/=100; |
naoki_westwell | 0:8483298c086d | 267 | sci.printf("\n---> Measurement Complete\r\n"); talk_line(7); |
naoki_westwell | 0:8483298c086d | 268 | }else |
naoki_westwell | 0:8483298c086d | 269 | //現在時刻設定 |
naoki_westwell | 0:8483298c086d | 270 | if(strcmp(cmd, "SCT")==0){ |
naoki_westwell | 0:8483298c086d | 271 | sci.printf("---> Set Current Time\r\n"); talk_line(17); |
naoki_westwell | 0:8483298c086d | 272 | rtc.tm_hour = int(atof(val)/10000); // 0-23 |
naoki_westwell | 0:8483298c086d | 273 | rtc.tm_min = int((atof(val)-rtc.tm_hour*10000)/100); // 0-59 |
naoki_westwell | 0:8483298c086d | 274 | rtc.tm_sec = int(atof(val)-rtc.tm_hour*10000-rtc.tm_min*100); // 0-59 |
naoki_westwell | 0:8483298c086d | 275 | seconds = mktime(&rtc); |
naoki_westwell | 0:8483298c086d | 276 | set_time(seconds); |
naoki_westwell | 0:8483298c086d | 277 | }else |
naoki_westwell | 0:8483298c086d | 278 | //打上時刻設定設定 |
naoki_westwell | 0:8483298c086d | 279 | if(strcmp(cmd, "SXT")==0){ |
naoki_westwell | 0:8483298c086d | 280 | sci.printf("---> Set Launch Time\r\n"); talk_line(16); |
naoki_westwell | 0:8483298c086d | 281 | xtime_h = int(atof(val)/10000); // 0-23 |
naoki_westwell | 0:8483298c086d | 282 | xtime_m = int((atof(val)-xtime_h*10000)/100); // 0-59 |
naoki_westwell | 0:8483298c086d | 283 | xtime_s = int(atof(val)-xtime_h*10000-xtime_m*100); // 0-59 |
naoki_westwell | 0:8483298c086d | 284 | }else |
naoki_westwell | 0:8483298c086d | 285 | //パルス設定プログラム |
naoki_westwell | 0:8483298c086d | 286 | if(strcmp(cmd, "PWM")==0){ |
naoki_westwell | 0:8483298c086d | 287 | servo = atof(val)*0.001/2 + 0.03; |
naoki_westwell | 0:8483298c086d | 288 | wait(1); servo.pulsewidth_us(0); |
naoki_westwell | 0:8483298c086d | 289 | sci.printf("\n---> Set Pulse Width\r\n"); talk_line(13); |
naoki_westwell | 0:8483298c086d | 290 | }else |
naoki_westwell | 0:8483298c086d | 291 | //フライトピンチェック |
naoki_westwell | 0:8483298c086d | 292 | if(strcmp(cmd, "FPC")==0){ |
naoki_westwell | 0:8483298c086d | 293 | if(f_pin){sci.printf("---> Flight Pin Go !\r\n");talk_line(18);} |
naoki_westwell | 0:8483298c086d | 294 | else{sci.printf("<!><!><!> Flight Pin No Go <!><!><!>\r\n");talk_line(19);} |
naoki_westwell | 0:8483298c086d | 295 | }else |
naoki_westwell | 0:8483298c086d | 296 | //サイレントモード |
naoki_westwell | 0:8483298c086d | 297 | if(strcmp(cmd, "SLT")==0){ |
naoki_westwell | 0:8483298c086d | 298 | if(silent==0){sci.printf("---> Silent Mode On !\r\n");silent=1;} |
naoki_westwell | 0:8483298c086d | 299 | else{sci.printf("---> Silent Mode OFF \r\n");silent=0;talk_line(20);} |
naoki_westwell | 0:8483298c086d | 300 | }else |
naoki_westwell | 0:8483298c086d | 301 | //スピーク・コマンド |
naoki_westwell | 0:8483298c086d | 302 | if(strcmp(cmd, "SPK")==0){ |
naoki_westwell | 0:8483298c086d | 303 | int line_num = atoi(val); |
naoki_westwell | 0:8483298c086d | 304 | sci.printf("---> Now talking \"%s\" \r\n", spk_line[line_num]); |
naoki_westwell | 0:8483298c086d | 305 | talk_line(line_num); |
naoki_westwell | 0:8483298c086d | 306 | }else |
naoki_westwell | 0:8483298c086d | 307 | //トーク・コマンド |
naoki_westwell | 0:8483298c086d | 308 | if(strcmp(cmd, "TLK")==0){ |
naoki_westwell | 0:8483298c086d | 309 | if(strlen(val)+4 > MAX_RDATA-3){ |
naoki_westwell | 0:8483298c086d | 310 | sci.printf("<!>Error<!> ---> Exceeds size limit\r\n"); |
naoki_westwell | 0:8483298c086d | 311 | }else{ |
naoki_westwell | 0:8483298c086d | 312 | sci.printf("---> Now talking \"%s\" \r\n", val); |
naoki_westwell | 0:8483298c086d | 313 | strcat(val, ".\r"); |
naoki_westwell | 0:8483298c086d | 314 | i2c.write(tlk_addr, val, strlen(val)); |
naoki_westwell | 0:8483298c086d | 315 | } |
naoki_westwell | 0:8483298c086d | 316 | }else{ |
naoki_westwell | 0:8483298c086d | 317 | //エラー |
naoki_westwell | 0:8483298c086d | 318 | sci.printf("<!>Error<!> ---> Unknown command \r\n"); |
naoki_westwell | 0:8483298c086d | 319 | } |
naoki_westwell | 0:8483298c086d | 320 | strcpy(cmd, ""); strcpy(val, ""); |
naoki_westwell | 0:8483298c086d | 321 | }else{ //受信データ区切りでないなら格納続ける |
naoki_westwell | 0:8483298c086d | 322 | sci_i++; |
naoki_westwell | 0:8483298c086d | 323 | } |
naoki_westwell | 0:8483298c086d | 324 | } |
naoki_westwell | 0:8483298c086d | 325 | |
naoki_westwell | 0:8483298c086d | 326 | //番号を指定してセリフを喋るよ |
naoki_westwell | 0:8483298c086d | 327 | void talk_line(int num){ |
naoki_westwell | 0:8483298c086d | 328 | if(silent==0) i2c.write(tlk_addr, spk_line[num], strlen(spk_line[num])); |
naoki_westwell | 0:8483298c086d | 329 | } |
naoki_westwell | 0:8483298c086d | 330 | |
naoki_westwell | 0:8483298c086d | 331 | void gps_rx () {//I2C Interrupt RX |
naoki_westwell | 0:8483298c086d | 332 | if( (gps_data[gps_i]=uar.getc()) != '$'){ |
naoki_westwell | 0:8483298c086d | 333 | gps_i++; |
naoki_westwell | 0:8483298c086d | 334 | }else{ |
naoki_westwell | 0:8483298c086d | 335 | gps_data[gps_i]='\0'; |
naoki_westwell | 0:8483298c086d | 336 | if ( sscanf(gps_data, "GPGGA,%f,%f,%c,%f,%c,%d,%d",&gps_time,&hokui,&ns,&tokei,&ew,&rlock,&stn) >= 1) { |
naoki_westwell | 0:8483298c086d | 337 | g_hokui=(int(hokui/100)+(hokui-int(hokui/100))/60)*1000000; |
naoki_westwell | 0:8483298c086d | 338 | g_tokei=(int(tokei/100)+(tokei-int(tokei/100))/60)*1000000; |
naoki_westwell | 0:8483298c086d | 339 | |
naoki_westwell | 0:8483298c086d | 340 | //time set |
naoki_westwell | 0:8483298c086d | 341 | htime=int(gps_time/10000); mtime=int((gps_time-htime*10000)/100); stime=int(gps_time-htime*10000-mtime*100); |
naoki_westwell | 0:8483298c086d | 342 | htime=htime+9;//UTC =>JST |
naoki_westwell | 0:8483298c086d | 343 | } |
naoki_westwell | 0:8483298c086d | 344 | gps_i=0; |
naoki_westwell | 0:8483298c086d | 345 | sprintf(gps_data,""); |
naoki_westwell | 0:8483298c086d | 346 | } |
naoki_westwell | 0:8483298c086d | 347 | } |
naoki_westwell | 0:8483298c086d | 348 | |
naoki_westwell | 0:8483298c086d | 349 | void random_talk(){ |
naoki_westwell | 0:8483298c086d | 350 | int rnd = timer.read_us()%6; |
naoki_westwell | 0:8483298c086d | 351 | char msg[128]; |
naoki_westwell | 0:8483298c086d | 352 | if(silent==0 && pre_cnt%600==0){ |
naoki_westwell | 0:8483298c086d | 353 | if(rnd>=0 && rnd<=1){ |
naoki_westwell | 0:8483298c086d | 354 | float nishitime=((xtime_h*3600+xtime_m*60+xtime_s)-(time_h*3600+time_m*60+time_s)); |
naoki_westwell | 0:8483298c086d | 355 | int ato_m = int(nishitime/60); int ato_s = int(nishitime-ato_m*60); |
naoki_westwell | 0:8483298c086d | 356 | sprintf(msg,";uchiageyotei,ji'kokumade,a'to <NUMK VAL=%d COUNTER=funn> <NUMK VAL=%d COUNTER=byo-> de_su.\r", |
naoki_westwell | 0:8483298c086d | 357 | ato_m, ato_s); |
naoki_westwell | 0:8483298c086d | 358 | }else |
naoki_westwell | 0:8483298c086d | 359 | if(rnd>=2 && rnd<=3){ |
naoki_westwell | 0:8483298c086d | 360 | sprintf(msg,";uchiageyotei,ji'kokuwa <NUMK VAL=%d COUNTER=ji> <NUMK VAL=%d COUNTER=funn> <NUMK VAL=%d COUNTER=byo-> de_su.\r", |
naoki_westwell | 0:8483298c086d | 361 | xtime_h, xtime_m, xtime_s); |
naoki_westwell | 0:8483298c086d | 362 | }else |
naoki_westwell | 0:8483298c086d | 363 | if(rnd==4){ |
naoki_westwell | 0:8483298c086d | 364 | sprintf(msg,"sintyokuwa,;do'ude_suka?ganbattekudasai\r"); |
naoki_westwell | 0:8483298c086d | 365 | }else |
naoki_westwell | 0:8483298c086d | 366 | if(rnd==5){ |
naoki_westwell | 0:8483298c086d | 367 | sprintf(msg,"watasini'wa,joukuude,hutatu'no,parashu-'towo,houshutusuru'toiu,sekininngaarima_su.mina'sann,o-enn,yorosikuonegaisima_su.\r"); |
naoki_westwell | 0:8483298c086d | 368 | } |
naoki_westwell | 0:8483298c086d | 369 | i2c.write(tlk_addr, msg, strlen(msg)); |
naoki_westwell | 0:8483298c086d | 370 | } |
naoki_westwell | 0:8483298c086d | 371 | } |
naoki_westwell | 0:8483298c086d | 372 | |
naoki_westwell | 0:8483298c086d | 373 | void deployment(){ |
naoki_westwell | 0:8483298c086d | 374 | //サーボ保持解除 |
naoki_westwell | 0:8483298c086d | 375 | if(servo_sw){ |
naoki_westwell | 0:8483298c086d | 376 | servo_sw++; |
naoki_westwell | 0:8483298c086d | 377 | if(servo_sw>10) { |
naoki_westwell | 0:8483298c086d | 378 | servo.pulsewidth_us(0); |
naoki_westwell | 0:8483298c086d | 379 | servo_sw=0; |
naoki_westwell | 0:8483298c086d | 380 | } |
naoki_westwell | 0:8483298c086d | 381 | } |
naoki_westwell | 0:8483298c086d | 382 | //発射判定 |
naoki_westwell | 0:8483298c086d | 383 | if(mode==1){ |
naoki_westwell | 0:8483298c086d | 384 | if(acc > 30000){ |
naoki_westwell | 0:8483298c086d | 385 | mode=2; talk_line(10); sci.printf("Lift off!! (accel)\r\n"); timer.reset(); |
naoki_westwell | 0:8483298c086d | 386 | launch_time = timer.read_ms(); |
naoki_westwell | 0:8483298c086d | 387 | }else |
naoki_westwell | 0:8483298c086d | 388 | if(f_pin==0){ |
naoki_westwell | 0:8483298c086d | 389 | mode=3; talk_line(10); sci.printf("Lift off!! (flight pin)\r\n"); timer.reset(); |
naoki_westwell | 0:8483298c086d | 390 | launch_time = timer.read_ms(); |
naoki_westwell | 0:8483298c086d | 391 | } |
naoki_westwell | 0:8483298c086d | 392 | }else |
naoki_westwell | 0:8483298c086d | 393 | //開放判定1 |
naoki_westwell | 0:8483298c086d | 394 | if(mode==2 || mode==3){ |
naoki_westwell | 0:8483298c086d | 395 | if(ms_time-launch_time > DEPLOY_TIME_1){ //タイマー開放 |
naoki_westwell | 0:8483298c086d | 396 | mode=4; servo = DUTY_1; servo_sw++; |
naoki_westwell | 0:8483298c086d | 397 | sci.printf("Parachute1 deployed (timer)\r\n"); |
naoki_westwell | 0:8483298c086d | 398 | deploy_time_1 = timer.read_ms(); talk_line(11); |
naoki_westwell | 0:8483298c086d | 399 | }else |
naoki_westwell | 0:8483298c086d | 400 | if(ms_time-launch_time>8000 && dep_sw==2){ //高度開放 |
naoki_westwell | 0:8483298c086d | 401 | mode=5; servo = DUTY_1; servo_sw++; |
naoki_westwell | 0:8483298c086d | 402 | sci.printf("Parachute1 deployed (altitude)\r\n"); |
naoki_westwell | 0:8483298c086d | 403 | deploy_time_1 = timer.read_ms(); talk_line(11); |
naoki_westwell | 0:8483298c086d | 404 | } |
naoki_westwell | 0:8483298c086d | 405 | }else |
naoki_westwell | 0:8483298c086d | 406 | //開放判定2 |
naoki_westwell | 0:8483298c086d | 407 | if(mode==4 || mode==5){ |
naoki_westwell | 0:8483298c086d | 408 | if(ms_time-deploy_time_1 > DEPLOY_TIME_2){ //タイマー開放 |
naoki_westwell | 0:8483298c086d | 409 | mode=6; servo = DUTY_2; servo_sw++; |
naoki_westwell | 0:8483298c086d | 410 | sci.printf("Parachute2 deployed (timer)\r\n"); |
naoki_westwell | 0:8483298c086d | 411 | deploy_time_2 = timer.read_ms(); talk_line(12); |
naoki_westwell | 0:8483298c086d | 412 | }else |
naoki_westwell | 0:8483298c086d | 413 | if(ms_time-deploy_time_1>8000 && altitude<DEPLOY_ALT2){ |
naoki_westwell | 0:8483298c086d | 414 | mode=7; servo = DUTY_2; servo_sw++; |
naoki_westwell | 0:8483298c086d | 415 | sci.printf("Parachute2 deployed (altitude)\r\n"); |
naoki_westwell | 0:8483298c086d | 416 | deploy_time_2 = timer.read_ms(); talk_line(12); |
naoki_westwell | 0:8483298c086d | 417 | } |
naoki_westwell | 0:8483298c086d | 418 | }else |
naoki_westwell | 0:8483298c086d | 419 | //着地判定 |
naoki_westwell | 0:8483298c086d | 420 | if(mode==6 || mode==7){ |
naoki_westwell | 0:8483298c086d | 421 | servo_sw2++; |
naoki_westwell | 0:8483298c086d | 422 | if(servo_sw2%10==0) { servo = DUTY_2; servo_sw++; } |
naoki_westwell | 0:8483298c086d | 423 | if(abs(acc-pre_acc)<1000){ |
naoki_westwell | 0:8483298c086d | 424 | acc_sw++; |
naoki_westwell | 0:8483298c086d | 425 | }else{acc_sw=0;} |
naoki_westwell | 0:8483298c086d | 426 | if(acc_sw==10){ |
naoki_westwell | 0:8483298c086d | 427 | sci.printf("Rocket Landing! \r\n"); talk_line(15); mode=8; |
naoki_westwell | 0:8483298c086d | 428 | } |
naoki_westwell | 0:8483298c086d | 429 | } |
naoki_westwell | 0:8483298c086d | 430 | } |