Diff: userTask.cpp

Revision:

39:1b76f7df8804

Parent:

38:24ee50452755

Child:

40:debe99e228d3

--- a/userTask.cpp	Fri Jan 18 11:52:00 2019 +0000
+++ b/userTask.cpp	Sat Jan 19 12:35:23 2019 +0000
@@ -81,6 +81,7 @@
     float TrtlRow;
     while(1){
         led1=!led1;
+        hb.proofOfSurvival();
         bDoCtrlAtt = false;
         bDoCtrlMot = false;
         bDoCtrlEng = false;
@@ -116,7 +117,7 @@
 
         //▼①状態読み出し系
         hb.getAttitude();     //現在角度読み出し
-        hb.controlEngine();   //エンジン回転数読み出し
+        hb.getEngine();   //エンジン回転数読み出し
         hb.getUserCommand();  //操作ボタン状態読み出し
        
         //▼②ステート遷移
@@ -133,12 +134,17 @@
                 }
                 if(hb.chkSWUserOpeAny()){
                     typUserSw sw = hb.getUserSw();
-                    sp.printf("%d%d%d%d%d%d%d%d%d%d\r\n",sw.bf.brk_r,sw.bf.brk_l,sw.bf.flt_on,sw.bf.flt_off,
-                                    sw.bf.f_eng_up,sw.bf.f_eng_down,sw.bf.r_eng_up,sw.bf.r_eng_down,
-                                    sw.bf.rsv_1, sw.bf.all_stop);
+                    sp.printf("%d%d%d%d%d%d%d%d%d%d\r\n",sw.bf.brk_l,sw.bf.flt_off,sw.bf.r_eng_down,sw.bf.r_eng_up,sw.bf.rsv_1
+                                    ,sw.bf.brk_r,sw.bf.flt_on,sw.bf.f_eng_down,sw.bf.f_eng_up, sw.bf.all_stop);
                 } else {
                     AxlRow = hb.getUserMotAxlRaw();
                     //TrtlRow = hb.getUserEngTrottleRaw();
+                    if(gf_Prt1.d1.bf.ain){
+
+                    }
+                    if(gf_Prt1.d2.bf.rsv_1){
+
+                    }
                     if(gf_Print.bf.ain){
                         sp.printf("MotAxl:%f\r\n",AxlRow);
                     }
@@ -194,11 +200,11 @@
                 if(gf_StateEnt){
                     // モーター回転数のチェック→モーターの回転数をオフセット値まで上げる
                     // TODO
+                    //gf_MtReqOfs[i]
                     sp.printf("STANDBY state\r\n");
                     gf_StateEnt = false;
                 }
                 else {
-
                     if(hb.chkInRangeIDLE()){
                         hb.calAtt();//現在値でヨー角校正
                         setState(IDLE);
@@ -222,7 +228,7 @@
                 hb.setMotVal(R_R,AxlRpm);
                 bDoCtrlEng = true;
                 bDoCtrlMot = true;
-                //bDoCtrlAtt = true;
+                bDoCtrlAtt = true;
             break;
             case TAKE_OFF:
                 if(gf_StateEnt){
@@ -231,7 +237,7 @@
                 }
                 // エンジン回転数のチェック
                 // 着陸ボタンが優先度が高いので先に判定、たが、まだ作ってないのでコメントアウト
-                // if(hb.chkSWUserOpe(HbUserOpe::STOP)){
+                // if(hb.chkSWUserOpe(HbUserOpe::FLT_OFF)){
                 //     setState(GROUND);
                 // }
                 //else
@@ -239,6 +245,9 @@
                 AxlRpm = hb.getUserMotAxl();
                 hb.setMotVal(R_L,AxlRpm);
                 hb.setMotVal(R_R,AxlRpm);
+                // そのままスルー
+                // TODO エンジン回転数が一定以下になることを確認する
+                setStateF(HOVER);
                 bDoCtrlEng = true;
                 bDoCtrlMot = true;
                 bDoCtrlAtt = true;
@@ -251,16 +260,46 @@
                 // そのままスルー
                 // TODO エンジン回転数が一定以下になることを確認する
                 setState(IDLE);
+                bDoCtrlEng = true;
+                bDoCtrlMot = true;
+                bDoCtrlAtt = true;
             break;
             case HOVER:
                 gf_StateEnt = false;
+                hb.chkSW(HOVER);
+                AxlRpm = hb.getUserMotAxl();
+                hb.setMotVal(R_L,AxlRpm);
+                hb.setMotVal(R_R,AxlRpm);
+                // 動いているかどうかは、別の要素で判断する必要がある
+                if(AxlRpm > 1500){
+                    setStateF(DRIVE);
+                }
+                bDoCtrlEng = true;
+                bDoCtrlMot = true;
+                bDoCtrlAtt = true;
                 // 
             break;
             case DRIVE:
                 gf_StateEnt = false;
+                hb.chkSW(DRIVE);
+                AxlRpm = hb.getUserMotAxl();
+                hb.setMotVal(R_L,AxlRpm);
+                hb.setMotVal(R_R,AxlRpm);
+                // 動いているかどうかは、別の要素で判断する必要がある
+                if(AxlRpm <= 1500){
+                    setStateF(HOVER);
+                }
+                bDoCtrlEng = true;
+                bDoCtrlMot = true;
+                bDoCtrlAtt = true;
             break;
             case EMGGND:
                 gf_StateEnt = false;
+                hb.chkSW(EMGGND);
+                gf_StopMot = true;
+                bDoCtrlEng = true;
+                bDoCtrlMot = true;
+                bDoCtrlAtt = true;
             break;
             case CHK_EG_ENT:
                 if(gf_StateEnt){
@@ -268,10 +307,10 @@
                     gf_StateEnt = false;
                 }
                 if(!hb.chkSWUserOpeAny()){
-                    AxlRpm = hb.getUserMotAxl();
+                    //AxlRpm = hb.getUserMotAxl();
                     TrtlVal = hb.getUserEngTrottle();
                     //if(AxlRpm > 0 || TrtlVal > 40){
-                    if(AxlRpm > 0 || TrtlVal > 400){//暫定的に400
+                    if(TrtlVal > 10){
                         sp.printf("Throttle Val:%d\r\n",TrtlVal);
                         break;
                     } else {
@@ -296,6 +335,7 @@
                         setState(CHK_EG_MID);
                     }
                 }
+                bDoCtrlEng = true;
             break;
             case CHK_EG_MID:
                 if(gf_StateEnt){
@@ -303,9 +343,10 @@
                     gf_StateEnt = false;
                 }
                 if(!hb.chkSWUserOpeAny()){
-                    AxlRpm = hb.getUserMotAxl();
+                    //AxlRpm = hb.getUserMotAxl();
                     TrtlVal = hb.getUserEngTrottle();
-                    if(AxlRpm > 0 || TrtlVal > 400){
+                    //if(AxlRpm > 0 || TrtlVal > 40){
+                    if(TrtlVal > 10){
                         break;
                     } else {
                         setState(CHK_EG_R);
@@ -328,6 +369,7 @@
                         setState(CHK_EG_EXIT);
                     }
                 }
+                bDoCtrlEng = true;
             break;
             case CHK_EG_EXIT:
                 if(gf_StateEnt){
@@ -335,9 +377,8 @@
                     gf_StateEnt = false;
                 }
                 if(!hb.chkSWUserOpeAny()){
-                    AxlRpm = hb.getUserMotAxl();
                     TrtlVal = hb.getUserEngTrottle();
-                    if(AxlRpm > 0 || TrtlVal > 400){
+                    if(TrtlVal > 10){
                         break;
                     } else {
                         setState(SLEEP);
@@ -386,6 +427,7 @@
                 }
                 bDoCtrlAtt = false;
                 bDoCtrlMot = true;
+                bDoCtrlEng = true;
                 bTmp = true;
                 for(int i = 0; i < 4; ++i){
                     if(gf_MtReq[i].req){
@@ -406,7 +448,8 @@
                 }
                 if(bTmp){
                     if(gf_FromActiveStat){
-                        setStateF(IDLE);
+                        //setStateF(IDLE);
+                        setStateF(STANDBY);
                     }
                     else{
                         setStateF(SLEEP);
@@ -430,7 +473,7 @@
         //hb.controlMotor();//モーター指令出し
         if(bDoCtrlAtt)hb.controlAttitude(); //姿勢制御
         if(bDoCtrlMot)hb.controlMotor();//モーター指令出し
-        //if(bDoCtrlEng)hb.controlEngine();//エンジン指令出し
+        if(bDoCtrlEng)hb.controlEngine();//エンジン指令出し
 
         if(gf_Print.bf.stat){
             sp.printf("stat : [%X]\r\n",gf_State);