Diff: userTask.cpp

Revision:

25:f3a6e7eec9c3

Parent:

24:c5945aaae777

Child:

26:732bc37fbefd

diff -r c5945aaae777 -r f3a6e7eec9c3 userTask.cpp
--- a/userTask.cpp	Mon Dec 10 12:29:37 2018 +0000
+++ b/userTask.cpp	Wed Dec 12 23:52:22 2018 +0000
@@ -87,6 +87,21 @@
             sp.printf("IMIN: [%f]\r\n",g_PidPara.IMin);
             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;
+            }
+        }
+        if(gf_StopMot){
+            setStateF(MOT_STOP);
+            hb.getCurMotVal();
+            gf_StopMot = false;
+        }
 
         //▼①状態読み出し系
         hb.getAttitude();     //現在角度読み出し
@@ -110,17 +125,31 @@
                 //各種モーター
                 //機材のチェック
 //                if(hb.chkMotor() == true){
+                    hb.calAtt();
+                    // STANDBYでモーター回転をオフセット値まで上げるためのフラグセット
+                    //for(int i = 0; i < 4; ++i){
+                    //    gf_MtReq[i].bf.req = true;
+                    //    gf_MtReq[i].bf.val = 200;
+                    //}
                     setState(STANDBY);
 //                }
             break;
             case STANDBY:
-                // モーター回転数のチェック
-                if(hb.chkInRangeIDLE()){
-                    setState(IDLE);
-                }
-                else{
-                    // デバッグのためここもスルー
-                    setState(IDLE);
+                // モーター回転数のチェック→モーターの回転数をオフセット値まで上げる
+                //nxtStatFlg = true;
+                //for(int i = 0; i < 4; ++i){
+                //    if(gf_MtReq[i].bf.req){
+                //        bDoCtrlMot = true;
+                //    }
+                //}
+                if(!bDoCtrlMot){
+                    if(hb.chkInRangeIDLE()){
+                        setState(IDLE);
+                    }
+                    else{
+                        // デバッグのためここもスルー
+                        setState(IDLE);
+                    }
                 }
             break;
             case IDLE:
@@ -130,15 +159,17 @@
                 if(hb.chkSWUserOpe(HbUserOpe::YAW_CTRL))
                 {
                     // TODO IMU Cal
+                    hb.calAtt();
                     setState(TAKE_OFF);
                 }
                 //else if(hb.chkSWUserOpe(HbUserOpe::ENG_STOP))
                 //{
                 //   setState(SLEEP);
                 //}
-                //if(hb.chkSWUserOpeAny()){
-                //    sp.printf("SW :[%X]\r\n",hb.getUserSw());
-                //}
+                if(hb.chkSWUserOpeAny()){
+                    sp.printf("SW :[%X]\r\n",hb.getUserSw());
+                }
+                bDoCtrlMot = true;
             break;
             case TAKE_OFF:
                 // エンジン回転数のチェック
@@ -182,6 +213,17 @@
             case CHK_EXIT:   //チェックステート脱出
                 sp.printf("Check Exit\r\n");
             break;
+            case MOT_STOP:
+                if(hb.stopMotor() == true){
+                    hb.initMotVal();
+                    setState(SLEEP);
+                    sp.printf("MOTOR STOPPED!\r\n");
+                }
+                else{
+                    //bDoCtrlMot = true;
+                    sp.printf(".");
+                }
+            break;
         }
         
         //▼③各種設定
@@ -211,18 +253,18 @@
     taskStop(2);//自ら止めてレジュームされるまで待つ
     UCHAR Num;
     INT16 iVal;
-    bool rvFlg;
+    //bool rvFlg;
 
     //この関数をリターンしてしまうと他のタスクも止まるのでwhileで回すこと！
     while(1){
         for(Num = 0; Num < 8; ++Num){
             for(iVal = 50; iVal < 300; ++iVal){
                 fpgaMotor(Num, iVal);
-                wait(0.01);
+                wait(0.002);
             }
             for(iVal = 300; iVal >= 50; --iVal){
                 fpgaMotor(Num, iVal);
-                wait(0.01);
+                wait(0.002);
             }
         }
         //自タスク停止させて次の周期まで待つ
@@ -243,7 +285,7 @@
     if(TaskRtn==pdTRUE){sp.printf("Task0 Set : Command parser\r\n");}
 
     //タスク１：制御タスク
-    TaskRtn= xTaskCreate(taskHbControl, (signed portCHAR *)"TaskHbCont", 256, NULL, 2, &tskHandle[1]);
+    TaskRtn= xTaskCreate(taskHbControl, (signed portCHAR *)"TaskHbCont", 512, NULL, 2, &tskHandle[1]);
     if(TaskRtn==pdTRUE){sp.printf("Task1 Set : Hoverbike Control\r\n");}
         
     //タスク２：デバッグタスク