MPUとHMCでうごくかもver
Dependencies: ConfigFile SDFileSystem mbed
Fork of LAURUS_program by
Revision 27:a26ff85bba23, committed 2015-06-28
- Comitter:
- ojan
- Date:
- Sun Jun 28 15:42:26 2015 +0000
- Parent:
- 26:6e09df57ee91
- Child:
- 28:d993f3bbe302
- Commit message:
- LAURUS_Program_v2.5.2
Changed in this revision
main.cpp | Show annotated file Show diff for this revision Revisions of this file |
--- a/main.cpp Sun Jun 28 05:48:46 2015 +0000 +++ b/main.cpp Sun Jun 28 15:42:26 2015 +0000 @@ -12,14 +12,14 @@ #include "ConfigFile.h" /****************************** private define ******************************/ -#define RULE1 +//#define RULE1 //#define RULE2 //#define RULE3 //#define SERVO_DEBUG -//#define DIRECTION_DEBUG +#define DIRECTION_DEBUG #ifdef DIRECTION_DEBUG -const float TargetDirection = 90.0f; // 真西に飛ぶ +const float TargetDirection = -90.0f; // 真西に飛ぶ #endif const float dt = 0.01f; // 割り込み周期(s) const float ServoMax = 0.0046f; // サーボの最大パルス長(s) @@ -52,7 +52,7 @@ FILE * fp; // ログファイルのポインタ BufferedSerial xbee(PA_9, PA_10, PC_1); // Xbee ConfigFile cfg; // ConfigFile -PwmOut servoL(PC_7), servoR(PB_6); // サーボ用PWM出力 +PwmOut servoL(PB_6), servoR(PC_7); // サーボ用PWM出力 AnalogIn optSensor(PC_0); // 照度センサ用アナログ入力 AnalogIn servoVcc(PA_1); // バッテリー電圧監視用アナログ入力(サーボ用) AnalogIn logicVcc(PA_0); // バッテリー電圧監視用アナログ入力(ロジック用) @@ -86,6 +86,7 @@ Vector pre_p(2); // 過去の位置情報(経度, 緯度)(rad) int UTC_t = 0; // UTC時刻 int pre_UTC_t = 0; // 前のUTC時刻 +int ss = 0.0f; // 時刻の秒数の小数部分 Vector b_f(3); // 機体座標に固定された、機体前方向きのベクトル(x軸) Vector b_u(3); // 機体座標に固定された、機体上方向きのベクトル(z軸) @@ -258,12 +259,12 @@ if(pull_R < 0) pull_R = 0; else if(pull_R > PullMax) pull_R = PullMax; - servoL.pulsewidth((ServoMax - ServoMin) * pull_L / (float)PullMax + ServoMin); + servoL.pulsewidth((ServoMax - ServoMin) * (PullMax - pull_L) / (float)PullMax + ServoMin); servoR.pulsewidth((ServoMax - ServoMin) * pull_R / (float)PullMax + ServoMin); // データをmicroSDに保存し、XBeeでPCへ送信する - sprintf(data, "%d, %.1f,%.1f,%.1f, %.3f,%.6f,%.6f, %.3f,%.3f,%.1f, %d, %d,%d\r\n", - UTC_t, yaw, pitch, roll, + sprintf(data, "%d.%d, %.1f,%.1f,%.1f, %.3f,%.6f,%.6f, %.3f,%.3f,%.1f, %d, %d,%d\r\n", + UTC_t + 90000, ss, yaw, pitch, roll, press, gms.longitude, gms.latitude, vrt_acc, height, Distance(target_p, p), optSensor.read_u16(), pull_R, pull_L); @@ -292,12 +293,12 @@ */ void DataProcessing() { - static float R_11; - static float R_12; - static float r_cos; - static float r_sin; - static float p_cos; - static float p_sin; + static float R_11; // 回転行列(1,1)成分 + static float R_12; // 回転行列(1,2)成分 + static float r_cos; // ロール角のcos値 + static float r_sin; // ロール角のsin値 + static float p_cos; // ピッチ角のcos値 + static float p_sin; // ピッチ角のsin値 gms.read(); // GPSデータ取得 UTC_t = (int)gms.time; @@ -344,9 +345,10 @@ new_p.SetComp(2, gms.latitude * DEG_TO_RAD); p = new_p; pre_UTC_t = UTC_t; - + ss = 0; } else { // 更新されていなかったら p += 0.2f * (new_p - pre_p); + ss += 2; } height = Height(press);