Diff: userTask.cpp

Revision:

40:debe99e228d3

Parent:

39:1b76f7df8804

Child:

41:45c982b1c5b6

--- a/userTask.cpp	Sat Jan 19 12:35:23 2019 +0000
+++ b/userTask.cpp	Mon Jan 21 06:32:24 2019 +0000
@@ -137,15 +137,9 @@
                     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){
+                        AxlRow = hb.getUserMotAxlRaw();
                         sp.printf("MotAxl:%f\r\n",AxlRow);
                     }
                 }
@@ -170,6 +164,8 @@
                 else {
                     cntSW2 = 0;
                 }
+                // Debug用
+                bDoCtrlMot = true;
             break;
             case WAKEUP:
                 if(gf_StateEnt){
@@ -200,11 +196,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);
@@ -228,7 +224,7 @@
                 hb.setMotVal(R_R,AxlRpm);
                 bDoCtrlEng = true;
                 bDoCtrlMot = true;
-                bDoCtrlAtt = true;
+                //bDoCtrlAtt = true;
             break;
             case TAKE_OFF:
                 if(gf_StateEnt){
@@ -237,7 +233,7 @@
                 }
                 // エンジン回転数のチェック
                 // 着陸ボタンが優先度が高いので先に判定、たが、まだ作ってないのでコメントアウト
-                // if(hb.chkSWUserOpe(HbUserOpe::FLT_OFF)){
+                // if(hb.chkSWUserOpe(HbUserOpe::STOP)){
                 //     setState(GROUND);
                 // }
                 //else
@@ -245,9 +241,6 @@
                 AxlRpm = hb.getUserMotAxl();
                 hb.setMotVal(R_L,AxlRpm);
                 hb.setMotVal(R_R,AxlRpm);
-                // そのままスルー
-                // TODO エンジン回転数が一定以下になることを確認する
-                setStateF(HOVER);
                 bDoCtrlEng = true;
                 bDoCtrlMot = true;
                 bDoCtrlAtt = true;
@@ -260,46 +253,16 @@
                 // そのままスルー
                 // 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){
@@ -427,7 +390,6 @@
                 }
                 bDoCtrlAtt = false;
                 bDoCtrlMot = true;
-                bDoCtrlEng = true;
                 bTmp = true;
                 for(int i = 0; i < 4; ++i){
                     if(gf_MtReq[i].req){
@@ -448,8 +410,7 @@
                 }
                 if(bTmp){
                     if(gf_FromActiveStat){
-                        //setStateF(IDLE);
-                        setStateF(STANDBY);
+                        setStateF(IDLE);
                     }
                     else{
                         setStateF(SLEEP);