hiroya taura
MPU6050のサンプルプログラム２
Dependencies: ConfigFile SDFileSystem mbed
Fork of
LAURUS_program
by 
LAURUS
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);
}