teamALI / Mbed 2 deprecated HB2018

Dependencies:   mbed FreeRTOS

Revision:
22:24c9c2dedca9
Parent:
21:78302ecdb661
Child:
23:79e20be4bc5b
--- a/hbCommand.cpp	Thu Dec 06 01:50:17 2018 +0000
+++ b/hbCommand.cpp	Thu Dec 06 11:03:13 2018 +0000
@@ -25,53 +25,105 @@
         }
     }
     
-    //リード系
-         if (strcmp(g_CmdBuf , "re"  )==0 ){gf_Print.bf.e1=true; gf_Print.bf.e2=true;}
-    else if (strcmp(g_CmdBuf , "re1" )==0 ){gf_Print.bf.e1=true;}
-    else if (strcmp(g_CmdBuf , "re2" )==0 ){gf_Print.bf.e2=true;}
-    else if (strcmp(g_CmdBuf , "rm"  )==0 ){gf_Print.bf.m1=true;gf_Print.bf.m2=true;gf_Print.bf.m3=true;gf_Print.bf.m4=true;}//リードモーター  
-    else if (strcmp(g_CmdBuf , "rm1" )==0 ){gf_Print.bf.m1=true;}//リードモーター1 
-    else if (strcmp(g_CmdBuf , "rm2" )==0 ){gf_Print.bf.m2=true;}//リードモーター2 
-    else if (strcmp(g_CmdBuf , "rm3" )==0 ){gf_Print.bf.m3=true;}//リードモーター3 
-    else if (strcmp(g_CmdBuf , "rm4" )==0 ){gf_Print.bf.m4=true;}//リードモーター4 
-    else if (strcmp(g_CmdBuf , "rg"  )==0 ){gf_Print.bf.gy=true;}//リードジャイロ  
-    else if (strcmp(g_CmdBuf , "ry"  )==0 ){gf_Print.bf.yaw=true;}//リード姿勢角ヨー
-    else if (strcmp(g_CmdBuf , "rimu")==0 ){gf_Print.bf.yaw=true;gf_Print.bf.gy=true;}//リードIMU
-    else if (strcmp(g_CmdBuf , "rs"  )==0 ){ }//リードスイッチ
-    else if (strcmp(g_CmdBuf , "rfb" )==0 ){gf_Print.bf.fb=true;}//PID制御フィードバック結果
-    else if (strcmp(g_CmdBuf , "rp"  )==0 ){gf_Print.bf.pp=true;gf_Print.bf.p=true;gf_Print.bf.i=true;gf_Print.bf.d=true;}//パラメータ全表示
-    else if (strcmp(g_CmdBuf , "rpP" )==0 ){gf_Print.bf.pp=true;}//パラメータアウターPゲイン表示    
-    else if (strcmp(g_CmdBuf , "rpp" )==0 ){gf_Print.bf.p=true;}//パラメータPゲイン表示 
-    else if (strcmp(g_CmdBuf , "rpi" )==0 ){gf_Print.bf.i=true;}//パラメータIゲイン表示 
-    else if (strcmp(g_CmdBuf , "rpd" )==0 ){gf_Print.bf.d=true;}//パラメータDゲイン表示
+    if(gf_State != SLEEP && gf_State != WAKEUP){
+        //リード系
+            if (strcmp(g_CmdBuf , "re"  )==0 ){gf_Print.bf.e1=true; gf_Print.bf.e2=true;}
+        else if (strcmp(g_CmdBuf , "re1" )==0 ){gf_Print.bf.e1=true;}
+        else if (strcmp(g_CmdBuf , "re2" )==0 ){gf_Print.bf.e2=true;}
+        else if (strcmp(g_CmdBuf , "rm"  )==0 ){gf_Print.bf.m1=true;gf_Print.bf.m2=true;gf_Print.bf.m3=true;gf_Print.bf.m4=true;}//リードモーター  
+        else if (strcmp(g_CmdBuf , "rm1" )==0 ){gf_Print.bf.m1=true;}//リードモーター1 
+        else if (strcmp(g_CmdBuf , "rm2" )==0 ){gf_Print.bf.m2=true;}//リードモーター2 
+        else if (strcmp(g_CmdBuf , "rm3" )==0 ){gf_Print.bf.m3=true;}//リードモーター3 
+        else if (strcmp(g_CmdBuf , "rm4" )==0 ){gf_Print.bf.m4=true;}//リードモーター4 
+        else if (strcmp(g_CmdBuf , "rg"  )==0 ){gf_Print.bf.gy=true;}//リードジャイロ  
+        else if (strcmp(g_CmdBuf , "ry"  )==0 ){gf_Print.bf.yaw=true;}//リード姿勢角ヨー
+        else if (strcmp(g_CmdBuf , "rimu")==0 ){gf_Print.bf.yaw=true;gf_Print.bf.gy=true;}//リードIMU
+        else if (strcmp(g_CmdBuf , "rs"  )==0 ){ }//リードスイッチ
+        else if (strcmp(g_CmdBuf , "rfb" )==0 ){gf_Print.bf.fb=true;}//PID制御フィードバック結果
+        else if (strcmp(g_CmdBuf , "rp"  )==0 ){gf_Print.bf.pp=true;gf_Print.bf.p=true;gf_Print.bf.i=true;gf_Print.bf.d=true;}//パラメータ全表示
+        else if (strcmp(g_CmdBuf , "rpP" )==0 ){gf_Print.bf.pp=true;}//パラメータアウターPゲイン表示    
+        else if (strcmp(g_CmdBuf , "rpp" )==0 ){gf_Print.bf.p=true;}//パラメータPゲイン表示 
+        else if (strcmp(g_CmdBuf , "rpi" )==0 ){gf_Print.bf.i=true;}//パラメータIゲイン表示 
+        else if (strcmp(g_CmdBuf , "rpd" )==0 ){gf_Print.bf.d=true;}//パラメータDゲイン表示
+        
+        //設定系
+        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 , "sm1"   )==0 ){gf_MtReq[0].bf.req=true; gf_MtReq[0].bf.val=(UINT16)atoi(&g_CmdBuf[arg2pos]);}
+        else if (strcmp(g_CmdBuf , "sm2"   )==0 ){gf_MtReq[1].bf.req=true; gf_MtReq[1].bf.val=(UINT16)atoi(&g_CmdBuf[arg2pos]);}
+        else if (strcmp(g_CmdBuf , "sm3"   )==0 ){gf_MtReq[2].bf.req=true; gf_MtReq[2].bf.val=(UINT16)atoi(&g_CmdBuf[arg2pos]);}
+        else if (strcmp(g_CmdBuf , "sm4"   )==0 ){gf_MtReq[3].bf.req=true; gf_MtReq[3].bf.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.bf.gy=true;gf_Mon.bf.gy=true;}//リードジャイロ  
+        else if (strcmp(g_CmdBuf , "my"  )==0 ){gf_Print.bf.yaw=true;gf_Mon.bf.yaw=true;}//リード姿勢角ヨー
+        else if (strcmp(g_CmdBuf , "mimu")==0 ){gf_Print.bf.yaw=true;gf_Print.bf.gy=true;gf_Mon.bf.yaw=true;gf_Mon.bf.gy=true;}//リードIMU
+        else if (strcmp(g_CmdBuf , "mfb" )==0 ){gf_Print.bf.fb=true;gf_Mon.bf.fb=true;}//PID制御フィードバック結果
+        else if (strcmp(g_CmdBuf , "kill")==0 ){gf_Mon.flg=0;}//モニタ停止
+        
+        //何にも引っ掛からない
+        else {sp.printf("command missmatch\r\n"); };
+    } else if(gf_State == SLEEP){
+            if (strcmp(g_CmdBuf , "sarm"   )==0 ){gf_Armed = true;}//アーミングフラグセット テスト用
+        else if (strcmp(g_CmdBuf , "dbg"   )==0 ){if(gf_Dbg != true) gf_Dbg = true;}//デバッグモード
+        //何にも引っ掛からない
+        else {sp.printf("command missmatch\r\n"); };
+    }
+//     //リード系
+//          if (strcmp(g_CmdBuf , "re"  )==0 ){gf_Print.bf.e1=true; gf_Print.bf.e2=true;}
+//     else if (strcmp(g_CmdBuf , "re1" )==0 ){gf_Print.bf.e1=true;}
+//     else if (strcmp(g_CmdBuf , "re2" )==0 ){gf_Print.bf.e2=true;}
+//     else if (strcmp(g_CmdBuf , "rm"  )==0 ){gf_Print.bf.m1=true;gf_Print.bf.m2=true;gf_Print.bf.m3=true;gf_Print.bf.m4=true;}//リードモーター  
+//     else if (strcmp(g_CmdBuf , "rm1" )==0 ){gf_Print.bf.m1=true;}//リードモーター1 
+//     else if (strcmp(g_CmdBuf , "rm2" )==0 ){gf_Print.bf.m2=true;}//リードモーター2 
+//     else if (strcmp(g_CmdBuf , "rm3" )==0 ){gf_Print.bf.m3=true;}//リードモーター3 
+//     else if (strcmp(g_CmdBuf , "rm4" )==0 ){gf_Print.bf.m4=true;}//リードモーター4 
+//     else if (strcmp(g_CmdBuf , "rg"  )==0 ){gf_Print.bf.gy=true;}//リードジャイロ  
+//     else if (strcmp(g_CmdBuf , "ry"  )==0 ){gf_Print.bf.yaw=true;}//リード姿勢角ヨー
+//     else if (strcmp(g_CmdBuf , "rimu")==0 ){gf_Print.bf.yaw=true;gf_Print.bf.gy=true;}//リードIMU
+//     else if (strcmp(g_CmdBuf , "rs"  )==0 ){ }//リードスイッチ
+//     else if (strcmp(g_CmdBuf , "rfb" )==0 ){gf_Print.bf.fb=true;}//PID制御フィードバック結果
+//     else if (strcmp(g_CmdBuf , "rp"  )==0 ){gf_Print.bf.pp=true;gf_Print.bf.p=true;gf_Print.bf.i=true;gf_Print.bf.d=true;}//パラメータ全表示
+//     else if (strcmp(g_CmdBuf , "rpP" )==0 ){gf_Print.bf.pp=true;}//パラメータアウターPゲイン表示    
+//     else if (strcmp(g_CmdBuf , "rpp" )==0 ){gf_Print.bf.p=true;}//パラメータPゲイン表示 
+//     else if (strcmp(g_CmdBuf , "rpi" )==0 ){gf_Print.bf.i=true;}//パラメータIゲイン表示 
+//     else if (strcmp(g_CmdBuf , "rpd" )==0 ){gf_Print.bf.d=true;}//パラメータDゲイン表示
     
-    //設定系
-    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 , "sm1"   )==0 ){gf_MtReq[0].bf.req=true; gf_MtReq[0].bf.val=(UINT16)atoi(&g_CmdBuf[arg2pos]);}
-    else if (strcmp(g_CmdBuf , "sm2"   )==0 ){gf_MtReq[1].bf.req=true; gf_MtReq[1].bf.val=(UINT16)atoi(&g_CmdBuf[arg2pos]);}
-    else if (strcmp(g_CmdBuf , "sm3"   )==0 ){gf_MtReq[2].bf.req=true; gf_MtReq[2].bf.val=(UINT16)atoi(&g_CmdBuf[arg2pos]);}
-    else if (strcmp(g_CmdBuf , "sm4"   )==0 ){gf_MtReq[3].bf.req=true; gf_MtReq[3].bf.val=(UINT16)atoi(&g_CmdBuf[arg2pos]);}
+//     //設定系
+//     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 , "sm1"   )==0 ){gf_MtReq[0].bf.req=true; gf_MtReq[0].bf.val=(UINT16)atoi(&g_CmdBuf[arg2pos]);}
+//     else if (strcmp(g_CmdBuf , "sm2"   )==0 ){gf_MtReq[1].bf.req=true; gf_MtReq[1].bf.val=(UINT16)atoi(&g_CmdBuf[arg2pos]);}
+//     else if (strcmp(g_CmdBuf , "sm3"   )==0 ){gf_MtReq[2].bf.req=true; gf_MtReq[2].bf.val=(UINT16)atoi(&g_CmdBuf[arg2pos]);}
+//     else if (strcmp(g_CmdBuf , "sm4"   )==0 ){gf_MtReq[3].bf.req=true; gf_MtReq[3].bf.val=(UINT16)atoi(&g_CmdBuf[arg2pos]);}
+//     else if (strcmp(g_CmdBuf , "sarm"   )==0 ){gf_Armed = true;}//アーミングフラグセット
     
-    //
-//    else if (strcmp(g_CmdBuf , "dbg"   )==0 ){}
+//     //
+// //    else if (strcmp(g_CmdBuf , "dbg"   )==0 ){}
     
-    //キャリブレーション
-    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 , "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.bf.gy=true;gf_Mon.bf.gy=true;}//リードジャイロ  
-    else if (strcmp(g_CmdBuf , "my"  )==0 ){gf_Print.bf.yaw=true;gf_Mon.bf.yaw=true;}//リード姿勢角ヨー
-    else if (strcmp(g_CmdBuf , "mimu")==0 ){gf_Print.bf.yaw=true;gf_Print.bf.gy=true;gf_Mon.bf.yaw=true;gf_Mon.bf.gy=true;}//リードIMU
-    else if (strcmp(g_CmdBuf , "mfb" )==0 ){gf_Print.bf.fb=true;gf_Mon.bf.fb=true;}//PID制御フィードバック結果
-    else if (strcmp(g_CmdBuf , "kill")==0 ){gf_Mon.flg=0;}//モニタ停止
+//     //連続モニタ
+//     else if (strcmp(g_CmdBuf , "mg"  )==0 ){gf_Print.bf.gy=true;gf_Mon.bf.gy=true;}//リードジャイロ  
+//     else if (strcmp(g_CmdBuf , "my"  )==0 ){gf_Print.bf.yaw=true;gf_Mon.bf.yaw=true;}//リード姿勢角ヨー
+//     else if (strcmp(g_CmdBuf , "mimu")==0 ){gf_Print.bf.yaw=true;gf_Print.bf.gy=true;gf_Mon.bf.yaw=true;gf_Mon.bf.gy=true;}//リードIMU
+//     else if (strcmp(g_CmdBuf , "mfb" )==0 ){gf_Print.bf.fb=true;gf_Mon.bf.fb=true;}//PID制御フィードバック結果
+//     else if (strcmp(g_CmdBuf , "kill")==0 ){gf_Mon.flg=0;}//モニタ停止
     
-    //何にも引っ掛からない
-    else {sp.printf("command missmatch\r\n"); };
+//     //何にも引っ掛からない
+//     else {sp.printf("command missmatch\r\n"); };
     
     //バッファクリア
     memset(&g_CmdBuf[0] , 0x00 , sizeof(g_CmdBuf) );