Revision 29:59f4808e2eb6, committed 2015-07-02

Comitter:

ojan

Date:

Thu Jul 02 16:00:30 2015 +0000

Parent:

28:d993f3bbe302

Child:

30:fb310564097b

Commit message:

LAURUS_Program_v2.6.0; ; + add RULE4 (combination of 2 & 3); + add SPIRAL_DEBUG mode; + change the number of data in LPS25H's moving average filter (16 -> 8)

Changed in this revision

--- a/LPS25H/LPS25H.cpp	Wed Jul 01 14:57:24 2015 +0000
+++ b/LPS25H/LPS25H.cpp	Thu Jul 02 16:00:30 2015 +0000
@@ -78,8 +78,8 @@
 {
     // Resolution config
     put(LPS25H_RES_CONF, 0x0A);
-    // FIFO 4samples moving average
-    put(LPS25H_FIFO_CTRL, 0xCF);
+    // FIFO 8samples moving average
+    put(LPS25H_FIFO_CTRL, 0xC7);
     // FIFO Enable
     put(LPS25H_CTRL_REG2, 0x40);
     // Power ON Cycle=12.5Hz

--- a/main.cpp	Wed Jul 01 14:57:24 2015 +0000
+++ b/main.cpp	Thu Jul 02 16:00:30 2015 +0000
@@ -15,11 +15,13 @@
 //#define RULE1
 //#define RULE2
 //#define RULE3
+//#define RULE4
 //#define SERVO_DEBUG
 #define DIRECTION_DEBUG
+//#define SPIRAL_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)
@@ -33,7 +35,8 @@
 const int   WaitTime        = 1;            // 投下後、安定するまで何秒滑空するか
 const float Alpha           = 30.0f;        // 目標方向と自分の進行方向との差の閾値(deg)（制御則1&2&3の定数
 const float Beta            = 60.0f;        // 目標方向と自分の進行方向との間に取るべき角度差(deg)（制御則3の定数
-const float BorderDistance  = 10.0f;        // 落下制御に入るための目標値との距離の閾値(m)（制御則2の定数
+const float BorderDistance  = 5.0f;         // 落下制御に入るための目標値との距離の閾値(m)（制御則2の定数
+const float BorderHeight    = 10.0f;        // 制御則3と2を切り替える高度の閾値(m)
 
 enum Direction {LEFT, RIGHT, NEUTRAL};
 
@@ -106,7 +109,6 @@
 float   vrt_acc = 0.0f;         // 鉛直方向の加速度成分（落下検知に使用）
 
 int     step = 0;               // シーケンス制御のステップ
-int     count = 0;              // 安定滑空までの時間測定用カウンター
 char    data[512] = {};         // 送信データ用配
 int     loopTime = 0;           // 1ループに掛かる時間（デバッグ用
 float   sv = 0.0f;              // サーボ電源電圧
@@ -264,7 +266,7 @@
         
         // データをmicroSDに保存し、XBeeでPCへ送信する
         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,
+                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);
@@ -301,7 +303,7 @@
     static float p_sin;                             // ピッチ角のsin値
     
     gms.read();                                     // GPSデータ取得
-    UTC_t = (int)gms.time;
+    UTC_t = (int)gms.time + 90000;
 
     // 参照座標系の基底を求める
     r_u = g;
@@ -314,12 +316,7 @@
 
     R_11 = r_f * b_f;                         // 回転行列の(1,1)成分
     R_12 = r_f * b_l;                         // 回転行列の(1,2)成分
-
-#ifdef RULE3
-    yaw = atan2(-R_12, R_11) * RAD_TO_DEG + MAG_DECLINATION - Beta;
-#else
     yaw = atan2(-R_12, R_11) * RAD_TO_DEG + MAG_DECLINATION;
-#endif
     r_cos = g.GetPerpCompTo(b_f).Normalize() * b_u;
     r_sin = Cross(g.GetPerpCompTo(b_f).Normalize(), b_u) * b_f;
     if(r_sin > 0.0f) {
@@ -336,8 +333,8 @@
         pitch = -acos(p_cos) * RAD_TO_DEG;
     }
 
-    if(yaw < -180.0f) yaw += 360.0f;                // ヨー角を[-π, π]に補正
     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;
@@ -362,6 +359,7 @@
  */
 void ControlRoutine()
 {
+    static int   count = 0;
     static float target_lng;
     static float target_lat;
     static float target_X;
@@ -381,7 +379,11 @@
                 if(count >= WaitTime*5) {
                     pull_L = 0;
                     pull_R = 0;
+#ifdef SPIRAL_DEBUG
+                    step = 2;
+#else
                     step = 1;
+#endif
                 }
             }
             break;
@@ -405,8 +407,23 @@
                 /* 北から西回りで目標方向の角度を出力 */
                 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;
+#else
                 theta = -atan2f( target_Y, target_X ) * RAD_TO_DEG;
-                delta_theta = 0.0f;
+#endif
+
 #ifdef DIRECTION_DEBUG
                 theta = TargetDirection;
 #endif
@@ -459,23 +476,31 @@
                     pull_R = 0;
                 }
             }
-
+#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;
 
-#ifdef RULE2
+#if defined(RULE2) || defined(RULE4) || defined(SPIRAL_DEBUG)
             // 落下シーケンス
         case 2:
-            pull_L = PullMax;
-            pull_R = 0;
+            pull_L = 0;
+            pull_R = PullMax;
 
+#ifndef SPIRAL_DEBUG
             // もし落下中に目標値から離れてしまった場合は、体勢を立て直して再び滑空
             // 境界で制御が不安定にならないよう閾値にマージンを取る
             if(Distance(target_p, p) > BorderDistance+3.0f) step = 1;
+#endif
             break;
 #endif
     }