Revision 30:fb310564097b, committed 2015-07-05

Comitter:

ojan

Date:

Sun Jul 05 00:13:06 2015 +0000

Parent:

29:59f4808e2eb6

Child:

31:2f88240999fe

Commit message:

LAURUS_Program_v2.6.1

Changed in this revision

--- a/main.cpp	Thu Jul 02 16:00:30 2015 +0000
+++ b/main.cpp	Sun Jul 05 00:13:06 2015 +0000
@@ -26,6 +26,8 @@
 const float dt              = 0.01f;        // 割り込み周期(s)
 const float ServoMax        = 0.0046f;      // サーボの最大パルス長(s)
 const float ServoMin        = 0.0012f;      // サーボの最小パルス長(s)
+const float Stabilize       = 0;            // 滑空安定時の引っ張り量
+const float Turn            = 20;           // 旋回時の引っ張り量
 const float PullMax         = 25;           // 引っ張れる紐の最大量(mm)
 const float BorderSpiral    = 40.0f;        // スパイラル検知角度
 const short BorderOpt       = 30000;        // 光センサーの閾値
@@ -89,7 +91,7 @@
 Vector  pre_p(2);               // 過去の位置情報(経度, 緯度)（rad）
 int     UTC_t = 0;              // UTC時刻
 int     pre_UTC_t = 0;          // 前のUTC時刻
-int     ss = 0;              // 時刻の秒数の小数部分
+int     ss = 0;                 // 時刻の秒数の小数部分
 
 Vector  b_f(3);                 // 機体座標に固定された、機体前方向きのベクトル（x軸）
 Vector  b_u(3);                 // 機体座標に固定された、機体上方向きのベクトル（z軸）
@@ -334,7 +336,7 @@
     }
 
     if(yaw > 180.0f) yaw -= 360.0f;                 // ヨー角を[-π, π]に補正
-    else 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;
@@ -373,9 +375,8 @@
         case 0:
             if(Thrown() || count != 0) {
                 count++;
-                // 投下直後に紐を引く場合はコメントアウトをはずす
-                 //pull_L = 15;
-                 //pull_R = 15;
+                    pull_L = Stabilize;
+                    pull_R = Stabilize;
                 if(count >= WaitTime*5) {
                     pull_L = 0;
                     pull_R = 0;
@@ -463,13 +464,13 @@
 
                 if(dir == LEFT) {           //目標は左方向
 
-                    pull_L = 20;
+                    pull_L = Turn;
                     pull_R = 0;
 
                 } else if (dir == RIGHT) {   //目標は右方向
 
                     pull_L = 0;
-                    pull_R = 20;
+                    pull_R = Turn;
 
                 } else if (dir == NEUTRAL) {
                     pull_L = 0;
@@ -795,10 +796,12 @@
     static int g_count = 0;
     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;