MPUとHMCでうごくかもver
Dependencies: ConfigFile SDFileSystem mbed
Fork of LAURUS_program by
Diff: main.cpp
- Revision:
- 24:8838be99cec3
- Parent:
- 23:79cdc1432160
- Child:
- 25:4c72d7420d8a
diff -r 79cdc1432160 -r 8838be99cec3 main.cpp --- a/main.cpp Wed Jun 24 16:16:14 2015 +0000 +++ b/main.cpp Thu Jun 25 17:40:18 2015 +0000 @@ -12,12 +12,19 @@ #include "ConfigFile.h" /****************************** private define ******************************/ -#define TRUE 1 -#define FALSE 0 +#define LEFT 0 +#define RIGHT 1 +#define NEUTRAL 2 #define RULE1 //#define RULE2 //#define RULE3 +//#define SERVO_DEBUG +//#define DIRECTION_DEBUG +#ifdef DIRECTION_DEBUG +const float TargetDirection = 90.0f; // 真西に飛ぶ +#endif + const float dt = 0.01f; // 割り込み周期(s) const float ServoMax = 0.0046f; // サーボの最大パルス長(s) @@ -48,7 +55,7 @@ FILE * fp; // ログファイルのポインタ BufferedSerial xbee(PA_9, PA_10, PC_1); // Xbee ConfigFile cfg; // ConfigFile -PwmOut servoL(PB_6), servoR(PC_7); // サーボ用PWM出力 +PwmOut servoL(PC_7), servoR(PB_6); // サーボ用PWM出力 AnalogIn optSensor(PC_0); // 照度センサ用アナログ入力 AnalogIn servoVcc(PA_1); // バッテリー電圧監視用アナログ入力(サーボ用) AnalogIn logicVcc(PA_0); // バッテリー電圧監視用アナログ入力(ロジック用) @@ -57,7 +64,7 @@ Timer timer; // 時間計測用タイマー int lps_cnt = 0; // 気圧センサ読み取りカウント -uint8_t INT_flag = TRUE; // 割り込み可否フラグ +bool INT_flag = true; // 割り込み可否フラグ /* こちらの変数群はメインループでは参照しない */ Vector raw_acc(3); // 加速度(m/s^2) 生 Vector raw_gyro(3); // 角速度(deg/s) 生 @@ -68,8 +75,10 @@ Vector acc(3); // 加速度(m/s^2) Vector gyro(3); // 角速度(deg/s) Vector geomag(3); // 地磁気(?) +float p0; // 気圧の初期値 float press; // 気圧(hPa) float temp; // 温度(℃) +float height; // 高さ(m) Vector raw_g(3); // 重力ベクトル 生 Vector g(3); // 重力ベクトル @@ -157,6 +166,10 @@ return ((a > b) ? b : a); } +inline float Height(float press, float temp) { + return (153.8f * (pow((p0/press),0.1902f)-1.0f)*(temp+273.15f)); +} + /****************************** main function ******************************/ int main() @@ -164,15 +177,19 @@ i2c.frequency(400000); // I2Cの通信速度を400kHzに設定 - // センサー関連の初期化 - SensorsInit(); - //Config読み取り - while(!LoadConfig()); + while(!LoadConfig()) { + wait(0.1f); + } xbee.printf("target(%.5f, %.5f)\r\n", target_x, target_y); // SDカード初期化 - while(!SD_Init()); + while(!SD_Init()) { + wait(0.1f); + } + + // センサー関連の初期化 + SensorsInit(); //カルマンフィルタ初期化 KalmanInit(); @@ -191,16 +208,31 @@ timer.start(); myled = 1; // LED is ON - INT_flag = FALSE; // 割り込みによる変数書き換えを阻止 + INT_flag = false; // 割り込みによる変数書き換えを阻止 /******************************* データ処理 *******************************/ DataProcessing(); /******************************* 制御ルーチン *******************************/ +#ifdef SERVO_DEBUG + if(xbee.readable()) { + char cmd = xbee.getc(); + if(cmd == 'w') { + pull_R += 5; + } else if(cmd == 's'){ + pull_R -= 5; + } else if(cmd == 'a'){ + pull_L += 5; + } else if(cmd == 'd'){ + pull_L -= 5; + } + } +#else ControlRoutine(); +#endif 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; @@ -211,14 +243,14 @@ 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", + sprintf(data, "%d, %.1f,%.1f,%.1f, %.3f,%.5f,%.5f, %.3f,%.3f,%.3f, %.3f,%d, %d,%d\r\n", UTC_t, yaw, pitch, roll, press, gms.longitude, gms.latitude, - vrt_acc, temp, (int)paraSensor, + vrt_acc, temp, height, Distance(target_p, p), optSensor.read_u16(), pull_R, pull_L); - INT_flag = TRUE; // 割り込み許可 + INT_flag = true; // 割り込み許可 fprintf(fp, data); fflush(fp); @@ -279,6 +311,8 @@ // GPSの補完処理を追加予定 } + height = Height(press, temp); + pre_p = p; pre_UTC_t = UTC_t; @@ -328,51 +362,54 @@ float targetX = cos( p.GetComp(2) ) * sin( target_lat ) - sin( p.GetComp(2) ) * cos( target_lat ) * cos( target_lng - p.GetComp(1) ); float theta = -atan2f( targetY, targetX ) * RAD_TO_DEG; float delta_theta = 0.0f; +#ifdef DIRECTION_DEBUG + theta = TargetDirection; +#endif if(yaw >= 0.0f) { // ヨー角が正 if(theta >= 0.0f) { // 目標角も正ならば、 - if(theta - yaw > Alpha) dir = 0; // 単純に差を取って閾値αと比べる - else if(theta - yaw < -Alpha) dir = 1; - else dir = 2; + if(theta - yaw > Alpha) dir = LEFT; // 単純に差を取って閾値αと比べる + else if(theta - yaw < -Alpha) dir = RIGHT; + else dir = NEUTRAL; } 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; + if(delta_theta > Alpha) dir = LEFT; + else dir = NEUTRAL; } else { // 180°より大きければ右旋回 - if(360.0f - delta_theta > Alpha) dir = 1; - else dir = 2; + if(360.0f - delta_theta > Alpha) dir = RIGHT; + else dir = NEUTRAL; } } } else { // ヨー角が負 if(theta <= 0.0f) { // 目標角も負ならば、 - if(theta - yaw > Alpha) dir = 0; // 単純に差を取って閾値αと比べる - else if(theta - yaw < -Alpha) dir = 1; - else dir = 2; + if(theta - yaw > Alpha) dir = LEFT; // 単純に差を取って閾値αと比べる + else if(theta - yaw < -Alpha) dir = RIGHT; + else dir = NEUTRAL; } else { // 目標角が正であれば、 delta_theta = theta - yaw; // 差を取る(yawから左回りに見たthetaとの差分) if(delta_theta < 180.0f) { // 差が180°より小さければ左旋回 - if(delta_theta > Alpha) dir = 0; - else dir = 2; + if(delta_theta > Alpha) dir = LEFT; + else dir = NEUTRAL; } else { // 180°より大きければ右旋回 - if(360.0f - delta_theta > Alpha) dir = 1; - else dir = 2; + if(360.0f - delta_theta > Alpha) dir = RIGHT; + else dir = NEUTRAL; } } } - if(dir == 0) { //目標は左方向 + if(dir == LEFT) { //目標は左方向 pull_L = 20; pull_R = 0; - } else if (dir == 1) { //目標は右方向 + } else if (dir == RIGHT) { //目標は右方向 pull_L = 0; pull_R = 20; - } else if (dir == 2) { + } else if (dir == NEUTRAL) { pull_L = 0; pull_R = 0; } @@ -425,6 +462,13 @@ // 目標座標をベクトルに代入 target_p.SetComp(1, target_x * DEG_TO_RAD); target_p.SetComp(2, target_y * DEG_TO_RAD); + + // 地表の気圧を取得 + p0 = 0; + for(int i=0; i<10; i++) { + p0 += (float)lps.pressure() * PRES_LSB_TO_HPA; + } + p0 /= 10.0f; } /** マイクロSDカードの初期化を行う関数 @@ -436,7 +480,7 @@ char filename[15]; int n = Find_last(); if(n < 0) { - xbee.printf("Could not read a SD Card.\n"); + pc.printf("Could not read a SD Card.\n"); return false; } sprintf(filename, "/sd/log%03d.csv", n+1); @@ -455,18 +499,18 @@ char value[20]; //Read a configuration file from a mbed. if (!cfg.read("/sd/config.txt")) { - xbee.printf("Config file does not exist\n"); + 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 { - xbee.printf("Failed to get value for target_x\n"); + 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 { - xbee.printf("Failed to get value for target_y\n"); + pc.printf("Failed to get value for target_y\n"); return false; } } @@ -712,10 +756,10 @@ lps_cnt = (lps_cnt+1)%10; if(lps_cnt == 0) { raw_press = (float)lps.pressure() * PRES_LSB_TO_HPA; - //raw_temp = TempLsbToDeg(lps.temperature()); + raw_temp = TempLsbToDeg(lps.temperature()); } - if(INT_flag != FALSE) { + if(INT_flag) { g = raw_g; for(int i=0; i<3; i++) { geomag.SetComp(i+1, post_x1.GetComp(i+1));