CORE / Mbed 2 deprecated Vertex_Integration_4

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?

UserRevisionLine numberNew 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 }