Diff: userTask.cpp

Revision:

24:c5945aaae777

Parent:

23:79e20be4bc5b

Child:

25:f3a6e7eec9c3

--- a/userTask.cpp	Sat Dec 08 12:08:25 2018 +0000
+++ b/userTask.cpp	Mon Dec 10 12:29:37 2018 +0000
@@ -8,6 +8,7 @@
 #include "HbManager.h"
 #include "hbCommand.h"
 #include "uart.h"
+#include "fpga.h"
 
 //タスクハンドル（停止とか再開に必要）
 static xTaskHandle  tskHandle[3]={NULL,};
@@ -43,12 +44,12 @@
         commandParse();
         
         //
-        if(gf_Armed){
-            setState(WAKEUP);
-            gf_Armed = false;
-            // taskStop(0);//制御タスクを止める
-            // taskStart(2);//デバッグタスク再開
-        }
+//        if(gf_Armed){
+//            setState(WAKEUP);
+//            gf_Armed = false;
+//            // taskStop(0);//制御タスクを止める
+//            // taskStart(2);//デバッグタスク再開
+//        }
         // else{
         //     taskStop(2);//デバッグタスクを止める
         //     taskStart(0);//制御タスク再開
@@ -74,6 +75,18 @@
         led1=!led1;
         bDoCtrlAtt = false;
         bDoCtrlMot = false;
+        
+        // PID係数アップデートチェック
+        if(gf_PidParaUpdate){
+            hb.setAttPara(g_PidPara);
+            sp.printf("Pp  : [%f]\r\n",g_PidPara.PP);
+            sp.printf("P   : [%f]\r\n",g_PidPara.P);
+            sp.printf("I   : [%f]\r\n",g_PidPara.I);
+            sp.printf("D   : [%f]\r\n",g_PidPara.D);
+            sp.printf("IMAX: [%f]\r\n",g_PidPara.IMax);
+            sp.printf("IMIN: [%f]\r\n",g_PidPara.IMin);
+            gf_PidParaUpdate = false;
+        }
 
         //▼①状態読み出し系
         hb.getAttitude();     //現在角度読み出し
@@ -84,19 +97,21 @@
         switch(gf_State){
             case SLEEP:
                 if(gf_Dbg){
+                    gf_Dbg = false;
                     taskStart(2);
                 }
                 else if(gf_Armed){
+                    gf_Armed = false;
+//                    hb.initChkMotor();
                     setState(WAKEUP);
-                    gf_Armed = false;
                 }
             break;
             case WAKEUP:
-                { //各種モーター
-                  //機材のチェック
-                  //今はそのまま次へ
+                //各種モーター
+                //機材のチェック
+//                if(hb.chkMotor() == true){
                     setState(STANDBY);
-                }
+//                }
             break;
             case STANDBY:
                 // モーター回転数のチェック
@@ -194,12 +209,22 @@
 void taskDebug(void *pvParameters){
 
     taskStop(2);//自ら止めてレジュームされるまで待つ
-    
+    UCHAR Num;
+    INT16 iVal;
+    bool rvFlg;
+
     //この関数をリターンしてしまうと他のタスクも止まるのでwhileで回すこと！
     while(1){
-
-
-
+        for(Num = 0; Num < 8; ++Num){
+            for(iVal = 50; iVal < 300; ++iVal){
+                fpgaMotor(Num, iVal);
+                wait(0.01);
+            }
+            for(iVal = 300; iVal >= 50; --iVal){
+                fpgaMotor(Num, iVal);
+                wait(0.01);
+            }
+        }
         //自タスク停止させて次の周期まで待つ
         gf_Dbg = false;
         sp.printf("Do DebugTask!");
@@ -218,7 +243,7 @@
     if(TaskRtn==pdTRUE){sp.printf("Task0 Set : Command parser\r\n");}
 
     //タスク１：制御タスク
-    TaskRtn= xTaskCreate(taskHbControl, (signed portCHAR *)"TaskHbCont", 192, NULL, 2, &tskHandle[1]);
+    TaskRtn= xTaskCreate(taskHbControl, (signed portCHAR *)"TaskHbCont", 256, NULL, 2, &tskHandle[1]);
     if(TaskRtn==pdTRUE){sp.printf("Task1 Set : Hoverbike Control\r\n");}
         
     //タスク２：デバッグタスク