Diff: userTask.cpp

Revision:

31:56c554c560c1

Parent:

30:13ada1a24c59

Child:

32:7f4145cc3551

--- a/userTask.cpp	Tue Dec 18 11:57:37 2018 +0000
+++ b/userTask.cpp	Wed Dec 19 12:22:22 2018 +0000
@@ -70,13 +70,14 @@
     
     bool bDoCtrlAtt;
     bool bDoCtrlMot;
-    INT16 tmpRpm, difRpm;
+    bool bDoCtrlEng;
+    //INT16 tmpRpm, difRpm;
     //
     while(1){
         led1=!led1;
         bDoCtrlAtt = false;
         bDoCtrlMot = false;
-        
+        bDoCtrlEng = false;
         // PID係数アップデートチェック
         if(gf_PidParaUpdate){
             hb.setAttPara(g_PidPara);
@@ -86,6 +87,7 @@
             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);
+            sp.printf("V   : [%f]\r\n",g_PidPara.V);
             gf_PidParaUpdate = false;
         }
         for(int i = 0; i < 4; ++i){
@@ -124,28 +126,12 @@
                     gf_Dbg = false;
                     taskStart(2);
                 }
-                for(int i = 0; i < 8; ++i){
-                    if(gf_MtReqDct[i].bf.req){
-                        hb.setMotFPGA(i, gf_MtReqDct[i].bf.val);
-                        gf_MtReqDct[i].bf.req = false;
-                    }
-                }
-                //else if(gf_Armed){
-                //    gf_Armed = false;
-//              //      hb.initChkMotor();
-                //    setState(WAKEUP);
-                //}
             break;
             case WAKEUP:
                 //各種モーター
                 //機材のチェック
 //                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;
@@ -169,110 +155,10 @@
             break;
             case IDLE:
                 //SWのチェック
-                    // Front Left 
-                if(hb.chkSWUserOpeRE(HbUserOpe::BRK_L)){
-                    gf_MtReqOP[0].req=true;
-                    tmpRpm = hb.getCurMotVal(F_L);
-                    if(tmpRpm < 6000){
-                        hb.setMotVal(F_L, 6000);
-                    }
-                } else if(hb.chkSWUserOpe(HbUserOpe::BRK_L)){
-                    if(gf_MtReqOP[0].req==true){
-                        hb.setMotVal(F_L, 6000);
-                    }
-                } else if(!hb.chkSWUserOpe(HbUserOpe::BRK_L)){
-                    if(gf_MtReqOP[0].req){
-                        tmpRpm = hb.getCurMotVal(F_L);
-                        if(tmpRpm == 0){
-                            init1PushStruct(gf_MtReqOP[0]);
-                        }else if(tmpRpm > 1000){
-                            tmpRpm-=100;
-                        } else if(tmpRpm >= 100){
-                            tmpRpm-=50;
-                        } else if(tmpRpm > 0){
-                            tmpRpm-=10;
-                            if(tmpRpm < 0) tmpRpm = 0;
-                        } else if(tmpRpm < -1000){
-                            tmpRpm+=100;
-                        } else if(tmpRpm <= -100){
-                            tmpRpm+=50;
-                        }else if(tmpRpm < 0){
-                            tmpRpm+=10;
-                            if(tmpRpm > 0) tmpRpm = 0;
-                        }
-                        hb.setMotVal(F_L, tmpRpm);
-                    }
-                }
-
-                if(hb.chkSWUserOpeRE(HbUserOpe::BRK_R)){
-                    gf_MtReqOP[1].req=true;
-                    tmpRpm = hb.getCurMotVal(F_R);
-                    if(tmpRpm < 6000){
-                        hb.setMotVal(F_R, 6000);
-                    }
-                } else if(hb.chkSWUserOpe(HbUserOpe::BRK_R)){
-                    if(gf_MtReqOP[1].req==true){
-                        hb.setMotVal(F_R, 6000);
-                    }
-                } else if(!hb.chkSWUserOpe(HbUserOpe::BRK_R)){
-                    if(gf_MtReqOP[1].req){
-                        tmpRpm = hb.getCurMotVal(F_R);
-                        if(tmpRpm == 0){
-                            init1PushStruct(gf_MtReqOP[1]);
-                        }else if(tmpRpm > 1000){
-                            tmpRpm-=100;
-                        } else if(tmpRpm >= 100){
-                            tmpRpm-=50;
-                        } else if(tmpRpm > 0){
-                            tmpRpm-=10;
-                            if(tmpRpm < 0) tmpRpm = 0;
-                        } else if(tmpRpm < -1000){
-                            tmpRpm+=100;
-                        } else if(tmpRpm <= -100){
-                            tmpRpm+=50;
-                        }else if(tmpRpm < 0){
-                            tmpRpm+=10;
-                            if(tmpRpm > 0) tmpRpm = 0;
-                        }
-                        hb.setMotVal(F_R, tmpRpm);
-                    }
-                }
-
-
-                // //if(hb.chkSWUserOpeRE(HbUserOpe::F_R)){
-                // if(hb.chkSWUserOpe(HbUserOpe::BRK_R)){
-                //     // Front Right 
-                //     //hb.addMotOfs(HbUserOpe::F_R);
-                //     // gf_MtReqOP[1].req=true; gf_MtReqOP[1].add_end=false; gf_MtReqOP[1].counter = 0; gf_MtReqOP[1].num = 20;
-                //     // gf_MtReqOP[1].addVal = 0;
-                // }
-                // if(hb.chkSWUserOpe(HbUserOpe::R_1)){
-                //     // Rear Left 
-                //     //hb.addMotOfs(HbUserOpe::R_L);
-                // }
-                // if(hb.chkSWUserOpe(HbUserOpe::R_2)){
-                //     // Rear Right
-                //     //hb.addMotOfs(HbUserOpe::R_R);
-                // }
-                if(hb.chkSWUserOpeRE(HbUserOpe::MOT_STOP)){
-                    // EMG STOP だが、テスト用のため、モーターのオフセットを0にして回転を止める
-                    //sp.printf("RisingEdge!\r\n");
-                    for(int i = 0; i < 4; ++i){
-                        gf_MtReq[i].bf.req = true;
-                        gf_MtReq[i].bf.val = 0;
-                        gf_MtReqOfs[i].bf.req = true;
-                        gf_MtReqOfs[i].bf.val = 0;
-                    }
-                }
-                
-                //else if(hb.chkSWUserOpe(HbUserOpe::ENG_STOP))
-                //{
-                //   setState(SLEEP);
-                //}
-                // if(hb.chkSWUserOpeAny()){
-                //     sp.printf("SW :[%X]\r\n",hb.getUserSw());
-                // }
+                hb.chkSW(IDLE);
+                bDoCtrlEng = true;
                 bDoCtrlMot = true;
+                bDoCtrlAtt = true;
             break;
             case TAKE_OFF:
                 // エンジン回転数のチェック
@@ -281,6 +167,7 @@
                 //     setState(GROUND);
                 // }
                 //else
+                bDoCtrlEng = true;
                 bDoCtrlMot = true;
                 bDoCtrlAtt = true;
                 //if(hb.chkSWUserOpe(HbUserOpe::STOP)){
@@ -334,7 +221,8 @@
         //hb.controlMotor();//モーター指令出し
         if(bDoCtrlAtt)hb.controlAttitude(); //姿勢制御
         if(bDoCtrlMot)hb.controlMotor();//モーター指令出し
-        
+        //if(bDoCtrlEng)hb.controlEngine();//エンジン指令出し
+
         if(gf_Print.bf.stat){
             sp.printf("stat : [%X]\r\n",gf_State);
         }