Diff: main.cpp

Revision:

31:2f88240999fe

Parent:

30:fb310564097b

Child:

32:b35efe3d1091

--- a/main.cpp	Sun Jul 05 00:13:06 2015 +0000
+++ b/main.cpp	Sun Jul 12 04:30:47 2015 +0000
@@ -16,25 +16,25 @@
 //#define RULE2
 //#define RULE3
 //#define RULE4
-//#define SERVO_DEBUG
-#define DIRECTION_DEBUG
+#define SERVO_DEBUG
+//#define DIRECTION_DEBUG
 //#define SPIRAL_DEBUG
 
 #ifdef DIRECTION_DEBUG
-const float TargetDirection = -90.0f;        // 真東に飛ぶ
+const float TargetDirection = 0.0f;        // 指定方向に飛ぶ
 #endif
 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 Turn            = 25;           // 旋回時の引っ張り量
 const float PullMax         = 25;           // 引っ張れる紐の最大量(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        = 1;            // 投下後、安定するまで何秒滑空するか
+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の定数
@@ -241,13 +241,13 @@
         if(xbee.readable()) {
             char cmd = xbee.getc();
             if(cmd == 'w') {
-                pull_R += 5;
+                pull_R = Turn;
             } else if(cmd == 's'){
-                pull_R -= 5;
+                pull_R = 0;
             } else if(cmd == 'a'){
-                pull_L += 5;
+                pull_L = Turn;
             } else if(cmd == 'd'){
-                pull_L -= 5;
+                pull_L = 0;
             } 
         }
 #else
@@ -393,13 +393,14 @@
         case 1:
             if(fabs(roll) > BorderSpiral) {
                 // スパイラル回避行動
-                if(roll > 0) {
+                /*if(roll > 0) {
                     pull_L = PullMax;
                     pull_R = 0;
                 } else {
                     pull_L = 0;
                     pull_R = PullMax;
-                }
+                }*/
+                
             } else {
 
                 /* いずれも地球を完全球体と仮定 */
@@ -525,7 +526,7 @@
     b_f.SetComp(1, 0.0f);
     b_f.SetComp(2, 0.0f);
     b_f.SetComp(3, -1.0f);
-    b_u.SetComp(1, -1.0f);
+    b_u.SetComp(1, 1.0f);
     b_u.SetComp(2, 0.0f);
     b_u.SetComp(3, 0.0f);
     b_l = Cross(b_u, b_f);
@@ -797,10 +798,10 @@
     static int para_count = 0;
 
 #ifndef SPIRAL_DEBUG
-    if(optSensor.read_u16() > BorderOpt) opt_count++;
+    /*if(optSensor.read_u16() > BorderOpt) opt_count++;
     else opt_count = 0;
     if(vrt_acc < BorderGravity) g_count++;
-    else g_count = 0;
+    else g_count = 0;*/
 #endif
     if((int)paraSensor == BorderParafoil) para_count++;
     else para_count = 0;