hiroya taura
MPU6050のサンプルプログラム２
Dependencies: ConfigFile SDFileSystem mbed
Fork of
LAURUS_program
by 
LAURUS
Diff: main.cpp
- Revision:
- 36:94dc027e05cd
- Parent:
- 34:4bda9af9a0cd
--- a/main.cpp Tue Nov 24 05:43:09 2015 +0000
+++ b/main.cpp Mon Dec 07 02:11:56 2015 +0000
@@ -11,45 +11,7 @@
#include "BufferedSerial.h"
#include "ConfigFile.h"
-/****************************** private define ******************************/
-//#define RULE1
-//#define RULE2
-//#define RULE3
-//#define RULE4
-//#define RULE3_1
-#define SERVO_DEBUG
-//#define DIRECTION_DEBUG
-//#define SPIRAL_DEBUG
-
-#ifdef DIRECTION_DEBUG
-const float TargetDirection = -179.0f; // 指定方向に飛ぶ
-
-#elif RULE3_1
-const float BetaMin = 0.2f * RAD_TO_DEG; // β最小値
-const float BetaVar = 0.3f * RAD_TO_DEG; // β変化量
-const float BetaMax = 1.0f * RAD_TO_DEG; // β最大値
-const float Span = 10.0f; // βの値が変わる距離間隔（Span[m]ごとにβの値がBetaVarだけ変化）
-
-#elif RULE4
-const float BorderHeight = 10.0f; // 制御則3と2を切り替える高度の閾値(m)
-#endif
-
const float dt = 0.01f; // 割り込み周期(s)
-const float ServoMax = 0.0046f; // サーボの最大パルス長(s)
-const float ServoMin = 0.0012f; // サーボの最小パルス長(s)
-const float Stabilize = 0.0f; // 滑空安定時の引っ張り量
-const float Turn = 25.0f; // 旋回時の引っ張り量
-const float PullRate = 7.5f; // 紐の引っ張り速度
-const float PullMax = 25.0f; // 引っ張れる紐の最大量(mm)
-const float BorderSpiral = 40.0f; // スパイラル検知角度
-const short BorderOpt = 30000; // 光センサーの閾値
-const float BorderGravity = 0.3f; // 無重力状態の閾値
-const int BorderParafoil = 0; // 物理スイッチのOFF出力
-const int MaxCount = 3; // 投下シグナルを何回連続で検知したら投下と判断するか（×0.2[s]）
-const int WaitTime = 6; // 投下後、安定するまで何秒滑空するか
-const float Alpha = 30.0f; // 目標方向と自分の進行方向との差の閾値(deg)（制御則1&2&3の定数
-const float Beta = 60.0f; // 目標方向と自分の進行方向との間に取るべき角度差(deg)（制御則3の定数
-const float BorderDistance = 5.0f; // 落下制御に入るための目標値との距離の閾値(m)（制御則2の定数
enum Direction {LEFT, RIGHT, NEUTRAL};
@@ -57,22 +19,13 @@
/****************************** private typedef ******************************/
/****************************** private variables ******************************/
DigitalOut myled(LED1); // デバッグ用LEDのためのデジタル出力
-I2C i2c(PB_9, PB_8); // I2Cポート
+I2C i2c(p9, p8); // I2Cポート
MPU6050 mpu(&i2c); // 加速度・角速度センサ
HMC5883L hmc(&i2c); // 地磁気センサ
LPS25H lps(&i2c); // 気圧センサ
-Serial gps(PA_11, PA_12); // GPS通信用シリアルポート
-Serial pc(SERIAL_TX, SERIAL_RX); // PC通信用シリアルポート
-GMS6_CR6 gms(&gps, &pc); // GPS
-SDFileSystem sd(PB_5, PB_4, PB_3, PB_10, "sd"); // microSD
+Serial pc(USBTX,USBRX); // PC通信用シリアルポート
FILE * fp; // ログファイルのポインタ
-BufferedSerial xbee(PA_9, PA_10, PC_1, 256, 16); // Xbee
ConfigFile cfg; // ConfigFile
-PwmOut servoL(PB_6), servoR(PC_7); // サーボ用PWM出力
-AnalogIn optSensor(PC_0); // 照度センサ用アナログ入力
-AnalogIn servoVcc(PA_1); // バッテリー電圧監視用アナログ入力(サーボ用)
-AnalogIn logicVcc(PA_0); // バッテリー電圧監視用アナログ入力(ロジック用)
-DigitalIn paraSensor(PB_0); // パラフォイルに繋がる（予定）の物理スイッチ
Ticker ticker; // 割り込みタイマー
Timer timer; // 時間計測用タイマー
Direction dir = NEUTRAL; // 旋回方向
@@ -166,24 +119,16 @@
/****************************** private functions ******************************/
-bool SD_Init(); // SDカード初期化
void SensorsInit(); // 各種センサーの初期化
void KalmanInit(); // カルマンフィルタ初期化
-bool LoadConfig(); // config読み取り
-int Find_last(); // SDカード初期化用関数
void DataProcessing(); // データ処理関数
-void ControlRoutine(); // 制御ルーチン関数
void KalmanUpdate(); // カルマンフィルタ更新
-float Distance(Vector p1, Vector p2); // 緯度・経度の差から2点間の距離を算出(m)
-bool Thrown(); // 投下されたかどうかを判断する
void DataUpdate(); // データ取得&更新関数
-void toString(Matrix& m); // デバッグ用
-void toString(Vector& v); // デバッグ用
/** 小さい値を返す関数
* @param a: 1つ目の入力値
* @param b: 2つ目の入力値
- *
+ *
* @return a,bのうち、小さい方の値
*/
inline float min(float a, float b)
@@ -191,52 +136,17 @@
return ((a > b) ? b : a);
}
-/** 気圧の変化から高度を計算する関数
- * @param press: 現在の気圧
- *
- * @return height: 現在の基準点からの高度
- */
-inline float Height(float press) {
- return (153.8f * (pow((p0/press),0.1902f)-1.0f)*(25.0f+273.15f));
-}
-
/****************************** main function ******************************/
int main()
{
i2c.frequency(400000); // I2Cの通信速度を400kHzに設定
-
- //Config読み取り
- xbee.printf("load...");
- while(!LoadConfig()) {
- wait(0.1f);
- xbee.printf(".");
- }
- xbee.printf("complete\r\n");
- xbee.printf("target(%.5f, %.5f)\r\n", target_x, target_y);
-
- // SDカード初期化
- xbee.printf("SD Init...");
- while(!SD_Init()) {
- wait(0.1f);
- xbee.printf(".");
- }
- xbee.printf("complete\r\n");
-
- // センサー関連の初期化
- xbee.printf("Sensors Init...");
- SensorsInit();
- xbee.printf("complete\r\n");
-
//カルマンフィルタ初期化
KalmanInit();
- NVIC_SetPriority(TIM5_IRQn, 5);
ticker.attach(&DataUpdate, dt); // 割り込み有効化（Freq = 0.01fなので、10msおきの割り込み）
-
- servoL.period(0.020f); // サーボの信号の周期は20ms
- servoR.period(0.020f);
+
/* ------------------------------ ↓↓↓ ここからメインループ ↓↓↓ ------------------------------ */
@@ -245,61 +155,12 @@
timer.reset();
timer.start();
myled = 1; // LED is ON
-
+
INT_flag = false; // 割り込みによる変数書き換えを阻止
/******************************* データ処理 *******************************/
DataProcessing();
-
- /******************************* 制御ルーチン *******************************/
-#ifdef SERVO_DEBUG
- if(xbee.readable()) {
- char cmd = xbee.getc();
- if(cmd == 'w') {
- pull_R = 20.0f;
- pull_L = 0.0f;
- } else if(cmd == 's'){
- pull_R = 10.0f;
- pull_L = 10.0f;
- } else if(cmd == 'a'){
- pull_R = 0.0f;
- pull_L = 20.0f;
- } else if(cmd == 'd'){
- pull_R = 10.0f;
- pull_L = 10.0f;
- }
- }
-#else
- ControlRoutine();
-#endif
-
- sv = (float)servoVcc.read_u16() * ADC_LSB_TO_V * 2.0f; // サーボ電源電圧
- lv = (float)logicVcc.read_u16() * ADC_LSB_TO_V * 2.0f; // ロジック電源電圧
-
-
-
- // データをmicroSDに保存し、XBeeでPCへ送信する
- sprintf(data, "%d.%d, %.1f,%.1f,%.1f, %.3f,%.6f,%.6f, %.3f,%.3f,%.1f, %d, %.1f,%.1f\r\n",
- UTC_t, ss, yaw, pitch, roll,
- press, gms.longitude, gms.latitude,
- vrt_acc, height, 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
-
- loopTime = timer.read_ms();
-
- // ループはきっかり0.2秒ごと
- while(timer.read_ms() < 200);
-
+ INT_flag = true;
}
-
/* ------------------------------ ↑↑↑ ここまでメインループ ↑↑↑ ------------------------------ */
}
@@ -314,9 +175,7 @@
static float r_sin; // ロール角のsin値
static float p_cos; // ピッチ角のcos値
static float p_sin; // ピッチ角のsin値
-
- gms.read(); // GPSデータ取得
- UTC_t = (int)gms.time + 90000;
+
// 参照座標系の基底を求める
r_u = g;
@@ -337,7 +196,7 @@
} else {
roll = -acos(r_cos) * RAD_TO_DEG;
}
-
+
p_cos = g.GetPerpCompTo(b_l).Normalize() * b_u;
p_sin = Cross(g.GetPerpCompTo(b_l).Normalize(), b_u) * b_l;
if(p_sin > 0.0f) {
@@ -348,196 +207,18 @@
if(yaw > 180.0f) yaw -= 360.0f; // ヨー角を[-π, π]に補正
else if(yaw < -180.0f) yaw += 360.0f; // ヨー角を[-π, π]に補正
-
- if(UTC_t - pre_UTC_t >= 1) { // GPSデータが更新されていたら
- pre_p = new_p;
- new_p.SetComp(1, gms.longitude * DEG_TO_RAD);
- 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);
-
-
-
}
-/** 制御ルーチン関数
- *
- */
-void ControlRoutine()
-{
- volatile static int count = 0;
- static float target_lng;
- static float target_lat;
- static float target_X;
- static float target_Y;
- static float theta;
- static float delta_theta;
-
- switch(step) {
-
- // 投下&安定滑空シーケンス
- case 0:
- if(Thrown() || count != 0) {
- count++;
- pull_L = Stabilize;
- pull_R = Stabilize;
- if(count >= WaitTime*5) {
- pull_L = 0.0f;
- pull_R = 0.0f;
-#ifdef SPIRAL_DEBUG
- step = 2;
-#else
- step = 1;
-#endif
- }
- }
- break;
-
- // 制御シーケンス
- case 1:
- if(fabs(roll) > BorderSpiral) {
- // スパイラル回避行動
- /*if(roll > 0) {
- pull_L = PullMax;
- pull_R = 0.0f;
- } else {
- pull_L = 0.0f;
- pull_R = PullMax;
- }*/
-
- } else {
-
- /* いずれも地球を完全球体と仮定 */
- target_lng = target_x * DEG_TO_RAD;
- target_lat = target_y * DEG_TO_RAD;
- /* 北から西回りで目標方向の角度を出力 */
- target_Y = cos( target_lat ) * sin( target_lng - p.GetComp(1) );
- target_X = cos( p.GetComp(2) ) * sin( target_lat ) - sin( p.GetComp(2) ) * cos( target_lat ) * cos( target_lng - p.GetComp(1) );
-
-#ifdef RULE4
- if(height > BorderHeight) {
- theta = -atan2f( target_Y, target_X ) * RAD_TO_DEG + Beta;
- if(theta > 180.0f) theta -= 360.0f;
- } else {
- theta = -atan2f( target_Y, target_X ) * RAD_TO_DEG;
- }
-#elif RULE3
- theta = -atan2f( target_Y, target_X ) * RAD_TO_DEG + Beta;
-
- if(theta > 180.0f) theta -= 360.0f;
- else if(theta < -180.0f) theta += 360.0f;
-#elif RULE3_1
- float d = Distance(target_p, p);
- beta = BetaMax - BetaVar * (int)(d / Span);
- if(beta < BetaMin) beta = BetaMin;
-
- theta = -atan2f( target_Y, target_X ) * RAD_TO_DEG + beta;
-
- if(theta > 180.0f) theta -= 360.0f;
- else if(theta < -180.0f) theta += 360.0f;
-
-#else
- theta = -atan2f( target_Y, target_X ) * RAD_TO_DEG;
-#endif
-
-#ifdef DIRECTION_DEBUG
- theta = TargetDirection;
-#endif
-
- if(yaw >= 0.0f) { // ヨー角が正
- if(theta >= 0.0f) { // 目標角も正ならば、
- 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 = LEFT;
- else dir = NEUTRAL;
- } else { // 180°より大きければ右旋回
- if(360.0f - delta_theta > Alpha) dir = RIGHT;
- else dir = NEUTRAL;
- }
- }
- } else { // ヨー角が負
- if(theta <= 0.0f) { // 目標角も負ならば、
- 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 = LEFT;
- else dir = NEUTRAL;
- } else { // 180°より大きければ右旋回
- if(360.0f - delta_theta > Alpha) dir = RIGHT;
- else dir = NEUTRAL;
- }
- }
- }
-
- if(dir == LEFT) { //目標は左方向
-
- pull_L = Turn;
- pull_R = 0.0f;
-
- } else if (dir == RIGHT) { //目標は右方向
-
- pull_L = 0.0f;
- pull_R = Turn;
-
- } else if (dir == NEUTRAL) {
- pull_L = 0.0f;
- pull_R = 0.0f;
- }
- }
-#ifdef RULE4
- if(height <= BorderHeight) {
- // 目標地点との距離が閾値以下だった場合、落下シーケンスへと移行する
- if(Distance(target_p, p) < BorderDistance) step = 2;
- }
-#else
-#ifdef RULE2
- // 目標地点との距離が閾値以下だった場合、落下シーケンスへと移行する
- if(Distance(target_p, p) < BorderDistance) step = 2;
-#endif
-#endif
-
- break;
-
-#if defined(RULE2) || defined(RULE4) || defined(SPIRAL_DEBUG)
- // 落下シーケンス
- case 2:
- pull_L = 0.0f;
- pull_R = PullMax;
-
-#ifndef SPIRAL_DEBUG
- // もし落下中に目標値から離れてしまった場合は、体勢を立て直して再び滑空
- // 境界で制御が不安定にならないよう閾値にマージンを取る
- if(Distance(target_p, p) > BorderDistance+3.0f) step = 1;
-#endif
- break;
-#endif
- }
-}
/** 各種センサーの初期化を行う関数
*
*/
void SensorsInit()
{
-
+
if(!mpu.init()) error("mpu6050 Initialize Error !!"); // mpu6050初期化
if(!hmc.init()) error("hmc5883l Initialize Error !!"); // hmc5883l初期化
-
+
//重力ベクトルの初期化
raw_g.SetComp(1, 0.0f);
raw_g.SetComp(2, 0.0f);
@@ -555,91 +236,8 @@
// 目標座標をベクトルに代入
target_p.SetComp(1, target_x * DEG_TO_RAD);
target_p.SetComp(2, target_y * DEG_TO_RAD);
-
- // 地表の気圧を取得
- p0 = 0.0f;
- for(int i=0; i<10; i++) {
- p0 += (float)lps.pressure() * PRES_LSB_TO_HPA;
- wait(0.1f);
- }
- p0 /= 10.0f;
-
- xbee.printf(".");
-}
-
-/** マイクロSDカードの初期化を行う関数
- *
- */
-bool SD_Init()
-{
- //SDカード初期化
- char filename[15];
- int n = Find_last();
- if(n < 0) {
- pc.printf("Could not read a SD Card.\n");
- 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;
}
-/** コンフィグファイルを読み込む関数
- *
- */
-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;
-}
-
-/** ログファイルの番号の最大値を得る関数
- *
- * @return マイクロSD内に存在するログファイル番号の最大値
- */
-int Find_last()
-{
- int i, n = 0;
- char c;
- DIR *dp;
- struct dirent *dirst;
- dp = opendir("/sd/");
- if (!dp) {
- pc.printf("Could not open directry.\n");
- return -1;
- }
- while((dirst = readdir(dp)) != NULL) {
- if(sscanf(dirst->d_name, "log%03d.csv%c", &i, &c) == 1 && i>n) {
- n = i;
- }
- }
- closedir(dp);
- return n;
-}
-
-/** カルマンフィルタの初期化を行う関数
- *
- */
void KalmanInit()
{
// 重力
@@ -778,63 +376,6 @@
}
}
-/** GPS座標から距離を算出
- * @param 座標1（経度、緯度）（rad）
- * @param 座標2（経度、緯度）（rad）
- *
- * @return 2点間の距離（m）
- */
-float Distance(Vector p1, Vector p2)
-{
- static float mu_y;
- static float s_mu_y;
- static float w;
- static float m;
- static float n;
- static float d1;
- static float d2;
-
- if(p1.GetDim() != p2.GetDim()) return 0.0f;
-
- mu_y = (p1.GetComp(2) + p2.GetComp(2)) * 0.5f;
- s_mu_y = sin(mu_y);
- w = sqrt(1 - GPS_SQ_E * s_mu_y * s_mu_y);
- m = GPS_A * (1 - GPS_SQ_E) / (w * w * w);
- n = GPS_A / w;
- d1 = m * (p1.GetComp(2) - p2.GetComp(2));
- d2 = n * cos(mu_y) * (p1.GetComp(1) - p2.GetComp(1));
-
- return sqrt(d1 * d1 + d2 * d2);
-}
-
-/** 投下を検知する関数
- *
- * @return 投下されたかどうか（true=投下 false=未投下
- *
- */
-bool Thrown()
-{
- volatile static int opt_count = 0;
- volatile static int g_count = 0;
- volatile static int para_count = 0;
-
-#ifndef SPIRAL_DEBUG
- /*if(optSensor.read_u16() > BorderOpt) opt_count++;
- else opt_count = 0;
- if(vrt_acc < BorderGravity) g_count++;
- else g_count = 0;*/
-#endif
- if((int)paraSensor == BorderParafoil) para_count++;
- else para_count = 0;
-
- if(opt_count >= MaxCount || g_count >= MaxCount || para_count >= MaxCount) {
- return true;
- }
-
- return false;
-
-}
-
/* ------------------------------ 割り込み関数 ------------------------------ */
/** データ取得＆更新関数
@@ -842,32 +383,6 @@
*/
void DataUpdate()
{
- // サーボ用変数（引き量）
- static float pL = 0.0f;
- static float pR = 0.0f;
-
- // 引き量が目標値から一定以上離れていれば、引き量を一定の速度で更新する
- if(pull_L - pL > PullRate * dt) {
- pL += PullRate * dt;
- } else if(pull_L - pL < -PullRate * dt) {
- pL -= PullRate * dt;
- }
-
- if(pull_R - pR > PullRate * dt) {
- pR += PullRate * dt;
- } else if(pull_R - pR < -PullRate * dt) {
- pR -= PullRate * dt;
- }
-
- // 指定された引っ張り量だけ紐を引っ張る
- if(pL < 0.0f) pL = 0.0f;
- else if(pL > PullMax) pL = PullMax;
- if(pR < 0.0f) pR = 0.0f;
- else if(pR > PullMax) pR = PullMax;
-
- servoL.pulsewidth((ServoMax - ServoMin) * (PullMax - pL) / PullMax + ServoMin);
- servoR.pulsewidth((ServoMax - ServoMin) * pR / PullMax + ServoMin);
-
// センサーの値を更新
mpu.read();
hmc.read();
@@ -884,13 +399,6 @@
KalmanUpdate();
- // LPS25Hによる気圧の取得は10Hz
- lps_cnt = (lps_cnt+1)%10;
- if(lps_cnt == 0) {
- raw_press = (float)lps.pressure() * PRES_LSB_TO_HPA;
- raw_temp = TempLsbToDeg(lps.temperature());
- }
-
if(INT_flag) {
g = raw_g;
for(int i=0; i<3; i++) {
@@ -902,30 +410,5 @@
temp = raw_temp;
vrt_acc = raw_acc * raw_g;
-
}
}
-
-/* ------------------------------ デバッグ用関数 ------------------------------ */
-
-void toString(Matrix& m)
-{
-
- for(int i=0; i<m.GetRow(); i++) {
- for(int j=0; j<m.GetCol(); j++) {
- pc.printf("%.6f\t", m.GetComp(i+1, j+1));
- }
- pc.printf("\r\n");
- }
-
-}
-
-void toString(Vector& v)
-{
-
- for(int i=0; i<v.GetDim(); i++) {
- pc.printf("%.6f\t", v.GetComp(i+1));
- }
- pc.printf("\r\n");
-
-}
\ No newline at end of file