MPUとHMCでうごくかもver
Dependencies: ConfigFile SDFileSystem mbed
Fork of LAURUS_program by
Diff: main.cpp
- Revision:
- 21:d417708e84a8
- Parent:
- 20:00afba164688
- Child:
- 22:3caa2983bf1d
--- a/main.cpp Mon Jun 22 18:17:16 2015 +0000 +++ b/main.cpp Tue Jun 23 14:59:21 2015 +0000 @@ -21,9 +21,9 @@ //#define RULE3 const float dt = 0.01f; // 割り込み周期(s) -const float ServoMax = 0.0023f; // サーボの最大パルス長(s) -const float ServoMin = 0.0006f; // サーボの最小パルス長(s) -const float PullMax = 25.0f; // 引っ張れる紐の最大量(mm) +const float ServoMax = 0.0046f; // サーボの最大パルス長(s) +const float ServoMin = 0.0012f; // サーボの最小パルス長(s) +const float PullMax = 25; // 引っ張れる紐の最大量(mm) const float BorderSpiral = 40.0f; // スパイラル検知角度 const short BorderOpt = 30000; // 光センサーの閾値 const float BorderGravity = 0.3f; // 無重力状態の閾値 @@ -48,8 +48,6 @@ SDFileSystem sd(PB_5, PB_4, PB_3, PB_10, "sd"); // microSD FILE * fp; // ログファイルのポインタ BufferedSerial xbee(PA_9, PA_10, PC_1); // Xbee -//Serial xbee(PA_9, PA_10); // XBee -//DigitalIn cts(PC_1); // ctsピン ConfigFile cfg; // ConfigFile PwmOut servoL(PB_6), servoR(PC_7); // サーボ用PWM出力 AnalogIn optSensor(PC_0); // 照度センサ用アナログ入力 @@ -109,7 +107,7 @@ * target_x=111.222 * target_y=33.444 */ -float target_x, target_y; +float target_x = 0.0f, target_y = 0.0f; /* ---------- Kalman Filter ---------- */ // 地磁気ベクトル用 @@ -139,10 +137,10 @@ /****************************** private functions ******************************/ -void SD_Init(); // SDカード初期化 +bool SD_Init(); // SDカード初期化 void VectorsInit(); // 各種ベクトルの初期化 void KalmanInit(); // カルマンフィルタ初期化 -void LoadConfig(); // config読み取り +bool LoadConfig(); // config読み取り int Find_last(); // SDカード初期化用関数 void DataProcessing(); // データ処理関数 void ControlRoutine(); // 制御ルーチン関数 @@ -168,11 +166,8 @@ if(!mpu.init()) AbortWithMsg("mpu6050 Initialize Error !!"); // mpu6050初期化 if(!hmc.init()) AbortWithMsg("hmc5883l Initialize Error !!"); // hmc5883l初期化 - //Config読み取り - LoadConfig(); - // SDカード初期化 - SD_Init(); + while(!SD_Init()); //カルマンフィルタ初期化 KalmanInit(); @@ -180,6 +175,10 @@ // 各種ベクトルの初期化 VectorsInit(); + //Config読み取り + while(!LoadConfig()); + xbee.printf("target(%.5f, %.5f)\r\n", target_x, target_y); + ticker.attach(&DataUpdate, dt); // 割り込み有効化(Freq = 0.01fなので、10msおきの割り込み) NVIC_SetPriority(TIM5_IRQn, 5); @@ -194,11 +193,39 @@ timer.start(); myled = 1; // LED is ON + INT_flag = FALSE; // 割り込みによる変数書き換えを阻止 /******************************* データ処理 *******************************/ DataProcessing(); /******************************* 制御ルーチン *******************************/ ControlRoutine(); + + float sv = (float)servoVcc.read_u16() * ADC_LSB_TO_V * 2.0f; // サーボ電源電圧 + float lv = (float)logicVcc.read_u16() * ADC_LSB_TO_V * 2.0f; // ロジック電源電圧 + + // 指定された引っ張り量だけ紐を引っ張る + if(pull_L < 0) pull_L = 0; + else if(pull_L > PullMax) pull_L = PullMax; + if(pull_R < 0) pull_R = 0; + else if(pull_R > PullMax) pull_R = PullMax; + + servoL.pulsewidth((ServoMax - ServoMin) * pull_L / (float)PullMax + ServoMin); + servoR.pulsewidth((ServoMax - ServoMin) * pull_R / (float)PullMax + ServoMin); + + // データをmicroSDに保存し、XBeeでPCへ送信する + sprintf(data, "%d, %.1f,%.1f,%.1f, %.3f,%.5f,%.5f, %.3f,%.3f,%d, %.3f,%d, %d,%d\r\n", + UTC_t, yaw, pitch, roll, + press, gms.longitude, gms.latitude, + lv, vrt_acc, loopTime, + Distance(target_p, p), optSensor.read_u16() + , pull_R, pull_L); + + INT_flag = TRUE; // 割り込み許可 + + fprintf(fp, data); + fflush(fp); + xbee.printf(data); + myled = 0; // LED is OFF @@ -217,7 +244,6 @@ */ void DataProcessing() { - INT_flag = FALSE; // 割り込みによる変数書き換えを阻止 gms.read(); // GPSデータ取得 UTC_t = (int)gms.time; @@ -258,26 +284,6 @@ pre_p = p; pre_UTC_t = UTC_t; - float sv = (float)servoVcc.read_u16() * ADC_LSB_TO_V * 2.0f; // サーボ電源電圧 - float lv = (float)logicVcc.read_u16() * ADC_LSB_TO_V * 2.0f; // ロジック電源電圧 - - // データをmicroSDに保存し、XBeeでPCへ送信する - sprintf(data, "%d, %.1f,%.1f,%.1f, %.3f,%.5f,%.5f, %.3f,%.3f,%d, %.3f,%d\r\n", - UTC_t, yaw, pitch, roll, - press, gms.longitude, gms.latitude, - lv, vrt_acc, loopTime, - Distance(target_p, p), optSensor.read_u16()); - fprintf(fp, data); - fflush(fp); - xbee.printf(data); - /* - int dataLength = strlen(data); - for(int i=0; i<dataLength; i++) { - while(cts); - xbee.putc(data[i]); - } - */ - INT_flag = TRUE; // 割り込み許可 } @@ -293,8 +299,8 @@ if(Thrown() || count != 0) { count++; // 投下直後に紐を引く場合はコメントアウトをはずす - // pull_L = 15; - // pull_R = 15; + //pull_L = 15; + //pull_R = 15; if(count >= WaitTime*5) { pull_L = 0; pull_R = 0; @@ -329,25 +335,31 @@ if(theta >= 0.0f) { // 目標角も正ならば、 if(theta - yaw > Alpha) dir = 0; // 単純に差を取って閾値αと比べる else if(theta - yaw < -Alpha) dir = 1; + else dir = 2; } else { // 目標角が負であれば theta += 360.0f; // 360°足して正の値に変換してから delta_theta = theta - yaw; // 差を取る(yawから左回りに見たthetaとの差分) if(delta_theta < 180.0f) { // 差が180°より小さければ左旋回 if(delta_theta > Alpha) dir = 0; + else dir = 2; } else { // 180°より大きければ右旋回 if(360.0f - delta_theta > Alpha) dir = 1; + else dir = 2; } } } else { // ヨー角が負 if(theta <= 0.0f) { // 目標角も負ならば、 if(theta - yaw > Alpha) dir = 0; // 単純に差を取って閾値αと比べる else if(theta - yaw < -Alpha) dir = 1; + else dir = 2; } else { // 目標角が正であれば、 delta_theta = theta - yaw; // 差を取る(yawから左回りに見たthetaとの差分) if(delta_theta < 180.0f) { // 差が180°より小さければ左旋回 if(delta_theta > Alpha) dir = 0; + else dir = 2; } else { // 180°より大きければ右旋回 if(360.0f - delta_theta > Alpha) dir = 1; + else dir = 2; } } } @@ -362,6 +374,9 @@ pull_L = 0; pull_R = 20; + } else if (dir == 2) { + pull_L = 0; + pull_R = 0; } } @@ -380,19 +395,10 @@ // もし落下中に目標値から離れてしまった場合は、体勢を立て直して再び滑空 // 境界で制御が不安定にならないよう閾値にマージンを取る - if(Distance(target_p, p) > BorderDistance+5.0f) step = 1; + if(Distance(target_p, p) > BorderDistance+3.0f) step = 1; break; #endif } - - // 指定された引っ張り量だけ紐を引っ張る - if(pull_L < 0) pull_L = 0; - else if(pull_L > PullMax) pull_L = PullMax; - if(pull_R < 0) pull_R = 0; - else if(pull_R > PullMax) pull_R = PullMax; - - servoL.pulsewidth((ServoMax - ServoMin) * pull_L / PullMax + ServoMin); - servoR.pulsewidth((ServoMax - ServoMin) * pull_R / PullMax + ServoMin); } /** 各種ベクトルの初期化を行う関数 @@ -409,7 +415,7 @@ b_f.SetComp(1, 0.0f); b_f.SetComp(2, 0.0f); b_f.SetComp(3, -1.0f); - b_u.SetComp(1, 1.0f); + b_u.SetComp(1, -1.0f); b_u.SetComp(2, 0.0f); b_u.SetComp(3, 0.0f); b_l = Cross(b_u, b_f); @@ -422,41 +428,47 @@ /** マイクロSDカードの初期化を行う関数 * */ -void SD_Init() +bool SD_Init() { //SDカード初期化 char filename[15]; int n = Find_last(); if(n < 0) { pc.printf("Could not read a SD Card.\n"); - return; + return false; } sprintf(filename, "/sd/log%03d.csv", n+1); fp = fopen(filename, "w"); fprintf(fp, "log data\r\n"); xbee.printf("log data\r\n"); + + return true; } /** コンフィグファイルを読み込む関数 * */ -void LoadConfig() +bool LoadConfig() { char value[20]; //Read a configuration file from a mbed. if (!cfg.read("/sd/config.txt")) { pc.printf("Config file does not exist\n"); + return false; } else { //Get values if (cfg.getValue("target_x", &value[0], sizeof(value))) target_x = atof(value); else { pc.printf("Failed to get value for target_x\n"); + return false; } if (cfg.getValue("target_y", &value[0], sizeof(value))) target_y = atof(value); else { pc.printf("Failed to get value for target_y\n"); + return false; } } + return true; } /** ログファイルの番号の最大値を得る関数 @@ -634,13 +646,13 @@ { if(p1.GetDim() != p2.GetDim()) return 0.0f; - static float mu_y = (p1.GetComp(2) + p2.GetComp(2)) * 0.5f; - static float s_mu_y = sin(mu_y); - static float w = sqrt(1 - GPS_SQ_E * s_mu_y * s_mu_y); - static float m = GPS_A * (1 - GPS_SQ_E) / (w * w * w); - static float n = GPS_A / w; - static float d1 = m * (p1.GetComp(2) - p2.GetComp(2)); - static float d2 = n * cos(mu_y) * (p1.GetComp(1) - p2.GetComp(1)); + float mu_y = (p1.GetComp(2) + p2.GetComp(2)) * 0.5f; + float s_mu_y = sin(mu_y); + float w = sqrt(1 - GPS_SQ_E * s_mu_y * s_mu_y); + float m = GPS_A * (1 - GPS_SQ_E) / (w * w * w); + float n = GPS_A / w; + float d1 = m * (p1.GetComp(2) - p2.GetComp(2)); + float d2 = n * cos(mu_y) * (p1.GetComp(1) - p2.GetComp(1)); return sqrt(d1 * d1 + d2 * d2); }