LAURUS / control_idea1

途中ですみませんすみません

control_idea1.cpp@0:fbc5f6dc768b, 2015-06-19 (annotated)

Committer:
wasaco
Date:
Fri Jun 19 06:58:12 2015 +0000
Revision:
0:fbc5f6dc768b
???1??????

Who changed what in which revision?

UserRevisionLine numberNew contents of line
wasaco 0:fbc5f6dc768b 1 #define target_lng 135
wasaco 0:fbc5f6dc768b 2 #define target_lat 35
wasaco 0:fbc5f6dc768b 3
wasaco 0:fbc5f6dc768b 4 float theta;
wasaco 0:fbc5f6dc768b 5 float alpha = 30; //閾値　定数のほうがいいの？
wasaco 0:fbc5f6dc768b 6
wasaco 0:fbc5f6dc768b 7 float targetX;
wasaco 0:fbc5f6dc768b 8 float targetY;
wasaco 0:fbc5f6dc768b 9
wasaco 0:fbc5f6dc768b 10
wasaco 0:fbc5f6dc768b 11 int step = 0;
wasaco 0:fbc5f6dc768b 12
wasaco 0:fbc5f6dc768b 13
wasaco 0:fbc5f6dc768b 14 /*--- 制御部分 ---*/
wasaco 0:fbc5f6dc768b 15
wasaco 0:fbc5f6dc768b 16 while(1) {
wasaco 0:fbc5f6dc768b 17 switch(step){
wasaco 0:fbc5f6dc768b 18
wasaco 0:fbc5f6dc768b 19 // 投下検出
wasaco 0:fbc5f6dc768b 20 case '0':
wasaco 0:fbc5f6dc768b 21 if(/*投下検出*/){
wasaco 0:fbc5f6dc768b 22
wasaco 0:fbc5f6dc768b 23 /*滑空安定まで数秒待機*/
wasaco 0:fbc5f6dc768b 24
wasaco 0:fbc5f6dc768b 25 step = 1;
wasaco 0:fbc5f6dc768b 26 break;
wasaco 0:fbc5f6dc768b 27 }
wasaco 0:fbc5f6dc768b 28
wasaco 0:fbc5f6dc768b 29
wasaco 0:fbc5f6dc768b 30 // 制御ルーチン
wasaco 0:fbc5f6dc768b 31 case '1':
wasaco 0:fbc5f6dc768b 32 /* いずれも地球を完全球体と仮定 */
wasaco 0:fbc5f6dc768b 33
wasaco 0:fbc5f6dc768b 34 /* 北から東回りで目標方向の角度を出力 */
wasaco 0:fbc5f6dc768b 35 targetY = cos( target_lat ) * sin( target_lng - gms.longitude );
wasaco 0:fbc5f6dc768b 36 targetX = cos( gms.latitude ) * sin( target_lat ) - sin( gms.latitude ) * cos( target_lat ) * cos( target_lng - gms.longitude );
wasaco 0:fbc5f6dc768b 37 theta = atan2f( targetY, targetX );
wasaco 0:fbc5f6dc768b 38
wasaco 0:fbc5f6dc768b 39 /* 南から東回りーπ〜＋πのθを算出できるように基底変換
wasaco 0:fbc5f6dc768b 40 targetX = - ( target_lat - gms.latitude ) * DEG_TO_RAD;
wasaco 0:fbc5f6dc768b 41 targetY = ( target_lng - gms.longitude ) * DEG_TO_RAD;
wasaco 0:fbc5f6dc768b 42
wasaco 0:fbc5f6dc768b 43 theta = atan2f( targetY, targetX ) + 3.14159265 // 北から西回り0〜2πの角度を算出
wasaco 0:fbc5f6dc768b 44 */
wasaco 0:fbc5f6dc768b 45
wasaco 0:fbc5f6dc768b 46 if(yaw < theta - alpha) { //目標は左方向
wasaco 0:fbc5f6dc768b 47
wasaco 0:fbc5f6dc768b 48 /*左ひっぱる*/
wasaco 0:fbc5f6dc768b 49
wasaco 0:fbc5f6dc768b 50 } else if (yaw > theta - alpha) { //目標は右方向
wasaco 0:fbc5f6dc768b 51
wasaco 0:fbc5f6dc768b 52 /*右ひっぱる*/
wasaco 0:fbc5f6dc768b 53
wasaco 0:fbc5f6dc768b 54 }
wasaco 0:fbc5f6dc768b 55
wasaco 0:fbc5f6dc768b 56
wasaco 0:fbc5f6dc768b 57 }
wasaco 0:fbc5f6dc768b 58
wasaco 0:fbc5f6dc768b 59
wasaco 0:fbc5f6dc768b 60 }