Revision 36:94dc027e05cd, committed 2015-12-07

Comitter:

taurin

Date:

Mon Dec 07 02:11:56 2015 +0000

Parent:

35:e490f3de2cde

Commit message:

MPU?HMC??????ver

Changed in this revision

--- 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