201903_14ISEで実際に使用した開放用プログラム. 使用マイコンがNUCLES-F303K8なので注意
Dependencies: mbed Madgwick MPU6050 Kalman BMP180
Diff: main.cpp
- Revision:
- 13:3e6411bfd581
- Parent:
- 12:6a3c0e9075eb
- Child:
- 14:f932a8a297ff
--- a/main.cpp Sun Mar 10 11:47:23 2019 +0000 +++ b/main.cpp Mon Mar 11 09:30:01 2019 +0000 @@ -3,6 +3,7 @@ #include "BMP180.h" /*しきい値など*/ +#define CNT_LAUNCH 10 //発射判定するときのしきい値 #define ACC_JUDGE_LAUNCH 3.0 //発射判定の合成加速度のしきい値 #define TIME_BURNING 6 //開放判定しない時間(燃焼時間) #define ALT_JUDGE_OPEN 1 //落下判定のカウントを1増やす高度差 @@ -39,8 +40,9 @@ //地上高度 float alt_gnd; float alt_max; -char c[3]; +char c[516]; int i = 0; +int cnt_gps=0; void main(){ ES920_RST = 0; myled =0; @@ -70,8 +72,8 @@ timer_data.start(); servo1_signal = 1; servo2_signal = 1; - servo1.pulsewidth(0.0024); - servo1.pulsewidth(0.0024); + servo1.pulsewidth(0.0015);//close + servo2.pulsewidth(0.0006);//close while(1){ switch(Phase){ case STANDBY: @@ -80,16 +82,16 @@ c[0]=pc.getc(); if(c[0] == 'o'){ myled = 1; - servo1.pulsewidth(0.0006); - servo2.pulsewidth(0.0006); + servo1.pulsewidth(0.0006);//open + servo2.pulsewidth(0.0015);//open mpu.getAccelero(acc); //pc.printf("OPEN:%f\r\n",sqrt(pow(acc[0]/9.81,2.0)+pow(acc[1]/9.81,2.0)+pow(acc[2]/9.81,2.0))); pc.printf("0,%f,%f,0,%f\r\n",sqrt(pow(acc[0]/9.81,2.0)+pow(acc[1]/9.81,2.0)+pow(acc[2]/9.81,2.0)),(acc[1]/9.81),timer_data.read()); } if(c[0] == 'l'){ myled = 0; - servo1.pulsewidth(0.0024); - servo2.pulsewidth(0.0024); + servo1.pulsewidth(0.0015); + servo2.pulsewidth(0.0006); mpu.getAccelero(acc); //pc.printf("LOCK:%f\r\n",sqrt(pow(acc[0]/9.81,2.0)+pow(acc[1]/9.81,2.0)+pow(acc[2]/9.81,2.0))); pc.printf("0,%f,%f,0,%f\r\n",sqrt(pow(acc[0]/9.81,2.0)+pow(acc[1]/9.81,2.0)+pow(acc[2]/9.81,2.0)),(acc[1]/9.81),timer_data.read()); @@ -100,6 +102,12 @@ servo1_signal = 0; servo2_signal = 0; } + if(c[0] == 'g'){ + pc.printf("9,0,0,0,0\r\n"); + wait(0.5); + tic_gps.attach(&_SendGPS, 1.0/RATE_GPS); + Phase = RECOVERY; + } break; case LAUNCH: @@ -111,14 +119,14 @@ if(timer_data.read() - t > TIME_SEND){ //pc.printf("LAUNCH,acc:%f,time:%3f,cnt:%d\r\n",acc_abs,timer_data.read(),cnt_judge); pc.printf("1,%f,%f,0,%3f\r\n",acc_abs,acc_abs,timer_data.read()); - + cnt_judge = 0; t = timer_data.read(); } /*加速度判定*/ if(acc_abs>ACC_JUDGE_LAUNCH){ cnt_judge++; } - if(cnt_judge==CNT_JUDGE){ + if(cnt_judge==CNT_LAUNCH){ servo1_signal = 1; servo1_signal = 1; cnt_judge=0; @@ -172,6 +180,11 @@ //if((timer_open.read()-time_judge) - TIME_JUDGE_CNT > 0) cnt_judge=0; if(cnt_judge == CNT_JUDGE || timer_open.read() > TIME_OPEN){ Phase = RECOVERY; + servo1_signal = 1; + servo2_signal = 1; + wait(0.1); + servo1.pulsewidth(0.0006);//open + servo2.pulsewidth(0.0015);//open pc.printf("9,0,0,0,0\r\n"); wait(0.5); tic_gps.attach(&_SendGPS, 1.0/RATE_GPS); @@ -222,7 +235,6 @@ void _SendGPS(){ char gps_data[256]; - int cnt_gps=0; while(1){ if(gps.readable()){ gps_data[cnt_gps] = gps.getc(); @@ -240,7 +252,7 @@ //pc.printf("%s\r\n",gps_data); //pc.printf("Lat:%f,Lon:%f\r\ntime:%f,sat_num:%d\r\n",lat_north,lon_east,world_time,sat_num); int japan_time = int(world_time) - 9; - pc.printf("Lat,%f,Lon,%f,MAX_ALT,%f\r\n",lat_north,lon_east,alt_max); + pc.printf("Lat:%f,Lon:%f,MAX_ALT:%f\r\n",lat_north,lon_east,alt_max); for(i=0;i<2;i++){ c[i]=pc.getc(); } @@ -262,7 +274,7 @@ break; } }else{ - //ffpc.printf("No_Satellite_signal\r\n"); + //pc.printf("No_Satellite_signal\r\n"); } }else{ cnt_gps++;