Diff: userTask.cpp

Revision:

36:2cc739c7e4cb

Parent:

35:3779201b4c73

Child:

37:d51dacb4c30f

--- a/userTask.cpp	Tue Dec 25 08:20:17 2018 +0000
+++ b/userTask.cpp	Wed Jan 16 10:51:07 2019 +0000
@@ -72,7 +72,10 @@
     bool bDoCtrlMot;
     bool bDoCtrlEng;
     //INT16 tmpRpm, difRpm;
+    INT16 cntSW1 = 0;
+    INT16 cntSW2 = 0;
     INT16 AxlRpm;
+    INT16 TrtlVal;
     while(1){
         led1=!led1;
         bDoCtrlAtt = false;
@@ -90,16 +93,16 @@
             sp.printf("V   : [%f]\r\n",g_PidPara.V);
             gf_PidParaUpdate = false;
         }
-        for(int i = 0; i < 4; ++i){
-            if(gf_MotParaUpdate[i]){
-//                hb.setMotPara((UCHAR)i, g_MotPara[i]);
-//                sp.printf("num   : [%d]\r\n",i);
-//                //sp.printf("ofs  : [%d]\r\n",g_MotPara[i].offset);
-//                sp.printf("low  : [%d]\r\n",g_MotPara[i].limit_low);
-//                sp.printf("hi   : [%d]\r\n",g_MotPara[i].limit_hi);
-                gf_MotParaUpdate[i] = false;
-            }
-        }
+        // for(int i = 0; i < 4; ++i){
+        //     if(gf_MotParaUpdate[i]){
+        //        hb.setMotPara((UCHAR)i, g_MotPara[i]);
+        //        sp.printf("num   : [%d]\r\n",i);
+        //        //sp.printf("ofs  : [%d]\r\n",g_MotPara[i].offset);
+        //        sp.printf("low  : [%d]\r\n",g_MotPara[i].limit_low);
+        //        sp.printf("hi   : [%d]\r\n",g_MotPara[i].limit_hi);
+        //         gf_MotParaUpdate[i] = false;
+        //     }
+        // }
         if(gf_StopMot){
             //setStateF(MOT_STOP);
             //hb.getCurMotVal();
@@ -121,43 +124,87 @@
         //▼②ステート遷移
         switch(gf_State){
             case SLEEP:
+                if(gf_StateEnt){
+                    sp.printf("SLEEP state\r\n");
+                    gf_StateEnt = false;
+                }
+                // DEBUGタスクスタートフラグの監視
                 if(gf_Dbg){
                     gf_Dbg = false;
                     taskStart(2);
                 }
+                // 浮上（離陸）ボタン、着陸ボタンの同時長押し監視
+                if(hb.chkSWUserOpeBoth(HbUserOpe::FLT_ON,HbUserOpe::FLT_OFF)){
+                    ++cntSW1;
+                    if(cntSW1 > 30){
+                        cntSW1 = 0;
+                        setStateF(CHK_EG_ENT);
+                    }
+                }
+                else {
+                    cntSW1 = 0;
+                }
+                if(hb.chkSWUserOpeBoth(HbUserOpe::BRK_L, HbUserOpe::BRK_R)){
+                    ++cntSW2;
+                    if(cntSW2 > 30){
+                        cntSW2 = 0;
+                        setStateF(WAKEUP);
+                    }
+                }
+                else {
+                    cntSW2 = 0;
+                }
             break;
             case WAKEUP:
-                //各種モーター
-                //機材のチェック
-                hb.calAtt();
-                AxlRpm = hb.getUserMotAxl();
-                if(AxlRpm > 0){
-                    // 将来的に警告音等
-                    sp.printf("Warning!! Motor Accel Opened!!\r\n");
-                    setStateF(SLEEP);
-                } else {
-                    setState(STANDBY);
+                if(gf_StateEnt){
+                    //スイッチが何も押されていないことの確認
+                    if(!hb.chkSWUserOpeAny()){
+                        sp.printf("WAKEUP state\r\n");
+                        gf_StateEnt = false;
+                    }
+                }
+                else
+                {
+                    //各種モーター
+                    //機材のチェック
+                    hb.calAtt();//現在値でヨー角校正
+                    //モーター用のアナログ入力が下げられているかの確認
+                    AxlRpm = hb.getUserMotAxl();
+                    if(AxlRpm > 0){
+                        // 将来的に警告音等
+                        sp.printf("Warning!! Motor Accel Opened!!\r\n");
+                        setStateF(SLEEP);
+                    } else {
+                        setState(STANDBY);
+                    }
                 }
             break;
             case STANDBY:
-                // モーター回転数のチェック→モーターの回転数をオフセット値まで上げる
-                //nxtStatFlg = true;
-                //for(int i = 0; i < 4; ++i){
-                //    if(gf_MtReq[i].bf.req){
-                //        bDoCtrlMot = true;
-                //    }
-                //}
-                if(!bDoCtrlMot){
+                bDoCtrlMot = true;
+                if(gf_StateEnt){
+                    // モーター回転数のチェック→モーターの回転数をオフセット値まで上げる
+                    // TODO
+                    sp.printf("STANDBY state\r\n");
+                    gf_StateEnt = false;
+                }
+                else {
+
                     if(hb.chkInRangeIDLE()){
+                        hb.calAtt();//現在値でヨー角校正
                         setState(IDLE);
                     }
                     else{
                         // デバッグのためここもスルー
+                        hb.calAtt();//現在値でヨー角校正
                         setState(IDLE);
                     }
                 }
             break;
             case IDLE:
+                if(gf_StateEnt){
+                    sp.printf("IDLE state\r\n");
+                    gf_StateEnt = false;
+                }
                 //SWのチェック
                 hb.chkSW(IDLE);
                 AxlRpm = hb.getUserMotAxl();
@@ -168,6 +215,10 @@
                 //bDoCtrlAtt = true;
             break;
             case TAKE_OFF:
+                if(gf_StateEnt){
+                    sp.printf("TAKE_OFF state\r\n");
+                    gf_StateEnt = false;
+                }
                 // エンジン回転数のチェック
                 // 着陸ボタンが優先度が高いので先に判定、たが、まだ作ってないのでコメントアウト
                 // if(hb.chkSWUserOpe(HbUserOpe::STOP)){
@@ -181,40 +232,140 @@
                 bDoCtrlEng = true;
                 bDoCtrlMot = true;
                 bDoCtrlAtt = true;
-                //if(hb.chkSWUserOpe(HbUserOpe::STOP)){
-                //    setState(HOVER);
-                //}
-                
             break;
             case GROUND:
+                if(gf_StateEnt){
+                    sp.printf("GROUND state\r\n");
+                    gf_StateEnt = false;
+                }
                 // そのままスルー
+                // TODO エンジン回転数が一定以下になることを確認する
                 setState(IDLE);
             break;
             case HOVER:
+                gf_StateEnt = false;
                 // 
             break;
             case DRIVE:
+                gf_StateEnt = false;
             break;
             case EMGGND:
+                gf_StateEnt = false;
+            break;
+            case CHK_EG_ENT:
+                if(gf_StateEnt){
+                    sp.printf("Check Engine enter\r\n");
+                    gf_StateEnt = false;
+                }
+                if(!hb.chkSWUserOpeAny()){
+                    AxlRpm = hb.getUserMotAxl();
+                    TrtlVal = hb.getUserEngTrottle();
+                    if(AxlRpm > 0 || TrtlVal > 40){
+                        break;
+                    } else {
+                        setState(CHK_EG_F);
+                    }
+                }
+            break;
+            case CHK_EG_F:
+                if(gf_StateEnt){
+                    sp.printf("Check Engine Front\r\n");
+                    gf_StateEnt = false;
+                }
+                TrtlVal = hb.getUserEngTrottle();
+                hb.setAccelVal(FRONT, TrtlVal);
+                if(hb.chkSWUserOpe(HbUserOpe::F_ENG_UP)){
+                    hb.setHvAxl(FRONT, TrtlVal);
+                }
+                if(hb.chkSWUserOpe(HbUserOpe::R_ENG_UP)){
+                    if(hb.chkSetHvAxl(FRONT)){
+                        setState(CHK_EG_MID);
+                    }
+                }
+            break;
+            case CHK_EG_MID:
+                if(gf_StateEnt){
+                    sp.printf("Check Engine MIDDLE\r\n");
+                    gf_StateEnt = false;
+                }
+                if(!hb.chkSWUserOpeAny()){
+                    AxlRpm = hb.getUserMotAxl();
+                    TrtlVal = hb.getUserEngTrottle();
+                    if(AxlRpm > 0 || TrtlVal > 40){
+                        break;
+                    } else {
+                        setState(CHK_EG_R);
+                    }
+                }
+            break;
+            case CHK_EG_R:
+                if(gf_StateEnt){
+                    sp.printf("Check Engine Rear\r\n");
+                    gf_StateEnt = false;
+                }
+                TrtlVal = hb.getUserEngTrottle();
+                hb.setAccelVal(REAR, TrtlVal);
+                if(hb.chkSWUserOpe(HbUserOpe::F_ENG_UP)){
+                    hb.setHvAxl(REAR, TrtlVal);
+                }
+                if(hb.chkSWUserOpe(HbUserOpe::R_ENG_UP)){
+                    if(hb.chkSetHvAxl(REAR)){
+                        setState(CHK_EG_EXIT);
+                    }
+                }
+            break;
+            case CHK_EG_EXIT:
+                if(gf_StateEnt){
+                    sp.printf("Check Engine Exit\r\n");
+                    gf_StateEnt = false;
+                }
+                if(!hb.chkSWUserOpeAny()){
+                    AxlRpm = hb.getUserMotAxl();
+                    TrtlVal = hb.getUserEngTrottle();
+                    if(AxlRpm > 0 || TrtlVal > 40){
+                        break;
+                    } else {
+                        setState(SLEEP);
+                    }
+                }
             break;
             case CHK_ENT:    //チェックエンター
-                sp.printf("Check enter\r\n");
+                if(gf_StateEnt){
+                    sp.printf("Check enter\r\n");
+                    gf_StateEnt = false;
+                }
             break;
             case CHK_MOT:    //モーターチェック
-                sp.printf("Check Motor\r\n");
+                if(gf_StateEnt){
+                    sp.printf("Check Motor\r\n");
+                    gf_StateEnt = false;
+                }
                 bDoCtrlMot = true;
             break;
             case CHK_AXL:    //アクセルサーボチェック
-                sp.printf("Check Accel Servo\r\n");
+                if(gf_StateEnt){
+                    sp.printf("Check Accel Servo\r\n");
+                    gf_StateEnt = false;
+                }
             break;
             case CHK_ATT:    //姿勢制御チェック
-                sp.printf("Check Attitude\r\n");
+                if(gf_StateEnt){
+                    sp.printf("Check Attitude\r\n");
+                    gf_StateEnt = false;
+                }
                 bDoCtrlAtt = true;
             break;
             case CHK_EXIT:   //チェックステート脱出
-                sp.printf("Check Exit\r\n");
+                if(gf_StateEnt){
+                    sp.printf("Check Exit\r\n");
+                    gf_StateEnt = false;
+                }
             break;
             case MOT_STOP:
+                if(gf_StateEnt){
+                    sp.printf("Motor Stop\r\n");
+                    gf_StateEnt = false;
+                }
                 // if(hb.stopMotor() == true){
                 //     hb.initMotVal();
                 //     setState(SLEEP);