Diff: hbCommand.cpp

Revision:

25:f3a6e7eec9c3

Parent:

24:c5945aaae777

Child:

26:732bc37fbefd

--- a/hbCommand.cpp	Mon Dec 10 12:29:37 2018 +0000
+++ b/hbCommand.cpp	Wed Dec 12 23:52:22 2018 +0000
@@ -1,5 +1,6 @@
 #include <string.h>
 #include <stdio.h>
+#include "typedef.h"
 #include "globalFlags.h"
 #include "hbCommand.h"
 
@@ -25,9 +26,12 @@
         }
     }
     
-    if(gf_State != SLEEP && gf_State != WAKEUP){
+    //if(gf_State != SLEEP && gf_State != WAKEUP){
+    if(gf_State != SLEEP){
+        //EMG STOP 現在はモーターの強制停止
+        if (g_CmdBuf[0] == ESC){gf_StopMot = true;}
         //リード系
-            if (strcmp(g_CmdBuf , "re"  )==0 ){gf_Print.bf.e1=true; gf_Print.bf.e2=true;}
+        else 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;}//リードモーター  
@@ -53,6 +57,16 @@
         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 , "sma"   )==0 ){
+            gf_MtReq[0].bf.req=true; 
+            gf_MtReq[0].bf.val=(UINT16)atoi(&g_CmdBuf[arg2pos]);
+            gf_MtReq[1].bf.req=true; 
+            gf_MtReq[1].bf.val=(UINT16)atoi(&g_CmdBuf[arg2pos]);
+            gf_MtReq[2].bf.req=true; 
+            gf_MtReq[2].bf.val=(UINT16)atoi(&g_CmdBuf[arg2pos]);
+            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;}//
@@ -66,77 +80,95 @@
         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 , "mm1" ) == 0 ){gf_Mon.bf.m1=true;}
+        else if (strcmp(g_CmdBuf , "mm2" ) == 0 ){gf_Mon.bf.m2=true;}
+        else if (strcmp(g_CmdBuf , "mm3" ) == 0 ){gf_Mon.bf.m3=true;}
+        else if (strcmp(g_CmdBuf , "mm4" ) == 0 ){gf_Mon.bf.m4=true;}
         else if (strcmp(g_CmdBuf , "kill")==0 ){gf_Mon.flg=0;}//モニタ停止
         
         // 開発、デバッグ用コマンド
         else if (strcmp(g_CmdBuf , "stat")==0 ){gf_Print.bf.stat=true;}//ステート表示
         else if (strcmp(g_CmdBuf , "sstatf")==0 ){setStateF((enmHbState)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 , "smot1ofs")==0 ){g_MotPara[0].offset = atoi(&g_CmdBuf[arg2pos]);gf_MotParaUpdate[0]=true;}//モーター１オフセット設定
+        else if (strcmp(g_CmdBuf , "smot1hi")==0 ){g_MotPara[0].limit_hi = atoi(&g_CmdBuf[arg2pos]);gf_MotParaUpdate[0]=true;}//モーター１上限値設定
+        else if (strcmp(g_CmdBuf , "smot1low")==0 ){g_MotPara[0].limit_low = atoi(&g_CmdBuf[arg2pos]);gf_MotParaUpdate[0]=true;}//モーター１下限値設定
+        //else if (strcmp(g_CmdBuf , "smot2ofs")==0 ){g_MotPara[1].offset = atoi(&g_CmdBuf[arg2pos]);gf_MotParaUpdate[1]=true;}//モーター２オフセット設定
+        else if (strcmp(g_CmdBuf , "smot2hi")==0 ){g_MotPara[1].limit_hi = atoi(&g_CmdBuf[arg2pos]);gf_MotParaUpdate[1]=true;}//モーター２上限値設定
+        else if (strcmp(g_CmdBuf , "smot2low")==0 ){g_MotPara[1].limit_low = atoi(&g_CmdBuf[arg2pos]);gf_MotParaUpdate[1]=true;}//モーター２下限値設定
+        //else if (strcmp(g_CmdBuf , "smot3ofs")==0 ){g_MotPara[2].offset = atoi(&g_CmdBuf[arg2pos]);gf_MotParaUpdate[2]=true;}//モーター３オフセット設定
+        else if (strcmp(g_CmdBuf , "smot3hi")==0 ){g_MotPara[2].limit_hi = atoi(&g_CmdBuf[arg2pos]);gf_MotParaUpdate[2]=true;}//モーター３上限値設定
+        else if (strcmp(g_CmdBuf , "smot3low")==0 ){g_MotPara[2].limit_low = atoi(&g_CmdBuf[arg2pos]);gf_MotParaUpdate[2]=true;}//モーター３下限値設定
+        //else if (strcmp(g_CmdBuf , "smot4ofs")==0 ){g_MotPara[3].offset = atoi(&g_CmdBuf[arg2pos]);gf_MotParaUpdate[3]=true;}//モーター４オフセット設定
+        else if (strcmp(g_CmdBuf , "smot4hi")==0 ){g_MotPara[3].limit_hi = atoi(&g_CmdBuf[arg2pos]);gf_MotParaUpdate[3]=true;}//モーター４上限値設定
+        else if (strcmp(g_CmdBuf , "smot4low")==0 ){g_MotPara[3].limit_low = atoi(&g_CmdBuf[arg2pos]);gf_MotParaUpdate[3]=true;}//モーター４下限値設定
+        // else if (strcmp(g_CmdBuf , "smotofs")==0 ){
+        //     g_MotPara[0].offset = atoi(&g_CmdBuf[arg2pos]);gf_MotParaUpdate[0]=true;
+        //     g_MotPara[1].offset = atoi(&g_CmdBuf[arg2pos]);gf_MotParaUpdate[1]=true;
+        //     g_MotPara[2].offset = atoi(&g_CmdBuf[arg2pos]);gf_MotParaUpdate[2]=true;
+        //     g_MotPara[3].offset = atoi(&g_CmdBuf[arg2pos]);gf_MotParaUpdate[3]=true;
+        //     }//モーター１~４オフセット設定
+        else if (strcmp(g_CmdBuf , "smothi")==0 ){
+            g_MotPara[0].limit_hi = atoi(&g_CmdBuf[arg2pos]);gf_MotParaUpdate[0]=true;
+            g_MotPara[1].limit_hi = atoi(&g_CmdBuf[arg2pos]);gf_MotParaUpdate[1]=true;
+            g_MotPara[2].limit_hi = atoi(&g_CmdBuf[arg2pos]);gf_MotParaUpdate[2]=true;
+            g_MotPara[3].limit_hi = atoi(&g_CmdBuf[arg2pos]);gf_MotParaUpdate[3]=true;
+            }//モーター１~４上限値設定
+        else if (strcmp(g_CmdBuf , "smotlow")==0 ){
+            g_MotPara[0].limit_low = atoi(&g_CmdBuf[arg2pos]);gf_MotParaUpdate[0]=true;
+            g_MotPara[1].limit_low = atoi(&g_CmdBuf[arg2pos]);gf_MotParaUpdate[1]=true;
+            g_MotPara[2].limit_low = atoi(&g_CmdBuf[arg2pos]);gf_MotParaUpdate[2]=true;
+            g_MotPara[3].limit_low = atoi(&g_CmdBuf[arg2pos]);gf_MotParaUpdate[3]=true;
+            }//モーター１~４下限値設定
+       
         
         //何にも引っ掛からない
         else {sp.printf("command missmatch\r\n"); };
     } else if(gf_State == SLEEP){
-            if (strcmp(g_CmdBuf , "sarm"   )==0 ){gf_Armed = true;}//アーミングフラグセット テスト用
+        //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 , "stat")==0 ){gf_Print.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].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 , "sma"   )==0 ){
+            gf_MtReq[0].bf.req=true; 
+            gf_MtReq[0].bf.val=(UINT16)atoi(&g_CmdBuf[arg2pos]);
+            gf_MtReq[1].bf.req=true; 
+            gf_MtReq[1].bf.val=(UINT16)atoi(&g_CmdBuf[arg2pos]);
+            gf_MtReq[2].bf.req=true; 
+            gf_MtReq[2].bf.val=(UINT16)atoi(&g_CmdBuf[arg2pos]);
+            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 , "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 {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 , "sarm"   )==0 ){gf_Armed = true;}//アーミングフラグセット
-    
-//     //
-// //    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 , "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"); };
     
     //バッファクリア
     memset(&g_CmdBuf[0] , 0x00 , sizeof(g_CmdBuf) );