Diff: userTask.cpp

Revision:

38:24ee50452755

Parent:

37:d51dacb4c30f

Child:

39:1b76f7df8804

--- a/userTask.cpp	Thu Jan 17 11:26:09 2019 +0000
+++ b/userTask.cpp	Fri Jan 18 11:52:00 2019 +0000
@@ -77,6 +77,8 @@
     INT16 cntSW2 = 0;
     INT16 AxlRpm;
     INT16 TrtlVal;
+    float AxlRow;
+    float TrtlRow;
     while(1){
         led1=!led1;
         bDoCtrlAtt = false;
@@ -96,8 +98,6 @@
         }
         // 確実にモーター等を止めるため
         if(gf_StopMot){
-            //setStateF(MOT_STOP);
-            //hb.getCurMotVal();
             for(int i = 0; i < 4; ++i){
                 gf_MtReq[i].req = true;
                 gf_MtReq[i].val = 0;
@@ -136,6 +136,12 @@
                     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);
+                } else {
+                    AxlRow = hb.getUserMotAxlRaw();
+                    //TrtlRow = hb.getUserEngTrottleRaw();
+                    if(gf_Print.bf.ain){
+                        sp.printf("MotAxl:%f\r\n",AxlRow);
+                    }
                 }
                 // 浮上（離陸）ボタン、着陸ボタンの同時長押し監視
                 if(hb.chkSWUserOpeBoth(HbUserOpe::FLT_ON,HbUserOpe::FLT_OFF)){
@@ -176,7 +182,7 @@
                     AxlRpm = hb.getUserMotAxl();
                     if(AxlRpm > 0){
                         // 将来的に警告音等
-                        sp.printf("Warning!! Motor Accel Opened!!\r\n");
+                        sp.printf("Warning!! Motor Accel Opened!![%d]\r\n",AxlRpm);
                         setStateF(SLEEP);
                     } else {
                         setState(STANDBY);
@@ -264,9 +270,12 @@
                 if(!hb.chkSWUserOpeAny()){
                     AxlRpm = hb.getUserMotAxl();
                     TrtlVal = hb.getUserEngTrottle();
-                    if(AxlRpm > 0 || TrtlVal > 40){
+                    //if(AxlRpm > 0 || TrtlVal > 40){
+                    if(AxlRpm > 0 || TrtlVal > 400){//暫定的に400
+                        sp.printf("Throttle Val:%d\r\n",TrtlVal);
                         break;
                     } else {
+                        hb.clearHvAxl();
                         setState(CHK_EG_F);
                     }
                 }
@@ -279,6 +288,7 @@
                 TrtlVal = hb.getUserEngTrottle();
                 hb.setAccelVal(FRONT, TrtlVal);
                 if(hb.chkSWUserOpe(HbUserOpe::F_ENG_UP)){
+                    sp.printf("FRONT Throttle Val:%d\r\n",TrtlVal);
                     hb.setHvAxl(FRONT, TrtlVal);
                 }
                 if(hb.chkSWUserOpe(HbUserOpe::R_ENG_UP)){
@@ -295,7 +305,7 @@
                 if(!hb.chkSWUserOpeAny()){
                     AxlRpm = hb.getUserMotAxl();
                     TrtlVal = hb.getUserEngTrottle();
-                    if(AxlRpm > 0 || TrtlVal > 40){
+                    if(AxlRpm > 0 || TrtlVal > 400){
                         break;
                     } else {
                         setState(CHK_EG_R);
@@ -310,6 +320,7 @@
                 TrtlVal = hb.getUserEngTrottle();
                 hb.setAccelVal(REAR, TrtlVal);
                 if(hb.chkSWUserOpe(HbUserOpe::F_ENG_UP)){
+                    sp.printf("REAR Throttle Val:%d\r\n",TrtlVal);
                     hb.setHvAxl(REAR, TrtlVal);
                 }
                 if(hb.chkSWUserOpe(HbUserOpe::R_ENG_UP)){
@@ -326,7 +337,7 @@
                 if(!hb.chkSWUserOpeAny()){
                     AxlRpm = hb.getUserMotAxl();
                     TrtlVal = hb.getUserEngTrottle();
-                    if(AxlRpm > 0 || TrtlVal > 40){
+                    if(AxlRpm > 0 || TrtlVal > 400){
                         break;
                     } else {
                         setState(SLEEP);
@@ -338,12 +349,14 @@
                     sp.printf("Check enter\r\n");
                     gf_StateEnt = false;
                 }
+                //setStateF(CHK_MOT);
             break;
             case CHK_MOT:    //モーターチェック
                 if(gf_StateEnt){
                     sp.printf("Check Motor\r\n");
                     gf_StateEnt = false;
                 }
+                //AxlRpm = hb.getUserMotAxl();
                 bDoCtrlMot = true;
             break;
             case CHK_AXL:    //アクセルサーボチェック
@@ -351,6 +364,7 @@
                     sp.printf("Check Accel Servo\r\n");
                     gf_StateEnt = false;
                 }
+                //TrtlVal = hb.getUserEngTrottle();
             break;
             case CHK_ATT:    //姿勢制御チェック
                 if(gf_StateEnt){