Diff: userTask.cpp

Revision:

23:79e20be4bc5b

Parent:

22:24c9c2dedca9

Child:

24:c5945aaae777

--- a/userTask.cpp	Thu Dec 06 11:03:13 2018 +0000
+++ b/userTask.cpp	Sat Dec 08 12:08:25 2018 +0000
@@ -67,9 +67,14 @@
     
     HbManager       hb;
     
+    bool bDoCtrlAtt;
+    bool bDoCtrlMot;
     //
     while(1){
         led1=!led1;
+        bDoCtrlAtt = false;
+        bDoCtrlMot = false;
+
         //▼①状態読み出し系
         hb.getAttitude();     //現在角度読み出し
         hb.controlEngine();   //エンジン回転数読み出し
@@ -81,30 +86,98 @@
                 if(gf_Dbg){
                     taskStart(2);
                 }
+                else if(gf_Armed){
+                    setState(WAKEUP);
+                    gf_Armed = false;
+                }
             break;
             case WAKEUP:
+                { //各種モーター
+                  //機材のチェック
+                  //今はそのまま次へ
+                    setState(STANDBY);
+                }
             break;
             case STANDBY:
+                // モーター回転数のチェック
+                if(hb.chkInRangeIDLE()){
+                    setState(IDLE);
+                }
+                else{
+                    // デバッグのためここもスルー
+                    setState(IDLE);
+                }
             break;
             case IDLE:
+                //SWのチェック
+                //離陸ボタン押下
+                //とりあえず、YawCtrlで代用
+                if(hb.chkSWUserOpe(HbUserOpe::YAW_CTRL))
+                {
+                    // TODO IMU Cal
+                    setState(TAKE_OFF);
+                }
+                //else if(hb.chkSWUserOpe(HbUserOpe::ENG_STOP))
+                //{
+                //   setState(SLEEP);
+                //}
+                //if(hb.chkSWUserOpeAny()){
+                //    sp.printf("SW :[%X]\r\n",hb.getUserSw());
+                //}
             break;
             case TAKE_OFF:
+                // エンジン回転数のチェック
+                // 着陸ボタンが優先度が高いので先に判定、たが、まだ作ってないのでコメントアウト
+                // if(hb.chkSWUserOpe(HbUserOpe::STOP)){
+                //     setState(GROUND);
+                // }
+                //else
+                bDoCtrlMot = true;
+                bDoCtrlAtt = true;
+                //if(hb.chkSWUserOpe(HbUserOpe::STOP)){
+                //    setState(HOVER);
+                //}
+                
             break;
             case GROUND:
+                // そのままスルー
+                setState(IDLE);
             break;
             case HOVER:
+                // 
             break;
             case DRIVE:
             break;
             case EMGGND:
             break;
+            case CHK_ENT:    //チェックエンター
+                sp.printf("Check enter\r\n");
+            break;
+            case CHK_MOT:    //モーターチェック
+                sp.printf("Check Motor\r\n");
+                bDoCtrlMot = true;
+            break;
+            case CHK_AXL:    //アクセルサーボチェック
+                sp.printf("Check Accel Servo\r\n");
+            break;
+            case CHK_ATT:    //姿勢制御チェック
+                sp.printf("Check Attitude\r\n");
+                bDoCtrlAtt = true;
+            break;
+            case CHK_EXIT:   //チェックステート脱出
+                sp.printf("Check Exit\r\n");
+            break;
         }
         
         //▼③各種設定
         //hb.controlAttitude(); //姿勢制御
         //hb.controlMotor();//モーター指令出し
+        if(bDoCtrlAtt)hb.controlAttitude(); //姿勢制御
+        if(bDoCtrlMot)hb.controlMotor();//モーター指令出し
         
-        
+        if(gf_Print.bf.stat){
+            sp.printf("stat : [%X]\r\n",gf_State);
+        }
         //表示フラグを落とす（けどモニタフラグが立ってる箇所は残る）
         if(gf_Print.flg!=0){
             gf_Print.flg=gf_Mon.flg;
@@ -127,8 +200,9 @@
 
 
 
+        //自タスク停止させて次の周期まで待つ
         gf_Dbg = false;
-        //自タスク停止させて次の周期まで待つ
+        sp.printf("Do DebugTask!");
         taskStop(2);
     }
 }
@@ -140,7 +214,7 @@
     portBASE_TYPE   TaskRtn;//タスククリエイトの結果を受け取る型
     
     //タスク０：コマンド解析タスク
-    TaskRtn= xTaskCreate(taskCmdParser, (signed portCHAR *)"TaskCmd", 32, NULL, 1, &tskHandle[0]);
+    TaskRtn= xTaskCreate(taskCmdParser, (signed portCHAR *)"TaskCmd", 256, NULL, 1, &tskHandle[0]);
     if(TaskRtn==pdTRUE){sp.printf("Task0 Set : Command parser\r\n");}
 
     //タスク１：制御タスク
@@ -148,7 +222,7 @@
     if(TaskRtn==pdTRUE){sp.printf("Task1 Set : Hoverbike Control\r\n");}
         
     //タスク２：デバッグタスク
-    TaskRtn= xTaskCreate(taskDebug, (signed portCHAR *)"TaskDebug", 64, NULL, 0, &tskHandle[2]);
+    TaskRtn= xTaskCreate(taskDebug, (signed portCHAR *)"TaskDebug", 192, NULL, 0, &tskHandle[2]);
     if(TaskRtn==pdTRUE){sp.printf("Task2 Set : Debug\r\n");}
     
     //RTOSカーネル起動（タスクが走り出す）