naoki nishii
あああ
Dependencies: MPU6050_2 MS5607 SDFileSystem mbed
Revision 1:491e67888f28, committed 2017-04-07
- Comitter:
- naoki_westwell
- Date:
- Fri Apr 07 05:49:22 2017 +0000
- Parent:
- 0:98c7c6fe50cc
- Commit message:
- FM???
Changed in this revision
| main.cpp | Show annotated file Show diff for this revision Revisions of this file |
diff -r 98c7c6fe50cc -r 491e67888f28 main.cpp
--- a/main.cpp Thu Mar 16 15:06:44 2017 +0000
+++ b/main.cpp Fri Apr 07 05:49:22 2017 +0000
@@ -4,13 +4,26 @@
#include "MPU6050.h"
#include <stdio.h>
+//#define TEST
+#define FLIGHT
/***************************/
/* 打上前に値を確認！！！！！ */
/***************************/
-#define L_HOKUI 35.546257 // 10進法 KBIC 3本の樹
-#define L_TOUKEI 139.671875
+#ifdef TEST
+#define L_HOKUI 34.74102777 // 10進法 urasabaku
+#define L_TOUKEI 139.42511111
#define LNCH_ACC 2 // 発射判定閾値[G]
#define TRANS_TIME 50 // 打上から制御開始までの時間[sec/10]
+#define OPEN_TIME 50 // 150mに達してから開放されるまで（含ロケットアビオとのラグ）
+#endif
+
+#ifdef FLIGHT
+#define L_HOKUI 34.74102777 // 10進法 urasabaku
+#define L_TOUKEI 139.42511111
+#define LNCH_ACC 4 // 発射判定閾値[G]
+#define TRANS_TIME 100 // 打上から制御開始までの時間[sec/10]
+#define OPEN_TIME 50 // 150mに達してから開放されるまで（含ロケットアビオとのラグ）
+#endif
/***************************/
#define DEG_TO_RAD(x) ( x * 0.01745329 )
@@ -34,9 +47,9 @@
#define KPE 2.0 // エレベータゲイン
#define GOAL_R 50 // 目標地点にこれだけ近づいたら制御方法変える
-#define DELTA_E_FULLUP 2000
+#define DELTA_E_FULLUP 1000
#define DELTA_E_NTRL 1500
-#define DELTA_E_FULLDOWN 1000
+#define DELTA_E_FULLDOWN 2000
#define DELTA_L_FULLR 2000
#define DELTA_L_NTRL 1500
#define DELTA_L_FULLL 1000
@@ -65,8 +78,8 @@
int gps_i; // 文字数，モード（センテンス終わりか？判定）
char gps_data[256];
float beta, alpha;
-float set_point_l, set_point_e;
-float delta_l=1500, delta_e=1500;
+float set_point_l=DELTA_L_NTRL, set_point_e=DELTA_E_NTRL;
+float delta_l=0, delta_e=0;
FILE *fp;
char all_data[256];
@@ -75,26 +88,57 @@
bool Init();
void getData();
float getAlt(float press, float press0, float temp);
-//void filter_x(double dt, double C, double x_est[], double x_gps[], double vg_gps[]);
-void filter_alt(double dt, double C, double &alt_est, double alt_s, double vz);
-void filter_vz(double dt, double C, double &vz_est, double vz);
+//void filter_alt(double dt, double C, double &alt_est, double alt_s, double vz);
+//void filter_vz(double dt, double C, double &vz_est, double vz);
bool Init()
{
+ /*①*/leds = leds|1;
+ pc.printf("*** Start up! ***\r\n");
gps.baud(9600); // ボーレート
pc.baud(115200);
twe.baud(115200);
eServo.period_us(20000); // servo requires a 20ms period
lServo.period_us(20000);
- lServo.pulsewidth_us(0);
- eServo.pulsewidth_us(0);
gps.printf("$PMTK314,0,1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0*29\r\n"); // GPRMCだけにする
gps.printf("$PMTK300,100,0,0,0,0*2C\r\n"); // position fixを100msecに（レート10spsにする）
mpu.initialize();
ms5607.printCoefficients();
-
+
+ /*②*/leds = leds | (1<<1);
+ pc.printf("Test start!\r\n");
+ lServo.pulsewidth_us(DELTA_L_NTRL);
+ eServo.pulsewidth_us(DELTA_E_NTRL);
+ wait(1);
+ lServo.pulsewidth_us(DELTA_L_FULLR);
+ eServo.pulsewidth_us(DELTA_E_FULLUP);
+ wait(1);
+ lServo.pulsewidth_us(DELTA_L_FULLL);
+ eServo.pulsewidth_us(DELTA_E_FULLDOWN);
+ wait(1);
+ lServo.pulsewidth_us(DELTA_L_NTRL);
+ eServo.pulsewidth_us(DELTA_E_NTRL);
+ wait(1);
+
+ /*③*/leds = leds | (1<<2);
+ float temp, press;
+ float fax, fay, faz;
+ int16_t ax, ay, az;
+ int16_t gx, gy, gz;
+ for(int i=0; i<10; i++){
+ temp = ms5607.getTemperature(); // 気圧
+ press = ms5607.getPressure(); // 温度
+ mpu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
+ fax = float(ax)/ACC_SSF; // mpu物理量に換算
+ fay = float(ay)/ACC_SSF;
+ faz = float(az)/ACC_SSF;
+ twe.printf("%f,%f,%f,%f,%f,%f,%f,%f", temp, press, fax, fay, faz);
+ wait(0.1);
+ }
+
+ /*④*/leds = leds | (1<<3);
pc.puts("Init finish!\n");
return true;
}
@@ -139,31 +183,45 @@
double yDist = dy*m;
/* 高度 */
- static float alt, alt0; //前の高度
- static float press_arr[PRESS_INIT]; // 基準高度はこれの平均をとる
- static float press0 = 0;
+ static float alt, alt0; // 高度，前の高度
+ static float press_arr[PRESS_INIT]; // 基準気圧はこれの平均をとる
+ static float press0 = 0; // 基準気圧（地上の気圧）
+ float vz; // 高度，沈下速度
float temp = ms5607.getTemperature(); // 気圧
float press = ms5607.getPressure(); // 温度
- /* 基準電圧の取得 */
+ /* 基準気圧の取得 */
if(mode == STDBY_MODE) {
for(int i=0; i<PRESS_INIT-1; i++) press_arr[i] = press_arr[i+1]; // 気圧データをシフト
press_arr[PRESS_INIT-1] = press;
}
- if(mode == TRANS_MODE) { //発射直後，基準電圧を計算
+ if(mode == TRANS_MODE) { //発射直後，基準気圧を計算
if(cnt==0 && press0==0) {
for(int i=0; i<PRESS_INIT; i++) press0 += press_arr[i];
press0 /= PRESS_INIT;
}
}
- double vz_calc = (alt - alt0) / 0.1;
+ /* 沈下速度計算 */
+ static float dt = 1/GPS_RATE;
alt0 = alt;
- alt = getAlt(press, press0, temp); //高度の算出
- double alt_est = 0.0, vz_est = 0.0;
- float dt = 1/GPS_RATE;
- filter_alt(dt, 0.1, alt_est, alt, vz_est);
- filter_vz(dt, 0.01, vz_est, vz_calc);
+ alt = getAlt(press, press0, temp);
+ vz = (alt0 - alt)*dt;
+
+ /* だきゅんフィルタ */
+ /*
+ double vz_calc, alt_est, vz_est;
+ double dt = 1/GPS_RATE;
+ if(mode >= TRANS_MODE){
+ static double alt0 = getAlt(press, press0, temp); //高度の算出
+ vz_calc = (alt - alt0) / dt;
+ alt0 = alt;
+ alt = getAlt(press, press0, temp); //高度の算出
+ alt_est = 0.0, vz_est = 0.0;
+ filter_alt(dt, 1, alt_est, alt, vz_est);
+ filter_vz(dt, 1, vz_est, vz_calc);
+ }
+ */
/* 加速度 */
int16_t ax, ay, az; // mpu生データ
@@ -185,12 +243,14 @@
if(beta <-180) beta += 360;
/* 降下角 */
- alpha = -atan(vz_est/vb);
+ alpha = atan(vz/vb)*180/PI;
/* サーボ操作 P制御 */
- if(distance <= GOAL_R){
+ if(mode!=CTRL_MODE) {
+ delta_l = 0; delta_l = 0;
+ }else if(distance > GOAL_R){ //目標地点のある半径外なら
// ラダー操舵
- set_point_l = DELTA_L_NTRL + 500*(beta * KPL / 180);
+ set_point_l = DELTA_L_NTRL - 500*(beta * KPL / 180);
if(set_point_l > DELTA_L_FULLR) set_point_l = DELTA_L_FULLR;
if(set_point_l < DELTA_L_FULLL) set_point_l = DELTA_L_FULLL;
if((set_point_l-delta_l)>=0){
@@ -202,45 +262,58 @@
}
// エレベータ制御
- set_point_e = DELTA_E_NTRL + 500*(alpha * KPE / 180);
- if(set_point_e > DELTA_E_FULLUP) set_point_e = DELTA_E_FULLUP;
- if(set_point_e < DELTA_E_FULLDOWN) set_point_e = DELTA_E_FULLDOWN;
- if((set_point_e-delta_e)>=0){
- delta_e += 100;
- if((set_point_e-delta_e)<0) delta_e = set_point_e;
- }else{
- delta_e -= 100;
- if((set_point_e-delta_e)>0) delta_e = set_point_e+1;
+ if(alpha>0){
+ set_point_e = DELTA_E_NTRL - 500*(alpha * KPE / 180);
+ if(set_point_e > DELTA_E_FULLUP) set_point_e = DELTA_E_FULLUP;
+ if(set_point_e < DELTA_E_FULLDOWN) set_point_e = DELTA_E_FULLDOWN;
+ if((set_point_e-delta_e)>=0){
+ delta_e += 100;
+ if((set_point_e-delta_e)<0) delta_e = set_point_e;
+ }else{
+ delta_e -= 100;
+ if((set_point_e-delta_e)>0) delta_e = set_point_e+1;
+ }
}
- }else{
+ }else{ // ある半径内なら
if(cnt%10) delta_e = DELTA_E_NTRL;
else delta_e = DELTA_E_FULLUP;
delta_l = DELTA_L_FULLR; // 要検討
}
+ if(g_tokei == 0){
+ delta_e = DELTA_E_NTRL;
+ delta_l = DELTA_L_FULLR;
+ }
lServo.pulsewidth_us(delta_l);
eServo.pulsewidth_us(delta_e);
/* 取得データの整理と出力 */
- sprintf(all_data, "%6lf,%.2f,%.2f,%.2f,%d,%d\r\n",
- distance, vb, beta, alt, cnt, mode);
+ /*sprintf(all_data, "%6lf,%.2f,%.2f,%.2f,%d,%d\r\n",
+ distance, vb, beta, alt, cnt, mode);*/
+ sprintf(all_data, "%.6f,%.6f,%.2f,%.2f,%.2f,%d,%d\r\n",
+ g_tokei, g_hokui, alt,
+ vb, beta2, cnt, mode);
pc.printf("%s", all_data);
- if(!(cnt%10)) twe.printf("%.6f,%.6f,%.2f,%d\r\n", g_tokei, g_hokui, alt, cnt);
+ if(!(cnt%10)) twe.printf("%.6f,%.6f,%.2f,%d,%d\r\n", g_tokei, g_hokui, alt, cnt, mode);
//pc.printf("%s\r\n",gps_data);
//pc.printf("%f,%f\r\n", delta_l, set_point_l);
+
+ /* SDカードに保存 */
+ if(mode >= TRANS_MODE){ // 輸送モード以降で保存を開始
+ if((cnt%SAVE_RATE)==0) fp = fopen("/sd/data.txt", "a");
+ fprintf(fp, "%s", all_data);
+ if((cnt%SAVE_RATE)==SAVE_RATE-1) fclose(fp);
+ sprintf(gps_data, "");
+ }
/* カメラスイッチ操作 */
if(mode==TRANS_MODE){
if(cnt<=20) camSw = HIGH; // 2秒間長押しで電源オンの後，一回短押しで撮影開始
- if(cnt>20 && cnt<=25) camSw = LOW;
- if(cnt>25 && cnt<=30) camSw = HIGH;
- if(cnt>30) camSw = LOW;
+ if(cnt>20 && cnt<=50) camSw = LOW;
+ if(cnt>50 && cnt<=55) camSw = HIGH;
+ if(cnt>55) camSw = LOW;
+ }else{
+ camSw = LOW;
}
-
- /* SDカードに保存 */
- if((cnt%SAVE_RATE)==0) fp = fopen("/sd/myfile.txt", "a");
- fprintf(fp, "%s", all_data);
- if((cnt%SAVE_RATE)==SAVE_RATE-1) fclose(fp);
- sprintf(gps_data, "");
/* カウント */
cnt++;
@@ -248,16 +321,22 @@
/* モード遷移判定 */
if(mode == STDBY_MODE) { // 待機⇒制御
- if(resAcc > LNCH_ACC) {
+ if(resAcc > LNCH_ACC && cnt>10) {
mode = TRANS_MODE;
cnt = 0;
}
leds = 1 << cnt%4;
}else
if(mode == TRANS_MODE) {
- if(cnt > TRANS_TIME) {
- mode = CTRL_MODE;
- cnt = 0;
+ if(cnt > TRANS_TIME && alt < 150) {
+ static int ctrl_start_cnt = 0;
+ ctrl_start_cnt++;
+ if(ctrl_start_cnt > OPEN_TIME){
+ delta_l = DELTA_L_NTRL; // 制御開始時に通常のデューティ比にする
+ delta_e = DELTA_E_NTRL;
+ mode = CTRL_MODE;
+ cnt = 0;
+ }
}
leds = 3 << cnt%4;
}else
@@ -271,7 +350,6 @@
int main()
{
Init();
- pc.printf("*** Start up! ***\r\n");
gps.attach(getData,Serial::RxIrq);
while(1) {}
}
@@ -279,52 +357,4 @@
float getAlt(float press, float press0, float temp)
{
return (pow((press0/press), (1.0f/5.257f))-1.0f)*(temp+273.15f)/0.0065f;
-}
-
-// 位置（速度）フィルタ
-void filter_x(double dt, double C, double x_est[], double x_gps[], double vg_gps[])
-{
- static double x0[] = { x_gps[0], x_gps[1], x_gps[2] };
- static int called = 0;
- if (!called) {
- for (int i = 0; i < 2; i++)
- x_est[i] = x0[i];
- called++;
- return;
- }
- double xt[2];
- for (int i = 0; i < 2; i++) {
- xt[i] = x0[i] + vg_gps[i] * dt;
- x_est[i] = C*x_gps[i] + (1 - C)*xt[i];
- x0[i] = x_est[i];
- }
-}
-// 高度フィルタ
-void filter_alt(double dt, double C, double &alt_est, double alt_s, double vz)
-{
- static double alt0 = alt_s;
- static int called = 0;
- if (!called) {
- alt_est = alt0;
- called++;
- return;
- }
- double altt = alt0 + vz * dt;
- {
- alt_est = C*alt_s + (1 - C)*altt;
- alt0 = alt_est;
- }
-}
-// 垂直速度フィルタ
-void filter_vz(double dt, double C, double &vz_est, double vz)
-{
- static double vz0 = 0.0;
- static int called = 0;
- if (!called) {
- vz_est = vz0;
- called++;
- return;
- }
- vz_est = C*vz + (1 - C)*vz0;
- vz0 = vz_est;
-}
+}
\ No newline at end of file