Diff: userTask.cpp

Revision:

56:f363a6877c6a

Parent:

53:b09c062cc31c

Child:

63:aee44afe6363

diff -r e245227d9fea -r f363a6877c6a userTask.cpp
--- a/userTask.cpp	Mon Mar 04 08:07:20 2019 +0000
+++ b/userTask.cpp	Wed Mar 06 02:18:47 2019 +0000
@@ -69,17 +69,13 @@
     
     HbManager       hb;
     
-    bool bDoCtrlAtt;
-    bool bDoCtrlMot;
-    bool bDoCtrlEng;
     bool bTmp;
-    //INT16 tmpRpm, difRpm;
+
     INT16 cntSW1 = 0;
     INT16 cntSW2 = 0;
     INT16 AxlRpm;
     INT16 TrtlVal;
     float AxlRow;
-    //float TrtlRow;
 
     
     while(1){
@@ -88,9 +84,7 @@
         setDO4LED(gf_State);
 
         hb.proofOfSurvival();
-        bDoCtrlAtt = false;
-        bDoCtrlMot = false;
-        bDoCtrlEng = false;
+
         // PID係数アップデートチェック
         if(gf_PidParaUpdate){
             hb.setAttPara(g_PidPara);
@@ -141,10 +135,7 @@
                 }
                 if(hb.chkSWUserOpeAny()){
                     typUserSw sw = hb.getUserSw();
-                    //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 {
-                    //TrtlRow = hb.getUserEngTrottleRaw();
                     if(gf_Print.d2.bf.ain){
                         AxlRow = hb.getUserMotAxlRaw();
                         sp.printf("MotAxl:%f ",AxlRow);
@@ -171,8 +162,6 @@
                 else {
                     cntSW2 = 0;
                 }
-                // Debug用
-                bDoCtrlMot = true;
             break;
             case WAKEUP:
                 if(gf_StateEnt){
@@ -199,7 +188,6 @@
                 }
             break;
             case STANDBY:
-                bDoCtrlMot = true;
                 if(gf_StateEnt){
                     // モーター回転数のチェック→モーターの回転数をオフセット値まで上げる
                     // TODO
@@ -222,18 +210,17 @@
             case IDLE:
                 if(gf_StateEnt){
                     sp.printf("IDLE state\r\n");
+                    hb.setAccelVal(FRONT,0);
+                    hb.setAccelVal(REAR, 0);
                     gf_StateEnt = false;
                 }
                 //SWのチェック
                 hb.chkSW(IDLE);
-                AxlRpm = hb.getUserMotAxl();
-                hb.setMotVal(R_L,AxlRpm);
-                hb.setMotVal(R_R,AxlRpm);
+                // AxlRpm = hb.getUserMotAxl();
+                // hb.setMotVal(R_L,AxlRpm);
+                // hb.setMotVal(R_R,AxlRpm);
                 // hb.setMotValOfs(R_L,AxlRpm);
                 // hb.setMotValOfs(R_R,AxlRpm);
-                bDoCtrlEng = true;
-                bDoCtrlMot = true;
-                //bDoCtrlAtt = true;
             break;
             case UPPER_IDLE:
                 if(gf_StateEnt){
@@ -243,11 +230,9 @@
                 }
                 //SWのチェック
                 hb.chkSW(UPPER_IDLE);
-                AxlRpm = hb.getUserMotAxl();
-                hb.setMotVal(R_L,AxlRpm);
-                hb.setMotVal(R_R,AxlRpm);
-                bDoCtrlEng = true;
-                bDoCtrlMot = true;
+                // AxlRpm = hb.getUserMotAxl();
+                // hb.setMotVal(R_L,AxlRpm);
+                // hb.setMotVal(R_R,AxlRpm);
             break;
             case TAKE_OFF:
                 if(gf_StateEnt){
@@ -262,18 +247,23 @@
                 // }
                 //else
                 hb.chkSW(TAKE_OFF);
-                AxlRpm = hb.getUserMotAxl();
-                hb.setMotVal(R_L,AxlRpm);
-                hb.setMotVal(R_R,AxlRpm);
+                // AxlRpm = hb.getUserMotAxl();
+                // hb.setMotVal(R_L,AxlRpm);
+                // hb.setMotVal(R_R,AxlRpm);
                 // hb.setMotValOfs(R_L,AxlRpm);
                 // hb.setMotValOfs(R_R,AxlRpm);
-                bDoCtrlEng = true;
-                bDoCtrlMot = true;
-                bDoCtrlAtt = true;
             break;
             case GROUND:
                 if(gf_StateEnt){
                     sp.printf("GROUND state\r\n");
+                    for(int i = 0; i < 4; ++i){
+                        gf_MtReq[i].req = true;
+                        gf_MtReq[i].val = 0;
+                        gf_MtReqOfs[i].req = true;
+                        gf_MtReqOfs[i].val = 0;
+                        gf_MtReqU[i].req = true;
+                        gf_MtReqU[i].val = 0;
+                    }
                     gf_StateEnt = false;
                 }
                 // そのままスルー
@@ -296,9 +286,7 @@
                     gf_StateEnt = false;
                 }
                 if(!hb.chkSWUserOpeAny()){
-                    //AxlRpm = hb.getUserMotAxl();
                     TrtlVal = hb.getUserEngTrottle();
-                    //if(AxlRpm > 0 || TrtlVal > 40){
                     if(TrtlVal > 10){
                         sp.printf("Throttle Val:%d\r\n",TrtlVal);
                         break;
@@ -322,13 +310,10 @@
                 if(hb.chkSWUserOpe(HbUserOpe::R_ENG_UP)){
                     if(hb.chkSetHvAxl(FRONT)){
                         gf_AxReq[0].bf.req = true;
-                        //gf_AxReq[1].bf.req = true;
                         gf_AxReq[0].bf.val = 0;
-                        //gf_AxReq[1].bf.val = 0;
                         setState(CHK_EG_MID);
                     }
                 }
-                bDoCtrlEng = true;
             break;
             case CHK_EG_MID:
                 if(gf_StateEnt){
@@ -336,16 +321,13 @@
                     gf_StateEnt = false;
                 }
                 if(!hb.chkSWUserOpeAny()){
-                    //AxlRpm = hb.getUserMotAxl();
                     TrtlVal = hb.getUserEngTrottle();
-                    //if(AxlRpm > 0 || TrtlVal > 40){
                     if(TrtlVal > 10){
                         break;
                     } else {
                         setState(CHK_EG_R);
                     }
                 }
-                bDoCtrlEng = true;
             break;
             case CHK_EG_R:
                 if(gf_StateEnt){
@@ -360,14 +342,11 @@
                 }
                 if(hb.chkSWUserOpe(HbUserOpe::R_ENG_UP)){
                     if(hb.chkSetHvAxl(REAR)){
-                        //gf_AxReq[0].bf.req = true;
                         gf_AxReq[1].bf.req = true;
-                        //gf_AxReq[0].bf.val = 0;
                         gf_AxReq[1].bf.val = 0;
                         setState(CHK_EG_EXIT);
                     }
                 }
-                bDoCtrlEng = true;
             break;
             case CHK_EG_EXIT:
                 if(gf_StateEnt){
@@ -382,7 +361,6 @@
                         setState(SLEEP);
                     }
                 }
-                bDoCtrlEng = true;
             break;
             case CHK_ENT:    //チェックエンター
                 if(gf_StateEnt){
@@ -397,7 +375,6 @@
                     gf_StateEnt = false;
                 }
                 //AxlRpm = hb.getUserMotAxl();
-                bDoCtrlMot = true;
             break;
             case CHK_AXL:    //アクセルサーボチェック
                 if(gf_StateEnt){
@@ -411,7 +388,6 @@
                     sp.printf("Check Attitude\r\n");
                     gf_StateEnt = false;
                 }
-                bDoCtrlAtt = true;
             break;
             case CHK_EXIT:   //チェックステート脱出
                 if(gf_StateEnt){
@@ -424,9 +400,6 @@
                     sp.printf("Motor Stop\r\n");
                     gf_StateEnt = false;
                 }
-                bDoCtrlAtt = false;
-                bDoCtrlMot = true;
-                bDoCtrlEng = true;
                 bTmp = true;
                 for(int i = 0; i < 4; ++i){
                     if(gf_MtReq[i].req){
@@ -460,18 +433,15 @@
                 //     sp.printf("MOTOR STOPPED!\r\n");
                 // }
                 // else{
-                //     //bDoCtrlMot = true;
                 //     sp.printf(".");
                 // }
             break;
         }
         
         //▼③各種設定
-        //hb.controlAttitude(); //姿勢制御
-        //hb.controlMotor();//モーター指令出し
-        if(bDoCtrlAtt)hb.controlAttitude(); //姿勢制御
-        if(bDoCtrlMot)hb.controlMotor();//モーター指令出し
-        if(bDoCtrlEng)hb.controlEngine(gf_State);//エンジン指令出し
+        hb.controlAttitude(); //姿勢制御
+        hb.controlMotor();//モーター指令出し
+        hb.controlEngine(gf_State);//エンジン指令出し
 
         if(gf_Print.d1.bf.stat){
             sp.printf("stat : [%X] ",gf_State);