Diff: main.cpp

Revision:

34:4bda9af9a0cd

Parent:

33:56163d4e2c53

Child:

36:94dc027e05cd

--- a/main.cpp	Sun Jul 19 02:21:43 2015 +0000
+++ b/main.cpp	Thu Aug 06 15:45:48 2015 +0000
@@ -16,19 +16,30 @@
 //#define RULE2
 //#define RULE3
 //#define RULE4
+//#define RULE3_1
 #define SERVO_DEBUG
 //#define DIRECTION_DEBUG
 //#define SPIRAL_DEBUG
 
 #ifdef DIRECTION_DEBUG
-const float TargetDirection = 0.0f;        // 指定方向に飛ぶ
+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        = 5.0f;         // 紐の引っ張り速度
+const float PullRate        = 7.5f;         // 紐の引っ張り速度
 const float PullMax         = 25.0f;        // 引っ張れる紐の最大量(mm)
 const float BorderSpiral    = 40.0f;        // スパイラル検知角度
 const short BorderOpt       = 30000;        // 光センサーの閾値
@@ -39,7 +50,6 @@
 const float Alpha           = 30.0f;        // 目標方向と自分の進行方向との差の閾値(deg)（制御則1&2&3の定数
 const float Beta            = 60.0f;        // 目標方向と自分の進行方向との間に取るべき角度差(deg)（制御則3の定数
 const float BorderDistance  = 5.0f;         // 落下制御に入るための目標値との距離の閾値(m)（制御則2の定数
-const float BorderHeight    = 10.0f;        // 制御則3と2を切り替える高度の閾値(m)
 
 enum Direction {LEFT, RIGHT, NEUTRAL};
 
@@ -67,8 +77,8 @@
 Timer           timer;                              // 時間計測用タイマー
 Direction       dir = NEUTRAL;                      // 旋回方向
 
-int     lps_cnt = 0;            // 気圧センサ読み取りカウント
-bool    INT_flag = true;        // 割り込み可否フラグ
+volatile int     lps_cnt = 0;                       // 気圧センサ読み取りカウント
+volatile bool    INT_flag = true;                   // 割り込み可否フラグ
 /* こちらの変数群はメインループでは参照しない */
 Vector  raw_acc(3);             // 加速度(m/s^2)  生
 Vector  raw_gyro(3);            // 角速度(deg/s)  生
@@ -102,8 +112,8 @@
 Vector  r_u(3);                 // 世界座標に固定された、上向きのベクトル（Z軸）
 Vector  r_l(3);                 // 世界座標に固定された、西向きのベクトル（Y軸）
 
-float     pull_L = 0.0f;             // 左サーボの引っ張り量（目標値）(mm：0~PullMax)
-float     pull_R = 0.0f;             // 右サーボの引っ張り量（目標値）(mm：0~PullMax)
+float     pull_L = 10.0f;             // 左サーボの引っ張り量（目標値）(mm：0~PullMax)
+float     pull_R = 10.0f;             // 右サーボの引っ張り量（目標値）(mm：0~PullMax)
 
 float   yaw = 0.0f;             // 本体のヨー角（deg）z軸周り
 float   pitch = 0.0f;           // 本体のピッチ角（deg）y軸周り
@@ -111,12 +121,15 @@
 
 float   vrt_acc = 0.0f;         // 鉛直方向の加速度成分（落下検知に使用）
 
-int     step = 0;               // シーケンス制御のステップ
+volatile int     step = 0;      // シーケンス制御のステップ
 char    data[512] = {};         // 送信データ用配
 int     loopTime = 0;           // 1ループに掛かる時間（デバッグ用
 float   sv = 0.0f;              // サーボ電源電圧
 float   lv = 0.0f;              // ロジック電源電圧
 
+#ifdef RULE3_1
+float beta = 0.0f;              // β変数 （制御則3_1で使用
+#endif
 /** config.txt **
 * #から始めるのはコメント行
 * #イコールの前後に空白を入れない
@@ -242,13 +255,17 @@
         if(xbee.readable()) {
             char cmd = xbee.getc();
             if(cmd == 'w') {
-                pull_R += 5.0f;
+                pull_R = 20.0f;
+                pull_L = 0.0f;
             } else if(cmd == 's'){
-                pull_R -= 5.0f;
+                pull_R = 10.0f;
+                pull_L = 10.0f;
             } else if(cmd == 'a'){
-                pull_L += 5.0f;
+                pull_R = 0.0f;
+                pull_L = 20.0f;
             } else if(cmd == 'd'){
-                pull_L -= 5.0f;
+                pull_R = 10.0f;
+                pull_L = 10.0f;
             } 
         }
 #else
@@ -261,7 +278,7 @@
         
         
         // データをmicroSDに保存し、XBeeでPCへ送信する
-        sprintf(data, "%d.%d, %.1f,%.1f,%.1f, %.3f,%.6f,%.6f, %.3f,%.3f,%.1f, %d, %d,%d\r\n",
+        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), 
@@ -355,7 +372,7 @@
  */
 void ControlRoutine()
 {
-    static int   count = 0;
+    volatile static int   count = 0;
     static float target_lng;
     static float target_lat;
     static float target_X;
@@ -416,6 +433,16 @@
                 
                 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
@@ -787,9 +814,9 @@
  */
 bool Thrown()
 {
-    static int opt_count = 0;
-    static int g_count = 0;
-    static int para_count = 0;
+    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++;
@@ -816,8 +843,8 @@
 void DataUpdate()
 {
     // サーボ用変数（引き量）
-    static float pL = pull_L;
-    static float pR = pull_R;
+    static float pL = 0.0f;
+    static float pR = 0.0f;
     
     // 引き量が目標値から一定以上離れていれば、引き量を一定の速度で更新する
     if(pull_L - pL > PullRate * dt) {