Diff: hbCommand.cpp

Revision:

56:f363a6877c6a

Parent:

53:b09c062cc31c

Child:

59:6b4dcd964d0f

--- a/hbCommand.cpp	Mon Mar 04 08:07:20 2019 +0000
+++ b/hbCommand.cpp	Wed Mar 06 02:18:47 2019 +0000
@@ -26,269 +26,164 @@
         }
     }
     
-    //if(gf_State != SLEEP && gf_State != WAKEUP){
-    if(gf_State != SLEEP){
-        //EMG STOP 現在はモーターの強制停止
-        if (g_CmdBuf[0] == ESC){gf_StopMot = true;}
-        //リード系
-        else if (strcmp(g_CmdBuf , "re"  )==0 ){gf_Print.d1.bf.e1=true; gf_Print.d1.bf.e2=true;}
-        else if (strcmp(g_CmdBuf , "re1" )==0 ){gf_Print.d1.bf.e1=true;}
-        else if (strcmp(g_CmdBuf , "re2" )==0 ){gf_Print.d1.bf.e2=true;}
-        else if (strcmp(g_CmdBuf , "rep"  )==0 ){gf_Print.d2.bf.ep1=true; gf_Print.d2.bf.ep2=true;}//エンジン用サーボにセットされた値
-        else if (strcmp(g_CmdBuf , "rep1" )==0 ){gf_Print.d2.bf.ep1=true;}//前エンジン用サーボにセットされた値
-        else if (strcmp(g_CmdBuf , "rep2" )==0 ){gf_Print.d2.bf.ep2=true;}//後エンジン用サーボにセットされた値
-        else if (strcmp(g_CmdBuf , "rm"  )==0 ){gf_Print.d1.bf.m1=true;gf_Print.d1.bf.m2=true;gf_Print.d1.bf.m3=true;gf_Print.d1.bf.m4=true;}//リードモーター  
-        else if (strcmp(g_CmdBuf , "rm1" )==0 ){gf_Print.d1.bf.m1=true;}//リードモーター1 
-        else if (strcmp(g_CmdBuf , "rm2" )==0 ){gf_Print.d1.bf.m2=true;}//リードモーター2 
-        else if (strcmp(g_CmdBuf , "rm3" )==0 ){gf_Print.d1.bf.m3=true;}//リードモーター3 
-        else if (strcmp(g_CmdBuf , "rm4" )==0 ){gf_Print.d1.bf.m4=true;}//リードモーター4 
-        else if (strcmp(g_CmdBuf , "rg"  )==0 ){gf_Print.d1.bf.gy=true;}//リードジャイロ  
-        else if (strcmp(g_CmdBuf , "ry"  )==0 ){gf_Print.d1.bf.yaw=true;}//リード姿勢角ヨー
-        else if (strcmp(g_CmdBuf , "rimu")==0 ){gf_Print.d1.bf.yaw=true;gf_Print.d1.bf.gy=true;}//リードIMU
-        else if (strcmp(g_CmdBuf , "rs"  )==0 ){gf_Print.d2.bf.sw=true;}//リードスイッチ
-        else if (strcmp(g_CmdBuf , "rfb" )==0 ){gf_Print.d1.bf.fb=true;}//PID制御フィードバック結果
-        else if (strcmp(g_CmdBuf , "rp"  )==0 ){gf_Print.d1.bf.pp=true;gf_Print.d1.bf.p=true;gf_Print.d1.bf.i=true;gf_Print.d1.bf.d=true;}//パラメータ全表示
-        else if (strcmp(g_CmdBuf , "rpP" )==0 ){gf_Print.d1.bf.pp=true;}//パラメータアウターPゲイン表示    
-        else if (strcmp(g_CmdBuf , "rpp" )==0 ){gf_Print.d1.bf.p=true;}//パラメータPゲイン表示 
-        else if (strcmp(g_CmdBuf , "rpi" )==0 ){gf_Print.d1.bf.i=true;}//パラメータIゲイン表示 
-        else if (strcmp(g_CmdBuf , "rpd" )==0 ){gf_Print.d1.bf.d=true;}//パラメータDゲイン表示
-        else if (strcmp(g_CmdBuf , "rmrpm" )==0 ){gf_Print.d2.bf.mrpm=true;}//Motorに支持されたRPM
-        
-        //設定系
-        else if (strcmp(g_CmdBuf , "se1"   )==0 ){gf_AxReq[0].bf.req=true; gf_AxReq[0].bf.val=(UINT16)atoi(&g_CmdBuf[arg2pos]);}
-        else if (strcmp(g_CmdBuf , "se2"   )==0 ){gf_AxReq[1].bf.req=true; gf_AxReq[1].bf.val=(UINT16)atoi(&g_CmdBuf[arg2pos]);}
-        else if (strcmp(g_CmdBuf , "sep1"   )==0 ){gf_AxReq[0].bf.val=(UINT16)atoi(&g_CmdBuf[arg2pos]);}
-        else if (strcmp(g_CmdBuf , "sep2"   )==0 ){gf_AxReq[1].bf.val=(UINT16)atoi(&g_CmdBuf[arg2pos]);}
-        else if (strcmp(g_CmdBuf , "seh1"   )==0 ){gf_AxReqH[0].bf.req=true; gf_AxReqH[0].bf.val=(UINT16)atoi(&g_CmdBuf[arg2pos]);}
-        else if (strcmp(g_CmdBuf , "seh2"   )==0 ){gf_AxReqH[1].bf.req=true; gf_AxReqH[1].bf.val=(UINT16)atoi(&g_CmdBuf[arg2pos]);}
-        else if (strcmp(g_CmdBuf , "ses1"   )==0 ){gf_AxStepReq[0].bf.req=true; gf_AxStepReq[0].bf.val=(UINT16)atoi(&g_CmdBuf[arg2pos]);}
-        else if (strcmp(g_CmdBuf , "ses2"   )==0 ){gf_AxStepReq[1].bf.req=true; gf_AxStepReq[1].bf.val=(UINT16)atoi(&g_CmdBuf[arg2pos]);}
-        else if (strcmp(g_CmdBuf , "seas"   )==0 ){ gf_AxAdjStepReq[0].bf.req=true; gf_AxAdjStepReq[0].bf.val=(UINT16)atoi(&g_CmdBuf[arg2pos]);
-                                                    gf_AxAdjStepReq[1].bf.req=true; gf_AxAdjStepReq[1].bf.val=(UINT16)atoi(&g_CmdBuf[arg2pos]);}
-        else if (strcmp(g_CmdBuf , "seas1"   )==0 ){gf_AxAdjStepReq[0].bf.req=true; gf_AxAdjStepReq[0].bf.val=(UINT16)atoi(&g_CmdBuf[arg2pos]);}
-        else if (strcmp(g_CmdBuf , "seas2"   )==0 ){gf_AxAdjStepReq[1].bf.req=true; gf_AxAdjStepReq[1].bf.val=(UINT16)atoi(&g_CmdBuf[arg2pos]);}
-        else if (strcmp(g_CmdBuf , "sm1"   )==0 ){gf_MtReq[0].req=true; gf_MtReq[0].val=(UINT16)atoi(&g_CmdBuf[arg2pos]);}
-        else if (strcmp(g_CmdBuf , "sm2"   )==0 ){gf_MtReq[1].req=true; gf_MtReq[1].val=(UINT16)atoi(&g_CmdBuf[arg2pos]);}
-        else if (strcmp(g_CmdBuf , "sm3"   )==0 ){gf_MtReq[2].req=true; gf_MtReq[2].val=(UINT16)atoi(&g_CmdBuf[arg2pos]);}
-        else if (strcmp(g_CmdBuf , "sm4"   )==0 ){gf_MtReq[3].req=true; gf_MtReq[3].val=(UINT16)atoi(&g_CmdBuf[arg2pos]);}
-        else if (strcmp(g_CmdBuf , "sm"   )==0 ){
-            for(int i = 0; i < 4; ++i){
-                gf_MtReq[i].req=true; 
-                gf_MtReq[i].val=(UINT16)atoi(&g_CmdBuf[arg2pos]);
-            }
-            }
-        else if (strcmp(g_CmdBuf , "smf"   )==0 ){
-            gf_MtReq[0].req=true;
-            gf_MtReq[0].val=(UINT16)atoi(&g_CmdBuf[arg2pos]);
-            gf_MtReq[1].req=true;
-            gf_MtReq[1].val=(UINT16)atoi(&g_CmdBuf[arg2pos]);}
-        else if (strcmp(g_CmdBuf , "smr"   )==0 ){
-            gf_MtReq[2].req=true;
-            gf_MtReq[2].val=(UINT16)atoi(&g_CmdBuf[arg2pos]);
-            gf_MtReq[3].req=true;
-            gf_MtReq[3].val=(UINT16)atoi(&g_CmdBuf[arg2pos]);            }
-        else if (strcmp(g_CmdBuf , "smao1"   )==0 ){gf_MtAttOfs[0].req=true; gf_MtAttOfs[0].val=(UINT16)atoi(&g_CmdBuf[arg2pos]);}
-        else if (strcmp(g_CmdBuf , "smao2"   )==0 ){gf_MtAttOfs[1].req=true; gf_MtAttOfs[1].val=(UINT16)atoi(&g_CmdBuf[arg2pos]);}
-        else if (strcmp(g_CmdBuf , "smao3"   )==0 ){gf_MtAttOfs[2].req=true; gf_MtAttOfs[2].val=(UINT16)atoi(&g_CmdBuf[arg2pos]);}
-        else if (strcmp(g_CmdBuf , "smao4"   )==0 ){gf_MtAttOfs[3].req=true; gf_MtAttOfs[3].val=(UINT16)atoi(&g_CmdBuf[arg2pos]);}
-        else if (strcmp(g_CmdBuf , "smao"   )==0 ){
-            for(int i = 0; i < 4; ++i){
-                gf_MtAttOfs[i].req=true; 
-                gf_MtAttOfs[i].val=(UINT16)atoi(&g_CmdBuf[arg2pos]);
-            }
+    //EMG STOP 現在はモーターの強制停止
+    if (g_CmdBuf[0] == ESC){gf_StopMot = true;}
+    //else if (strcmp(g_CmdBuf , "sarm"   )==0 ){gf_Armed = true;}//アーミングフラグセット テスト用
+    else if (strcmp(g_CmdBuf , "dbg"   )==0 ){if(gf_Dbg != true) gf_Dbg = true;}//デバッグタスク起動
+    else if (strcmp(g_CmdBuf , "smfpga1"   )==0 ){gf_MtReqDct[0].req=true; gf_MtReqDct[0].val=(UINT16)atoi(&g_CmdBuf[arg2pos]);}
+    else if (strcmp(g_CmdBuf , "smfpga2"   )==0 ){gf_MtReqDct[1].req=true; gf_MtReqDct[1].val=(UINT16)atoi(&g_CmdBuf[arg2pos]);}
+    else if (strcmp(g_CmdBuf , "smfpga3"   )==0 ){gf_MtReqDct[2].req=true; gf_MtReqDct[2].val=(UINT16)atoi(&g_CmdBuf[arg2pos]);}
+    else if (strcmp(g_CmdBuf , "smfpga4"   )==0 ){gf_MtReqDct[3].req=true; gf_MtReqDct[3].val=(UINT16)atoi(&g_CmdBuf[arg2pos]);}
+    else if (strcmp(g_CmdBuf , "smfpga5"   )==0 ){gf_MtReqDct[4].req=true; gf_MtReqDct[4].val=(UINT16)atoi(&g_CmdBuf[arg2pos]);}
+    else if (strcmp(g_CmdBuf , "smfpga6"   )==0 ){gf_MtReqDct[5].req=true; gf_MtReqDct[5].val=(UINT16)atoi(&g_CmdBuf[arg2pos]);}
+    else if (strcmp(g_CmdBuf , "smfpga7"   )==0 ){gf_MtReqDct[6].req=true; gf_MtReqDct[6].val=(UINT16)atoi(&g_CmdBuf[arg2pos]);}
+    else if (strcmp(g_CmdBuf , "smfpga8"   )==0 ){gf_MtReqDct[7].req=true; gf_MtReqDct[7].val=(UINT16)atoi(&g_CmdBuf[arg2pos]);}
+    //リード系
+    else if (strcmp(g_CmdBuf , "re"  )==0 ){gf_Print.d1.bf.e1=true; gf_Print.d1.bf.e2=true;}
+    else if (strcmp(g_CmdBuf , "re1" )==0 ){gf_Print.d1.bf.e1=true;}
+    else if (strcmp(g_CmdBuf , "re2" )==0 ){gf_Print.d1.bf.e2=true;}
+    else if (strcmp(g_CmdBuf , "rep"  )==0 ){gf_Print.d2.bf.ep1=true; gf_Print.d2.bf.ep2=true;}//エンジン用サーボにセットされた値
+    else if (strcmp(g_CmdBuf , "rep1" )==0 ){gf_Print.d2.bf.ep1=true;}//前エンジン用サーボにセットされた値
+    else if (strcmp(g_CmdBuf , "rep2" )==0 ){gf_Print.d2.bf.ep2=true;}//後エンジン用サーボにセットされた値
+    else if (strcmp(g_CmdBuf , "rm"  )==0 ){gf_Print.d1.bf.m1=true;gf_Print.d1.bf.m2=true;gf_Print.d1.bf.m3=true;gf_Print.d1.bf.m4=true;}//リードモーター  
+    else if (strcmp(g_CmdBuf , "rm1" )==0 ){gf_Print.d1.bf.m1=true;}//リードモーター1 
+    else if (strcmp(g_CmdBuf , "rm2" )==0 ){gf_Print.d1.bf.m2=true;}//リードモーター2 
+    else if (strcmp(g_CmdBuf , "rm3" )==0 ){gf_Print.d1.bf.m3=true;}//リードモーター3 
+    else if (strcmp(g_CmdBuf , "rm4" )==0 ){gf_Print.d1.bf.m4=true;}//リードモーター4 
+    else if (strcmp(g_CmdBuf , "rg"  )==0 ){gf_Print.d1.bf.gy=true;}//リードジャイロ  
+    else if (strcmp(g_CmdBuf , "ry"  )==0 ){gf_Print.d1.bf.yaw=true;}//リード姿勢角ヨー
+    else if (strcmp(g_CmdBuf , "rimu")==0 ){gf_Print.d1.bf.yaw=true;gf_Print.d1.bf.gy=true;}//リードIMU
+    else if (strcmp(g_CmdBuf , "rs"  )==0 ){gf_Print.d2.bf.sw=true;}//リードスイッチ
+    else if (strcmp(g_CmdBuf , "rfb" )==0 ){gf_Print.d1.bf.fb=true;}//PID制御フィードバック結果
+    else if (strcmp(g_CmdBuf , "rp"  )==0 ){gf_Print.d1.bf.pp=true;gf_Print.d1.bf.p=true;gf_Print.d1.bf.i=true;gf_Print.d1.bf.d=true;}//パラメータ全表示
+    else if (strcmp(g_CmdBuf , "rpP" )==0 ){gf_Print.d1.bf.pp=true;}//パラメータアウターPゲイン表示    
+    else if (strcmp(g_CmdBuf , "rpp" )==0 ){gf_Print.d1.bf.p=true;}//パラメータPゲイン表示 
+    else if (strcmp(g_CmdBuf , "rpi" )==0 ){gf_Print.d1.bf.i=true;}//パラメータIゲイン表示 
+    else if (strcmp(g_CmdBuf , "rpd" )==0 ){gf_Print.d1.bf.d=true;}//パラメータDゲイン表示
+    else if (strcmp(g_CmdBuf , "rmrpm" )==0 ){gf_Print.d2.bf.mrpm=true;}//Motorに支持されたRPM
+    
+    //設定系
+    else if (strcmp(g_CmdBuf , "se1"   )==0 ){gf_AxReq[0].bf.req=true; gf_AxReq[0].bf.val=(UINT16)atoi(&g_CmdBuf[arg2pos]);}
+    else if (strcmp(g_CmdBuf , "se2"   )==0 ){gf_AxReq[1].bf.req=true; gf_AxReq[1].bf.val=(UINT16)atoi(&g_CmdBuf[arg2pos]);}
+    else if (strcmp(g_CmdBuf , "sep1"   )==0 ){gf_AxReq[0].bf.val=(UINT16)atoi(&g_CmdBuf[arg2pos]);}
+    else if (strcmp(g_CmdBuf , "sep2"   )==0 ){gf_AxReq[1].bf.val=(UINT16)atoi(&g_CmdBuf[arg2pos]);}
+    else if (strcmp(g_CmdBuf , "seh1"   )==0 ){gf_AxReqH[0].bf.req=true; gf_AxReqH[0].bf.val=(UINT16)atoi(&g_CmdBuf[arg2pos]);}
+    else if (strcmp(g_CmdBuf , "seh2"   )==0 ){gf_AxReqH[1].bf.req=true; gf_AxReqH[1].bf.val=(UINT16)atoi(&g_CmdBuf[arg2pos]);}
+    else if (strcmp(g_CmdBuf , "ses1"   )==0 ){gf_AxStepReq[0].bf.req=true; gf_AxStepReq[0].bf.val=(UINT16)atoi(&g_CmdBuf[arg2pos]);}
+    else if (strcmp(g_CmdBuf , "ses2"   )==0 ){gf_AxStepReq[1].bf.req=true; gf_AxStepReq[1].bf.val=(UINT16)atoi(&g_CmdBuf[arg2pos]);}
+    else if (strcmp(g_CmdBuf , "seas"   )==0 ){ gf_AxAdjStepReq[0].bf.req=true; gf_AxAdjStepReq[0].bf.val=(UINT16)atoi(&g_CmdBuf[arg2pos]);
+                                                gf_AxAdjStepReq[1].bf.req=true; gf_AxAdjStepReq[1].bf.val=(UINT16)atoi(&g_CmdBuf[arg2pos]);}
+    else if (strcmp(g_CmdBuf , "seas1"   )==0 ){gf_AxAdjStepReq[0].bf.req=true; gf_AxAdjStepReq[0].bf.val=(UINT16)atoi(&g_CmdBuf[arg2pos]);}
+    else if (strcmp(g_CmdBuf , "seas2"   )==0 ){gf_AxAdjStepReq[1].bf.req=true; gf_AxAdjStepReq[1].bf.val=(UINT16)atoi(&g_CmdBuf[arg2pos]);}
+    else if (strcmp(g_CmdBuf , "sm1"   )==0 ){gf_MtReq[0].req=true; gf_MtReq[0].val=(UINT16)atoi(&g_CmdBuf[arg2pos]);}
+    else if (strcmp(g_CmdBuf , "sm2"   )==0 ){gf_MtReq[1].req=true; gf_MtReq[1].val=(UINT16)atoi(&g_CmdBuf[arg2pos]);}
+    else if (strcmp(g_CmdBuf , "sm3"   )==0 ){gf_MtReq[2].req=true; gf_MtReq[2].val=(UINT16)atoi(&g_CmdBuf[arg2pos]);}
+    else if (strcmp(g_CmdBuf , "sm4"   )==0 ){gf_MtReq[3].req=true; gf_MtReq[3].val=(UINT16)atoi(&g_CmdBuf[arg2pos]);}
+    else if (strcmp(g_CmdBuf , "sm"   )==0 ){
+        for(int i = 0; i < 4; ++i){
+            gf_MtReq[i].req=true; 
+            gf_MtReq[i].val=(UINT16)atoi(&g_CmdBuf[arg2pos]);
         }
-        else if (strcmp(g_CmdBuf , "smo1"   )==0 ){gf_MtReqOfs[0].req=true; gf_MtReqOfs[0].val=(UINT16)atoi(&g_CmdBuf[arg2pos]);}
-        else if (strcmp(g_CmdBuf , "smo2"   )==0 ){gf_MtReqOfs[1].req=true; gf_MtReqOfs[1].val=(UINT16)atoi(&g_CmdBuf[arg2pos]);}
-        else if (strcmp(g_CmdBuf , "smo3"   )==0 ){gf_MtReqOfs[2].req=true; gf_MtReqOfs[2].val=(UINT16)atoi(&g_CmdBuf[arg2pos]);}
-        else if (strcmp(g_CmdBuf , "smo4"   )==0 ){gf_MtReqOfs[3].req=true; gf_MtReqOfs[3].val=(UINT16)atoi(&g_CmdBuf[arg2pos]);}
-        else if (strcmp(g_CmdBuf , "smo"   )==0 ){
-            for(int i = 0; i < 4; ++i){
-                gf_MtReqOfs[i].req=true; 
-                gf_MtReqOfs[i].val=(UINT16)atoi(&g_CmdBuf[arg2pos]);
-            }
+        }
+    else if (strcmp(g_CmdBuf , "smf"   )==0 ){
+        gf_MtReq[0].req=true;
+        gf_MtReq[0].val=(UINT16)atoi(&g_CmdBuf[arg2pos]);
+        gf_MtReq[1].req=true;
+        gf_MtReq[1].val=(UINT16)atoi(&g_CmdBuf[arg2pos]);}
+    else if (strcmp(g_CmdBuf , "smr"   )==0 ){
+        gf_MtReq[2].req=true;
+        gf_MtReq[2].val=(UINT16)atoi(&g_CmdBuf[arg2pos]);
+        gf_MtReq[3].req=true;
+        gf_MtReq[3].val=(UINT16)atoi(&g_CmdBuf[arg2pos]);            }
+    else if (strcmp(g_CmdBuf , "smao1"   )==0 ){gf_MtAttOfs[0].req=true; gf_MtAttOfs[0].val=(UINT16)atoi(&g_CmdBuf[arg2pos]);}//!< Set Motor Attitude Offset 1
+    else if (strcmp(g_CmdBuf , "smao2"   )==0 ){gf_MtAttOfs[1].req=true; gf_MtAttOfs[1].val=(UINT16)atoi(&g_CmdBuf[arg2pos]);}//!< Set Motor Attitude Offset 2
+    else if (strcmp(g_CmdBuf , "smao3"   )==0 ){gf_MtAttOfs[2].req=true; gf_MtAttOfs[2].val=(UINT16)atoi(&g_CmdBuf[arg2pos]);}//!< Set Motor Attitude Offset 3
+    else if (strcmp(g_CmdBuf , "smao4"   )==0 ){gf_MtAttOfs[3].req=true; gf_MtAttOfs[3].val=(UINT16)atoi(&g_CmdBuf[arg2pos]);}//!< Set Motor Attitude Offset 4
+    else if (strcmp(g_CmdBuf , "smao"   )==0 ){//!< Set Motor Attitude Offset
+        for(int i = 0; i < 4; ++i){
+            gf_MtAttOfs[i].req=true; 
+            gf_MtAttOfs[i].val=(UINT16)atoi(&g_CmdBuf[arg2pos]);
         }
-        else if (strcmp(g_CmdBuf , "smof"   )==0 ){
-            gf_MtReqOfs[0].req=true;
-            gf_MtReqOfs[0].val=(UINT16)atoi(&g_CmdBuf[arg2pos]);
-            gf_MtReqOfs[1].req=true;
-            gf_MtReqOfs[1].val=(UINT16)atoi(&g_CmdBuf[arg2pos]);}
-        else if (strcmp(g_CmdBuf , "smor"   )==0 ){
-            gf_MtReqOfs[2].req=true;
-            gf_MtReqOfs[2].val=(UINT16)atoi(&g_CmdBuf[arg2pos]);
-            gf_MtReqOfs[3].req=true;
-            gf_MtReqOfs[3].val=(UINT16)atoi(&g_CmdBuf[arg2pos]);            }
-
-        //キャリブレーション
-        else if (strcmp(g_CmdBuf , "calyaw")==0 ){gf_Cal.bf.yaw=true;}//
-        else if (strcmp(g_CmdBuf , "calgyro")==0 ){gf_Cal.bf.gy=true;}//
-        else if (strcmp(g_CmdBuf , "calimu")==0 ){gf_Cal.bf.gy=true;gf_Cal.bf.yaw=true;}//
-        
-        //チェック系
-
-        //連続モニタ
-        else if (strcmp(g_CmdBuf , "mg"  )==0 ){gf_Print.d1.bf.gy=true;gf_Mon.d1.bf.gy=true;}//リードジャイロ  
-        else if (strcmp(g_CmdBuf , "my"  )==0 ){gf_Print.d1.bf.yaw=true;gf_Mon.d1.bf.yaw=true;}//リード姿勢角ヨー
-        else if (strcmp(g_CmdBuf , "mimu")==0 ){gf_Print.d1.bf.yaw=true;gf_Print.d1.bf.gy=true;gf_Mon.d1.bf.yaw=true;gf_Mon.d1.bf.gy=true;}//リードIMU
-        else if (strcmp(g_CmdBuf , "mfb" )==0 ){gf_Print.d1.bf.fb=true;gf_Mon.d1.bf.fb=true;}//PID制御フィードバック結果
-        else if (strcmp(g_CmdBuf , "mm1" ) == 0 ){gf_Mon.d1.bf.m1=true;}
-        else if (strcmp(g_CmdBuf , "mm2" ) == 0 ){gf_Mon.d1.bf.m2=true;}
-        else if (strcmp(g_CmdBuf , "mm3" ) == 0 ){gf_Mon.d1.bf.m3=true;}
-        else if (strcmp(g_CmdBuf , "mm4" ) == 0 ){gf_Mon.d1.bf.m4=true;}
-        else if (strcmp(g_CmdBuf , "msw" ) == 0 ){gf_Print.d2.bf.sw=true;gf_Mon.d2.bf.sw=true;}
-        else if (strcmp(g_CmdBuf , "mana1" ) == 0 ){gf_Print.d2.bf.ain=true;gf_Mon.d2.bf.ain=true;}
-        else if (strcmp(g_CmdBuf , "mmrpm" ) == 0 ){gf_Print.d2.bf.mrpm=true;gf_Mon.d2.bf.mrpm=true;}
-        else if (strcmp(g_CmdBuf , "kill")==0 ){gf_Mon.d1.flg=0;gf_Mon.d2.flg=0;}//モニタ停止
-        
-        // 開発、デバッグ用コマンド
-        else if (strcmp(g_CmdBuf , "stat")==0 ){gf_Print.d1.bf.stat=true;}//ステート表示
-        else if (strcmp(g_CmdBuf , "sstatf")==0 ){setStateF((enmHbState)atoi(&g_CmdBuf[arg2pos]));}//ステート強制遷移
-
-        else if (strcmp(g_CmdBuf , "sw0")==0 ){gf_SwCmd.bf.req=true;gf_SwCmd.bf.val=0;}//SW 0bit目をON
-        else if (strcmp(g_CmdBuf , "sw1")==0 ){gf_SwCmd.bf.req=true;gf_SwCmd.bf.val=1;}//SW 1bit目をON
-        else if (strcmp(g_CmdBuf , "sw2")==0 ){gf_SwCmd.bf.req=true;gf_SwCmd.bf.val=2;}//SW 2bit目をON
-        else if (strcmp(g_CmdBuf , "sw3")==0 ){gf_SwCmd.bf.req=true;gf_SwCmd.bf.val=3;}//SW 3bit目をON
-        else if (strcmp(g_CmdBuf , "sw4")==0 ){gf_SwCmd.bf.req=true;gf_SwCmd.bf.val=4;}//SW 4bit目をON
-        else if (strcmp(g_CmdBuf , "sw5")==0 ){gf_SwCmd.bf.req=true;gf_SwCmd.bf.val=5;}//SW 5bit目をON
-        else if (strcmp(g_CmdBuf , "sw6")==0 ){gf_SwCmd.bf.req=true;gf_SwCmd.bf.val=6;}//SW 6bit目をON
-        else if (strcmp(g_CmdBuf , "sw7")==0 ){gf_SwCmd.bf.req=true;gf_SwCmd.bf.val=7;}//SW 7bit目をON
-        else if (strcmp(g_CmdBuf , "sw8")==0 ){gf_SwCmd.bf.req=true;gf_SwCmd.bf.val=8;}//SW 8bit目をON
-        else if (strcmp(g_CmdBuf , "sw9")==0 ){gf_SwCmd.bf.req=true;gf_SwCmd.bf.val=9;}//SW 9bit目をON
+    }
+    else if (strcmp(g_CmdBuf , "smo1"   )==0 ){gf_MtReqOfs[0].req=true; gf_MtReqOfs[0].val=(UINT16)atoi(&g_CmdBuf[arg2pos]);}
+    else if (strcmp(g_CmdBuf , "smo2"   )==0 ){gf_MtReqOfs[1].req=true; gf_MtReqOfs[1].val=(UINT16)atoi(&g_CmdBuf[arg2pos]);}
+    else if (strcmp(g_CmdBuf , "smo3"   )==0 ){gf_MtReqOfs[2].req=true; gf_MtReqOfs[2].val=(UINT16)atoi(&g_CmdBuf[arg2pos]);}
+    else if (strcmp(g_CmdBuf , "smo4"   )==0 ){gf_MtReqOfs[3].req=true; gf_MtReqOfs[3].val=(UINT16)atoi(&g_CmdBuf[arg2pos]);}
+    else if (strcmp(g_CmdBuf , "smo"   )==0 ){
+        for(int i = 0; i < 4; ++i){
+            gf_MtReqOfs[i].req=true; 
+            gf_MtReqOfs[i].val=(UINT16)atoi(&g_CmdBuf[arg2pos]);
+        }
+    }
+    else if (strcmp(g_CmdBuf , "smof"   )==0 ){
+        gf_MtReqOfs[0].req=true;
+        gf_MtReqOfs[0].val=(UINT16)atoi(&g_CmdBuf[arg2pos]);
+        gf_MtReqOfs[1].req=true;
+        gf_MtReqOfs[1].val=(UINT16)atoi(&g_CmdBuf[arg2pos]);}
+    else if (strcmp(g_CmdBuf , "smor"   )==0 ){
+        gf_MtReqOfs[2].req=true;
+        gf_MtReqOfs[2].val=(UINT16)atoi(&g_CmdBuf[arg2pos]);
+        gf_MtReqOfs[3].req=true;
+        gf_MtReqOfs[3].val=(UINT16)atoi(&g_CmdBuf[arg2pos]);            }
+    
+    else if (strcmp(g_CmdBuf , "smbrk"   )==0 ){gf_MtBrk.req=true; gf_MtBrk.val=(UINT16)atoi(&g_CmdBuf[arg2pos]);}
 
-        else if (strcmp(g_CmdBuf , "stopmot")==0 ){gf_StopMot = true;}//モーター強制停止
-        
-        else if (strcmp(g_CmdBuf , "spidpp")==0 ){g_PidPara.PP = atof(&g_CmdBuf[arg2pos]);gf_PidParaUpdate=true;}//PIDの係数 Pp設定
-        else if (strcmp(g_CmdBuf , "spidp")==0 ){g_PidPara.P = atof(&g_CmdBuf[arg2pos]);gf_PidParaUpdate=true;}//PIDの係数 P設定
-        else if (strcmp(g_CmdBuf , "spidi")==0 ){g_PidPara.I = atof(&g_CmdBuf[arg2pos]);gf_PidParaUpdate=true;}//PIDの係数 I設定
-        else if (strcmp(g_CmdBuf , "spidd")==0 ){g_PidPara.D = atof(&g_CmdBuf[arg2pos]);gf_PidParaUpdate=true;}//PIDの係数 D設定
-        else if (strcmp(g_CmdBuf , "spidimax")==0 ){g_PidPara.IMax = atof(&g_CmdBuf[arg2pos]);gf_PidParaUpdate=true;}//PIDの係数 積分上限値設定
-        else if (strcmp(g_CmdBuf , "spidimin")==0 ){g_PidPara.IMin = atof(&g_CmdBuf[arg2pos]);gf_PidParaUpdate=true;}//PIDの係数 積分下限値設定
-        else if (strcmp(g_CmdBuf , "spidv")==0 ){g_PidPara.V = atof(&g_CmdBuf[arg2pos]);gf_PidParaUpdate=true;}//PID2の角速度の係数
-        else if (strcmp(g_CmdBuf , "spidmode" )==0 ){g_PidPara.mode = (ePIDMode)atoi(&g_CmdBuf[arg2pos]);gf_PidParaUpdate=true;}//PIDのモード 
-
-        else if (strcmp(g_CmdBuf , "rmo"  )==0 ){gf_Print.d2.bf.mo1=true;gf_Print.d2.bf.mo2=true;gf_Print.d2.bf.mo3=true;gf_Print.d2.bf.mo4=true;}//リードモーターオフセット
-        else if (strcmp(g_CmdBuf , "rmo1" )==0 ){gf_Print.d2.bf.mo1=true;}//リードモーターオフセット1 
-        else if (strcmp(g_CmdBuf , "rmo2" )==0 ){gf_Print.d2.bf.mo2=true;}//リードモーターオフセット2 
-        else if (strcmp(g_CmdBuf , "rmo3" )==0 ){gf_Print.d2.bf.mo3=true;}//リードモーターオフセット3 
-        else if (strcmp(g_CmdBuf , "rmo4" )==0 ){gf_Print.d2.bf.mo4=true;}//リードモーターオフセット4 
-        else if (strcmp(g_CmdBuf , "rana1" )==0 ){gf_Print.d2.bf.ain=true;}//アナログ入力リード forモーターアクセル 
-
+    //キャリブレーション
+    else if (strcmp(g_CmdBuf , "calyaw")==0 ){gf_Cal.bf.yaw=true;}//
+    else if (strcmp(g_CmdBuf , "calgyro")==0 ){gf_Cal.bf.gy=true;}//
+    else if (strcmp(g_CmdBuf , "calimu")==0 ){gf_Cal.bf.gy=true;gf_Cal.bf.yaw=true;}//
+    
+    //チェック系
 
-               
-        //何にも引っ掛からない
-        else {sp.printf("command missmatch\r\n"); };
-    } else if(gf_State == SLEEP){
-        //EMG STOP 現在はモーターの強制停止
-        if (g_CmdBuf[0] == ESC){gf_StopMot = true;}
-        else if (strcmp(g_CmdBuf , "sarm"   )==0 ){gf_Armed = true;}//アーミングフラグセット テスト用
-        else if (strcmp(g_CmdBuf , "dbg"   )==0 ){if(gf_Dbg != true) gf_Dbg = true;}//デバッグタスク起動
-        else if (strcmp(g_CmdBuf , "rs"  )==0 ){gf_Print.d2.bf.sw=true;}//リードスイッチ
- 
-        else if (strcmp(g_CmdBuf , "stat")==0 ){gf_Print.d1.bf.stat=true;}//ステート表示
-        else if (strcmp(g_CmdBuf , "sstatf")==0 ){setStateF((enmHbState)atoi(&g_CmdBuf[arg2pos]));}//ステート強制遷移
-
-        else if (strcmp(g_CmdBuf , "re"  )==0 ){gf_Print.d1.bf.e1=true; gf_Print.d1.bf.e2=true;}
-        else if (strcmp(g_CmdBuf , "re1" )==0 ){gf_Print.d1.bf.e1=true;}
-        else if (strcmp(g_CmdBuf , "re2" )==0 ){gf_Print.d1.bf.e2=true;}
-
-        else if (strcmp(g_CmdBuf , "sw0")==0 ){gf_SwCmd.bf.req=true;gf_SwCmd.bf.val=0;}//SW 0bit目をON
-        else if (strcmp(g_CmdBuf , "sw1")==0 ){gf_SwCmd.bf.req=true;gf_SwCmd.bf.val=1;}//SW 1bit目をON
-        else if (strcmp(g_CmdBuf , "sw2")==0 ){gf_SwCmd.bf.req=true;gf_SwCmd.bf.val=2;}//SW 2bit目をON
-        else if (strcmp(g_CmdBuf , "sw3")==0 ){gf_SwCmd.bf.req=true;gf_SwCmd.bf.val=3;}//SW 3bit目をON
-        else if (strcmp(g_CmdBuf , "sw4")==0 ){gf_SwCmd.bf.req=true;gf_SwCmd.bf.val=4;}//SW 4bit目をON
-        else if (strcmp(g_CmdBuf , "sw5")==0 ){gf_SwCmd.bf.req=true;gf_SwCmd.bf.val=5;}//SW 5bit目をON
-        else if (strcmp(g_CmdBuf , "sw6")==0 ){gf_SwCmd.bf.req=true;gf_SwCmd.bf.val=6;}//SW 6bit目をON
-        else if (strcmp(g_CmdBuf , "sw7")==0 ){gf_SwCmd.bf.req=true;gf_SwCmd.bf.val=7;}//SW 7bit目をON
-        else if (strcmp(g_CmdBuf , "sw8")==0 ){gf_SwCmd.bf.req=true;gf_SwCmd.bf.val=8;}//SW 8bit目をON
-        else if (strcmp(g_CmdBuf , "sw9")==0 ){gf_SwCmd.bf.req=true;gf_SwCmd.bf.val=9;}//SW 9bit目をON
-
-        else if (strcmp(g_CmdBuf , "ses1"   )==0 ){gf_AxStepReq[0].bf.req=true; gf_AxStepReq[0].bf.val=(UINT16)atoi(&g_CmdBuf[arg2pos]);}
-        else if (strcmp(g_CmdBuf , "ses2"   )==0 ){gf_AxStepReq[1].bf.req=true; gf_AxStepReq[1].bf.val=(UINT16)atoi(&g_CmdBuf[arg2pos]);}
-        else if (strcmp(g_CmdBuf , "seas"   )==0 ){ gf_AxAdjStepReq[0].bf.req=true; gf_AxAdjStepReq[0].bf.val=(UINT16)atoi(&g_CmdBuf[arg2pos]);
-                                                    gf_AxAdjStepReq[1].bf.req=true; gf_AxAdjStepReq[1].bf.val=(UINT16)atoi(&g_CmdBuf[arg2pos]);}
-        else if (strcmp(g_CmdBuf , "seas1"   )==0 ){gf_AxAdjStepReq[0].bf.req=true; gf_AxAdjStepReq[0].bf.val=(UINT16)atoi(&g_CmdBuf[arg2pos]);}
-        else if (strcmp(g_CmdBuf , "seas2"   )==0 ){gf_AxAdjStepReq[1].bf.req=true; gf_AxAdjStepReq[1].bf.val=(UINT16)atoi(&g_CmdBuf[arg2pos]);}
+    //連続モニタ
+    else if (strcmp(g_CmdBuf , "mg"  )==0 ){gf_Print.d1.bf.gy=true;gf_Mon.d1.bf.gy=true;}//リードジャイロ  
+    else if (strcmp(g_CmdBuf , "my"  )==0 ){gf_Print.d1.bf.yaw=true;gf_Mon.d1.bf.yaw=true;}//リード姿勢角ヨー
+    else if (strcmp(g_CmdBuf , "mimu")==0 ){gf_Print.d1.bf.yaw=true;gf_Print.d1.bf.gy=true;gf_Mon.d1.bf.yaw=true;gf_Mon.d1.bf.gy=true;}//リードIMU
+    else if (strcmp(g_CmdBuf , "mfb" )==0 ){gf_Print.d1.bf.fb=true;gf_Mon.d1.bf.fb=true;}//PID制御フィードバック結果
+    else if (strcmp(g_CmdBuf , "mm1" ) == 0 ){gf_Mon.d1.bf.m1=true;}
+    else if (strcmp(g_CmdBuf , "mm2" ) == 0 ){gf_Mon.d1.bf.m2=true;}
+    else if (strcmp(g_CmdBuf , "mm3" ) == 0 ){gf_Mon.d1.bf.m3=true;}
+    else if (strcmp(g_CmdBuf , "mm4" ) == 0 ){gf_Mon.d1.bf.m4=true;}
+    else if (strcmp(g_CmdBuf , "msw" ) == 0 ){gf_Print.d2.bf.sw=true;gf_Mon.d2.bf.sw=true;}
+    else if (strcmp(g_CmdBuf , "mana1" ) == 0 ){gf_Print.d2.bf.ain=true;gf_Mon.d2.bf.ain=true;}
+    else if (strcmp(g_CmdBuf , "mmrpm" ) == 0 ){gf_Print.d2.bf.mrpm=true;gf_Mon.d2.bf.mrpm=true;}
+    else if (strcmp(g_CmdBuf , "kill")==0 ){gf_Mon.d1.flg=0;gf_Mon.d2.flg=0;}//モニタ停止
+    
+    // 開発、デバッグ用コマンド
+    else if (strcmp(g_CmdBuf , "stat")==0 ){gf_Print.d1.bf.stat=true;}//ステート表示
+    else if (strcmp(g_CmdBuf , "sstatf")==0 ){setStateF((enmHbState)atoi(&g_CmdBuf[arg2pos]));}//ステート強制遷移
 
-        else if (strcmp(g_CmdBuf , "sm1"   )==0 ){gf_MtReq[0].req=true; gf_MtReq[0].val=(UINT16)atoi(&g_CmdBuf[arg2pos]);}
-        else if (strcmp(g_CmdBuf , "sm2"   )==0 ){gf_MtReq[1].req=true; gf_MtReq[1].val=(UINT16)atoi(&g_CmdBuf[arg2pos]);}
-        else if (strcmp(g_CmdBuf , "sm3"   )==0 ){gf_MtReq[2].req=true; gf_MtReq[2].val=(UINT16)atoi(&g_CmdBuf[arg2pos]);}
-        else if (strcmp(g_CmdBuf , "sm4"   )==0 ){gf_MtReq[3].req=true; gf_MtReq[3].val=(UINT16)atoi(&g_CmdBuf[arg2pos]);}
-        else if (strcmp(g_CmdBuf , "sm"   )==0 ){
-            for(int i = 0; i < 4; ++i){
-                gf_MtReq[i].req=true; 
-                gf_MtReq[i].val=(UINT16)atoi(&g_CmdBuf[arg2pos]);
-            }
-            }
-        else if (strcmp(g_CmdBuf , "smf"   )==0 ){
-            gf_MtReq[0].req=true;
-            gf_MtReq[0].val=(UINT16)atoi(&g_CmdBuf[arg2pos]);
-            gf_MtReq[1].req=true;
-            gf_MtReq[1].val=(UINT16)atoi(&g_CmdBuf[arg2pos]);}
-        else if (strcmp(g_CmdBuf , "smr"   )==0 ){
-            gf_MtReq[2].req=true;
-            gf_MtReq[2].val=(UINT16)atoi(&g_CmdBuf[arg2pos]);
-            gf_MtReq[3].req=true;
-            gf_MtReq[3].val=(UINT16)atoi(&g_CmdBuf[arg2pos]);            }
-        else if (strcmp(g_CmdBuf , "smo1"   )==0 ){gf_MtReqOfs[0].req=true; gf_MtReqOfs[0].val=(UINT16)atoi(&g_CmdBuf[arg2pos]);}
-        else if (strcmp(g_CmdBuf , "smo2"   )==0 ){gf_MtReqOfs[1].req=true; gf_MtReqOfs[1].val=(UINT16)atoi(&g_CmdBuf[arg2pos]);}
-        else if (strcmp(g_CmdBuf , "smo3"   )==0 ){gf_MtReqOfs[2].req=true; gf_MtReqOfs[2].val=(UINT16)atoi(&g_CmdBuf[arg2pos]);}
-        else if (strcmp(g_CmdBuf , "smo4"   )==0 ){gf_MtReqOfs[3].req=true; gf_MtReqOfs[3].val=(UINT16)atoi(&g_CmdBuf[arg2pos]);}
-        else if (strcmp(g_CmdBuf , "smo"   )==0 ){
-            for(int i = 0; i < 4; ++i){
-                gf_MtReqOfs[i].req=true; 
-                gf_MtReqOfs[i].val=(UINT16)atoi(&g_CmdBuf[arg2pos]);
-            }
-        }
-        else if (strcmp(g_CmdBuf , "smof"   )==0 ){
-            gf_MtReqOfs[0].req=true;
-            gf_MtReqOfs[0].val=(UINT16)atoi(&g_CmdBuf[arg2pos]);
-            gf_MtReqOfs[1].req=true;
-            gf_MtReqOfs[1].val=(UINT16)atoi(&g_CmdBuf[arg2pos]);}
-        else if (strcmp(g_CmdBuf , "smor"   )==0 ){
-            gf_MtReqOfs[2].req=true;
-            gf_MtReqOfs[2].val=(UINT16)atoi(&g_CmdBuf[arg2pos]);
-            gf_MtReqOfs[3].req=true;
-            gf_MtReqOfs[3].val=(UINT16)atoi(&g_CmdBuf[arg2pos]);}
-        else if (strcmp(g_CmdBuf , "smao1"   )==0 ){gf_MtAttOfs[0].req=true; gf_MtAttOfs[0].val=(UINT16)atoi(&g_CmdBuf[arg2pos]);}//!< Set Motor Attitude Offset 1
-        else if (strcmp(g_CmdBuf , "smao2"   )==0 ){gf_MtAttOfs[1].req=true; gf_MtAttOfs[1].val=(UINT16)atoi(&g_CmdBuf[arg2pos]);}//!< Set Motor Attitude Offset 2
-        else if (strcmp(g_CmdBuf , "smao3"   )==0 ){gf_MtAttOfs[2].req=true; gf_MtAttOfs[2].val=(UINT16)atoi(&g_CmdBuf[arg2pos]);}//!< Set Motor Attitude Offset 3
-        else if (strcmp(g_CmdBuf , "smao4"   )==0 ){gf_MtAttOfs[3].req=true; gf_MtAttOfs[3].val=(UINT16)atoi(&g_CmdBuf[arg2pos]);}//!< Set Motor Attitude Offset 4
-        else if (strcmp(g_CmdBuf , "smao"   )==0 ){//!< Set Motor Attitude Offset
-            for(int i = 0; i < 4; ++i){
-                gf_MtAttOfs[i].req=true; 
-                gf_MtAttOfs[i].val=(UINT16)atoi(&g_CmdBuf[arg2pos]);
-            }
-        }
-        else if (strcmp(g_CmdBuf , "smfpga1"   )==0 ){gf_MtReqDct[0].req=true; gf_MtReqDct[0].val=(UINT16)atoi(&g_CmdBuf[arg2pos]);}
-        else if (strcmp(g_CmdBuf , "smfpga2"   )==0 ){gf_MtReqDct[1].req=true; gf_MtReqDct[1].val=(UINT16)atoi(&g_CmdBuf[arg2pos]);}
-        else if (strcmp(g_CmdBuf , "smfpga3"   )==0 ){gf_MtReqDct[2].req=true; gf_MtReqDct[2].val=(UINT16)atoi(&g_CmdBuf[arg2pos]);}
-        else if (strcmp(g_CmdBuf , "smfpga4"   )==0 ){gf_MtReqDct[3].req=true; gf_MtReqDct[3].val=(UINT16)atoi(&g_CmdBuf[arg2pos]);}
-        else if (strcmp(g_CmdBuf , "smfpga5"   )==0 ){gf_MtReqDct[4].req=true; gf_MtReqDct[4].val=(UINT16)atoi(&g_CmdBuf[arg2pos]);}
-        else if (strcmp(g_CmdBuf , "smfpga6"   )==0 ){gf_MtReqDct[5].req=true; gf_MtReqDct[5].val=(UINT16)atoi(&g_CmdBuf[arg2pos]);}
-        else if (strcmp(g_CmdBuf , "smfpga7"   )==0 ){gf_MtReqDct[6].req=true; gf_MtReqDct[6].val=(UINT16)atoi(&g_CmdBuf[arg2pos]);}
-        else if (strcmp(g_CmdBuf , "smfpga8"   )==0 ){gf_MtReqDct[7].req=true; gf_MtReqDct[7].val=(UINT16)atoi(&g_CmdBuf[arg2pos]);}
-        
-        //キャリブレーション
-        else if (strcmp(g_CmdBuf , "calyaw")==0 ){gf_Cal.bf.yaw=true;}//
-        else if (strcmp(g_CmdBuf , "calgyro")==0 ){gf_Cal.bf.gy=true;}//
-        else if (strcmp(g_CmdBuf , "calimu")==0 ){gf_Cal.bf.gy=true;gf_Cal.bf.yaw=true;}//
+    else if (strcmp(g_CmdBuf , "sw0")==0 ){gf_SwCmd.bf.req=true;gf_SwCmd.bf.val=0;}//SW 0bit目をON
+    else if (strcmp(g_CmdBuf , "sw1")==0 ){gf_SwCmd.bf.req=true;gf_SwCmd.bf.val=1;}//SW 1bit目をON
+    else if (strcmp(g_CmdBuf , "sw2")==0 ){gf_SwCmd.bf.req=true;gf_SwCmd.bf.val=2;}//SW 2bit目をON
+    else if (strcmp(g_CmdBuf , "sw3")==0 ){gf_SwCmd.bf.req=true;gf_SwCmd.bf.val=3;}//SW 3bit目をON
+    else if (strcmp(g_CmdBuf , "sw4")==0 ){gf_SwCmd.bf.req=true;gf_SwCmd.bf.val=4;}//SW 4bit目をON
+    else if (strcmp(g_CmdBuf , "sw5")==0 ){gf_SwCmd.bf.req=true;gf_SwCmd.bf.val=5;}//SW 5bit目をON
+    else if (strcmp(g_CmdBuf , "sw6")==0 ){gf_SwCmd.bf.req=true;gf_SwCmd.bf.val=6;}//SW 6bit目をON
+    else if (strcmp(g_CmdBuf , "sw7")==0 ){gf_SwCmd.bf.req=true;gf_SwCmd.bf.val=7;}//SW 7bit目をON
+    else if (strcmp(g_CmdBuf , "sw8")==0 ){gf_SwCmd.bf.req=true;gf_SwCmd.bf.val=8;}//SW 8bit目をON
+    else if (strcmp(g_CmdBuf , "sw9")==0 ){gf_SwCmd.bf.req=true;gf_SwCmd.bf.val=9;}//SW 9bit目をON
 
-        else if (strcmp(g_CmdBuf , "spidpp")==0 ){g_PidPara.PP = atof(&g_CmdBuf[arg2pos]);gf_PidParaUpdate=true;}//PIDの係数 Pp設定
-        else if (strcmp(g_CmdBuf , "spidp")==0 ){g_PidPara.P = atof(&g_CmdBuf[arg2pos]);gf_PidParaUpdate=true;}//PIDの係数 P設定
-        else if (strcmp(g_CmdBuf , "spidi")==0 ){g_PidPara.I = atof(&g_CmdBuf[arg2pos]);gf_PidParaUpdate=true;}//PIDの係数 I設定
-        else if (strcmp(g_CmdBuf , "spidd")==0 ){g_PidPara.D = atof(&g_CmdBuf[arg2pos]);gf_PidParaUpdate=true;}//PIDの係数 D設定
-        else if (strcmp(g_CmdBuf , "spidimax")==0 ){g_PidPara.IMax = atof(&g_CmdBuf[arg2pos]);gf_PidParaUpdate=true;}//PIDの係数 積分上限値設定
-        else if (strcmp(g_CmdBuf , "spidimin")==0 ){g_PidPara.IMin = atof(&g_CmdBuf[arg2pos]);gf_PidParaUpdate=true;}//PIDの係数 積分下限値設定
-        else if (strcmp(g_CmdBuf , "spidv")==0 ){g_PidPara.V = atof(&g_CmdBuf[arg2pos]);gf_PidParaUpdate=true;}//PID2の角速度の係数
-        else if (strcmp(g_CmdBuf , "spidmode" )==0 ){g_PidPara.mode = (ePIDMode)atoi(&g_CmdBuf[arg2pos]);gf_PidParaUpdate=true;}//PIDのモード 
+    else if (strcmp(g_CmdBuf , "stopmot")==0 ){gf_StopMot = true;}//モーター強制停止
+    
+    else if (strcmp(g_CmdBuf , "spidpp")==0 ){g_PidPara.PP = atof(&g_CmdBuf[arg2pos]);gf_PidParaUpdate=true;}//PIDの係数 Pp設定
+    else if (strcmp(g_CmdBuf , "spidp")==0 ){g_PidPara.P = atof(&g_CmdBuf[arg2pos]);gf_PidParaUpdate=true;}//PIDの係数 P設定
+    else if (strcmp(g_CmdBuf , "spidi")==0 ){g_PidPara.I = atof(&g_CmdBuf[arg2pos]);gf_PidParaUpdate=true;}//PIDの係数 I設定
+    else if (strcmp(g_CmdBuf , "spidd")==0 ){g_PidPara.D = atof(&g_CmdBuf[arg2pos]);gf_PidParaUpdate=true;}//PIDの係数 D設定
+    else if (strcmp(g_CmdBuf , "spidimax")==0 ){g_PidPara.IMax = atof(&g_CmdBuf[arg2pos]);gf_PidParaUpdate=true;}//PIDの係数 積分上限値設定
+    else if (strcmp(g_CmdBuf , "spidimin")==0 ){g_PidPara.IMin = atof(&g_CmdBuf[arg2pos]);gf_PidParaUpdate=true;}//PIDの係数 積分下限値設定
+    else if (strcmp(g_CmdBuf , "spidv")==0 ){g_PidPara.V = atof(&g_CmdBuf[arg2pos]);gf_PidParaUpdate=true;}//PID2の角速度の係数
+    else if (strcmp(g_CmdBuf , "spidmode" )==0 ){g_PidPara.mode = (ePIDMode)atoi(&g_CmdBuf[arg2pos]);gf_PidParaUpdate=true;}//PIDのモード 
 
-        else if (strcmp(g_CmdBuf , "rana1" )==0 ){gf_Print.d2.bf.ain=true;}//アナログ入力リード forモーターアクセル 
+    else if (strcmp(g_CmdBuf , "rmo"  )==0 ){gf_Print.d2.bf.mo1=true;gf_Print.d2.bf.mo2=true;gf_Print.d2.bf.mo3=true;gf_Print.d2.bf.mo4=true;}//リードモーターオフセット
+    else if (strcmp(g_CmdBuf , "rmo1" )==0 ){gf_Print.d2.bf.mo1=true;}//リードモーターオフセット1 
+    else if (strcmp(g_CmdBuf , "rmo2" )==0 ){gf_Print.d2.bf.mo2=true;}//リードモーターオフセット2 
+    else if (strcmp(g_CmdBuf , "rmo3" )==0 ){gf_Print.d2.bf.mo3=true;}//リードモーターオフセット3 
+    else if (strcmp(g_CmdBuf , "rmo4" )==0 ){gf_Print.d2.bf.mo4=true;}//リードモーターオフセット4 
+    else if (strcmp(g_CmdBuf , "rana1" )==0 ){gf_Print.d2.bf.ain=true;}//アナログ入力リード forモーターアクセル 
 
-        else if (strcmp(g_CmdBuf , "msw" ) == 0 ){gf_Print.d2.bf.sw=true;gf_Mon.d2.bf.sw=true;}
-        else if (strcmp(g_CmdBuf , "mana1" ) == 0 ){gf_Print.d2.bf.ain=true;gf_Mon.d2.bf.ain=true;}
-        else if (strcmp(g_CmdBuf , "kill")==0 ){gf_Mon.d1.flg=0;gf_Mon.d2.flg=0;}//モニタ停止
-        //何にも引っ掛からない
-        else {sp.printf("command missmatch\r\n"); };
-    }
+    //何にも引っ掛からない
+    else {sp.printf("command missmatch\r\n"); };
     
     //バッファクリア
     memset(&g_CmdBuf[0] , 0x00 , sizeof(g_CmdBuf) );