Diff: userTask.cpp

Revision:

30:13ada1a24c59

Parent:

29:eb3d72dd94aa

Child:

31:56c554c560c1

--- a/userTask.cpp	Mon Dec 17 13:25:00 2018 +0000
+++ b/userTask.cpp	Tue Dec 18 11:57:37 2018 +0000
@@ -70,6 +70,7 @@
     
     bool bDoCtrlAtt;
     bool bDoCtrlMot;
+    INT16 tmpRpm, difRpm;
     //
     while(1){
         led1=!led1;
@@ -168,37 +169,92 @@
             break;
             case IDLE:
                 //SWのチェック
-                //離陸ボタン押下
-                //とりあえず、YawCtrlで代用
-                //if(hb.chkSWUserOpe(HbUserOpe::YAW_CTRL))
-                //{
-                //    // TODO IMU Cal
-                //    hb.calAtt();
-                //    setState(TAKE_OFF);
-                //}
-                //if(hb.chkSWUserOpeRE(HbUserOpe::F_L)){
-                if(hb.chkSWUserOpe(HbUserOpe::F_L)){
                     // Front Left 
-                    //hb.addMotOfs(HbUserOpe::F_L);
-                    gf_MtReqOP[0].req=true; gf_MtReqOP[0].add_end=false; gf_MtReqOP[0].counter = 0; gf_MtReqOP[0].num = 20;
-                    gf_MtReqOP[0].addVal = 0;
+                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::F_R)){
-                if(hb.chkSWUserOpe(HbUserOpe::F_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.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.chkSWUserOpe(HbUserOpe::R_L)){
-                    // Rear Left 
-                    //hb.addMotOfs(HbUserOpe::R_L);
-                }
-                if(hb.chkSWUserOpe(HbUserOpe::R_R)){
-                    // Rear Right
-                    //hb.addMotOfs(HbUserOpe::R_R);
-                }
-                if(hb.chkSWUserOpeRE(HbUserOpe::EMG_STOP)){
+
+
+                // //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){
@@ -213,9 +269,9 @@
                 //{
                 //   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: