Diff: userTask.cpp

Revision:

37:d51dacb4c30f

Parent:

36:2cc739c7e4cb

Child:

38:24ee50452755

--- a/userTask.cpp	Wed Jan 16 10:51:07 2019 +0000
+++ b/userTask.cpp	Thu Jan 17 11:26:09 2019 +0000
@@ -71,6 +71,7 @@
     bool bDoCtrlAtt;
     bool bDoCtrlMot;
     bool bDoCtrlEng;
+    bool bTmp;
     //INT16 tmpRpm, difRpm;
     INT16 cntSW1 = 0;
     INT16 cntSW2 = 0;
@@ -93,16 +94,7 @@
             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;
-        //     }
-        // }
+        // 確実にモーター等を止めるため
         if(gf_StopMot){
             //setStateF(MOT_STOP);
             //hb.getCurMotVal();
@@ -112,6 +104,12 @@
                 gf_MtReqOfs[i].req = true;
                 gf_MtReqOfs[i].val = 0;
             }
+            for(int i = 0; i < 2; ++i){
+                gf_AxReq[i].bf.req = true;
+                gf_AxReq[i].bf.val = 0;
+            }
+            gf_FromActiveStat = isActiveState();
+            setStateF(MOT_STOP);
             gf_StopMot = false;
         }
 
@@ -133,6 +131,12 @@
                     gf_Dbg = false;
                     taskStart(2);
                 }
+                if(hb.chkSWUserOpeAny()){
+                    typUserSw sw = hb.getUserSw();
+                    sp.printf("%d%d%d%d%d%d%d%d%d%d\r\n",sw.bf.brk_r,sw.bf.brk_l,sw.bf.flt_on,sw.bf.flt_off,
+                                    sw.bf.f_eng_up,sw.bf.f_eng_down,sw.bf.r_eng_up,sw.bf.r_eng_down,
+                                    sw.bf.rsv_1, sw.bf.all_stop);
+                }
                 // 浮上（離陸）ボタン、着陸ボタンの同時長押し監視
                 if(hb.chkSWUserOpeBoth(HbUserOpe::FLT_ON,HbUserOpe::FLT_OFF)){
                     ++cntSW1;
@@ -366,6 +370,35 @@
                     sp.printf("Motor Stop\r\n");
                     gf_StateEnt = false;
                 }
+                bDoCtrlAtt = false;
+                bDoCtrlMot = true;
+                bTmp = true;
+                for(int i = 0; i < 4; ++i){
+                    if(gf_MtReq[i].req){
+                        bTmp = false;
+                        break;
+                    }
+                    if(gf_MtReqOfs[i].req){
+                        bTmp = false;
+                        break;
+                    }
+                }
+                if(!bTmp)break;
+                for(int i = 0; i < 2; ++i){
+                    if(gf_AxReq[i].bf.req){
+                        bTmp = false;
+                        break;
+                    }
+                }
+                if(bTmp){
+                    if(gf_FromActiveStat){
+                        setStateF(IDLE);
+                    }
+                    else{
+                        setStateF(SLEEP);
+                    }
+                }
+
                 // if(hb.stopMotor() == true){
                 //     hb.initMotVal();
                 //     setState(SLEEP);