Revision 0:fbc5f6dc768b, committed 2015-06-19

Comitter:

wasaco

Date:

Fri Jun 19 06:58:12 2015 +0000

Commit message:

???1??????

Changed in this revision

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/control_idea1.cpp	Fri Jun 19 06:58:12 2015 +0000
@@ -0,0 +1,60 @@
+#define target_lng 135
+#define target_lat 35
+
+float theta;
+float alpha = 30; //閾値　定数のほうがいいの？
+
+float targetX;
+float targetY;
+
+
+int step = 0;
+
+
+/*--- 制御部分 ---*/
+
+while(1) {
+    switch(step){
+    
+    // 投下検出
+    case '0':
+        if(/*投下検出*/){
+
+            /*滑空安定まで数秒待機*/
+
+            step = 1;
+            break;
+        }
+
+
+    // 制御ルーチン
+    case '1':
+        /* いずれも地球を完全球体と仮定 */
+
+        /* 北から東回りで目標方向の角度を出力 */
+        targetY = cos( target_lat ) * sin( target_lng - gms.longitude );
+        targetX = cos( gms.latitude ) * sin( target_lat ) - sin( gms.latitude ) * cos( target_lat ) * cos( target_lng - gms.longitude );
+        theta = atan2f( targetY, targetX ); 
+
+        /* 南から東回りーπ〜＋πのθを算出できるように基底変換
+        targetX = - ( target_lat - gms.latitude ) * DEG_TO_RAD;
+        targetY =  ( target_lng - gms.longitude ) * DEG_TO_RAD;
+        
+        theta = atan2f( targetY, targetX ) + 3.14159265 // 北から西回り0〜2πの角度を算出
+        */
+
+        if(yaw < theta - alpha) { //目標は左方向
+
+            /*左ひっぱる*/
+
+        } else if (yaw > theta - alpha) { //目標は右方向
+
+            /*右ひっぱる*/
+
+        }
+
+
+    }
+
+
+}
\ No newline at end of file