hiroya taura
MPU6050のサンプルプログラム２
Dependencies: ConfigFile SDFileSystem mbed
Fork of
LAURUS_program
by 
LAURUS
Diff: main.cpp
- Revision:
- 24:8838be99cec3
- Parent:
- 23:79cdc1432160
- Child:
- 25:4c72d7420d8a
--- 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));