teamALI / Mbed 2 deprecated HB2018

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   mbed FreeRTOS

hbCommand.cpp@29:eb3d72dd94aa, 2018-12-17 (annotated)

Committer:
MasashiNomura
Date:
Mon Dec 17 13:25:00 2018 +0000
Revision:
29:eb3d72dd94aa
Parent:
27:ff63c23bc689
Child:
30:13ada1a24c59
2018/12/17 modify HbMotCtrl HbSubProp etc

Who changed what in which revision?

UserRevisionLine numberNew contents of line
takeru0x1103 18:5aa48aec9cae 1 #include <string.h>
takeru0x1103 18:5aa48aec9cae 2 #include <stdio.h>
MasashiNomura 25:f3a6e7eec9c3 3 #include "typedef.h"
takeru0x1103 18:5aa48aec9cae 4 #include "globalFlags.h"
takeru0x1103 18:5aa48aec9cae 5 #include "hbCommand.h"
takeru0x1103 18:5aa48aec9cae 6
takeru0x1103 18:5aa48aec9cae 7 //-------------------------------------------------------------
takeru0x1103 19:4b0fe9a5ec38 8 //シリアルコマンド解析
takeru0x1103 18:5aa48aec9cae 9 //-------------------------------------------------------------
takeru0x1103 18:5aa48aec9cae 10 void commandParse(){
takeru0x1103 18:5aa48aec9cae 11 //フラグが立ってなければ何もしないで終わる
takeru0x1103 18:5aa48aec9cae 12 if(gf_CmdPrs==false){
takeru0x1103 18:5aa48aec9cae 13 return;
takeru0x1103 18:5aa48aec9cae 14 }
takeru0x1103 18:5aa48aec9cae 15 //フラグを落として以降の処理をする
takeru0x1103 18:5aa48aec9cae 16 gf_CmdPrs=false;
takeru0x1103 18:5aa48aec9cae 17
takeru0x1103 18:5aa48aec9cae 18 //第二引数の開始位置
takeru0x1103 18:5aa48aec9cae 19 UCHAR arg2pos=0;
takeru0x1103 18:5aa48aec9cae 20 while (1) {
takeru0x1103 19:4b0fe9a5ec38 21 if(g_CmdBuf[arg2pos]==NULL){
takeru0x1103 18:5aa48aec9cae 22 arg2pos++;
takeru0x1103 18:5aa48aec9cae 23 break;
takeru0x1103 18:5aa48aec9cae 24 }else{
takeru0x1103 18:5aa48aec9cae 25 arg2pos++;
takeru0x1103 18:5aa48aec9cae 26 }
takeru0x1103 18:5aa48aec9cae 27 }
takeru0x1103 18:5aa48aec9cae 28
MasashiNomura 25:f3a6e7eec9c3 29 //if(gf_State != SLEEP && gf_State != WAKEUP){
MasashiNomura 25:f3a6e7eec9c3 30 if(gf_State != SLEEP){
MasashiNomura 25:f3a6e7eec9c3 31 //EMG STOP 現在はモーターの強制停止
MasashiNomura 25:f3a6e7eec9c3 32 if (g_CmdBuf[0] == ESC){gf_StopMot = true;}
MasashiNomura 22:24c9c2dedca9 33 //リード系
MasashiNomura 25:f3a6e7eec9c3 34 else if (strcmp(g_CmdBuf , "re" )==0 ){gf_Print.bf.e1=true; gf_Print.bf.e2=true;}
MasashiNomura 22:24c9c2dedca9 35 else if (strcmp(g_CmdBuf , "re1" )==0 ){gf_Print.bf.e1=true;}
MasashiNomura 22:24c9c2dedca9 36 else if (strcmp(g_CmdBuf , "re2" )==0 ){gf_Print.bf.e2=true;}
MasashiNomura 22:24c9c2dedca9 37 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;}//リードモーター
MasashiNomura 22:24c9c2dedca9 38 else if (strcmp(g_CmdBuf , "rm1" )==0 ){gf_Print.bf.m1=true;}//リードモーター1
MasashiNomura 22:24c9c2dedca9 39 else if (strcmp(g_CmdBuf , "rm2" )==0 ){gf_Print.bf.m2=true;}//リードモーター2
MasashiNomura 22:24c9c2dedca9 40 else if (strcmp(g_CmdBuf , "rm3" )==0 ){gf_Print.bf.m3=true;}//リードモーター3
MasashiNomura 22:24c9c2dedca9 41 else if (strcmp(g_CmdBuf , "rm4" )==0 ){gf_Print.bf.m4=true;}//リードモーター4
MasashiNomura 22:24c9c2dedca9 42 else if (strcmp(g_CmdBuf , "rg" )==0 ){gf_Print.bf.gy=true;}//リードジャイロ
MasashiNomura 22:24c9c2dedca9 43 else if (strcmp(g_CmdBuf , "ry" )==0 ){gf_Print.bf.yaw=true;}//リード姿勢角ヨー
MasashiNomura 22:24c9c2dedca9 44 else if (strcmp(g_CmdBuf , "rimu")==0 ){gf_Print.bf.yaw=true;gf_Print.bf.gy=true;}//リードIMU
MasashiNomura 22:24c9c2dedca9 45 else if (strcmp(g_CmdBuf , "rs" )==0 ){ }//リードスイッチ
MasashiNomura 22:24c9c2dedca9 46 else if (strcmp(g_CmdBuf , "rfb" )==0 ){gf_Print.bf.fb=true;}//PID制御フィードバック結果
MasashiNomura 22:24c9c2dedca9 47 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;}//パラメータ全表示
MasashiNomura 22:24c9c2dedca9 48 else if (strcmp(g_CmdBuf , "rpP" )==0 ){gf_Print.bf.pp=true;}//パラメータアウターPゲイン表示
MasashiNomura 22:24c9c2dedca9 49 else if (strcmp(g_CmdBuf , "rpp" )==0 ){gf_Print.bf.p=true;}//パラメータPゲイン表示
MasashiNomura 22:24c9c2dedca9 50 else if (strcmp(g_CmdBuf , "rpi" )==0 ){gf_Print.bf.i=true;}//パラメータIゲイン表示
MasashiNomura 22:24c9c2dedca9 51 else if (strcmp(g_CmdBuf , "rpd" )==0 ){gf_Print.bf.d=true;}//パラメータDゲイン表示
MasashiNomura 22:24c9c2dedca9 52
MasashiNomura 22:24c9c2dedca9 53 //設定系
MasashiNomura 22:24c9c2dedca9 54 else if (strcmp(g_CmdBuf , "se1" )==0 ){gf_AxReq[0].bf.req=true; gf_AxReq[0].bf.val=(UINT16)atoi(&g_CmdBuf[arg2pos]);}
MasashiNomura 22:24c9c2dedca9 55 else if (strcmp(g_CmdBuf , "se2" )==0 ){gf_AxReq[1].bf.req=true; gf_AxReq[1].bf.val=(UINT16)atoi(&g_CmdBuf[arg2pos]);}
MasashiNomura 22:24c9c2dedca9 56 else if (strcmp(g_CmdBuf , "sm1" )==0 ){gf_MtReq[0].bf.req=true; gf_MtReq[0].bf.val=(UINT16)atoi(&g_CmdBuf[arg2pos]);}
MasashiNomura 22:24c9c2dedca9 57 else if (strcmp(g_CmdBuf , "sm2" )==0 ){gf_MtReq[1].bf.req=true; gf_MtReq[1].bf.val=(UINT16)atoi(&g_CmdBuf[arg2pos]);}
MasashiNomura 22:24c9c2dedca9 58 else if (strcmp(g_CmdBuf , "sm3" )==0 ){gf_MtReq[2].bf.req=true; gf_MtReq[2].bf.val=(UINT16)atoi(&g_CmdBuf[arg2pos]);}
MasashiNomura 22:24c9c2dedca9 59 else if (strcmp(g_CmdBuf , "sm4" )==0 ){gf_MtReq[3].bf.req=true; gf_MtReq[3].bf.val=(UINT16)atoi(&g_CmdBuf[arg2pos]);}
MasashiNomura 27:ff63c23bc689 60 else if (strcmp(g_CmdBuf , "sm" )==0 ){
MasashiNomura 27:ff63c23bc689 61 for(int i = 0; i < 4; ++i){
MasashiNomura 27:ff63c23bc689 62 gf_MtReq[i].bf.req=true;
MasashiNomura 27:ff63c23bc689 63 gf_MtReq[i].bf.val=(UINT16)atoi(&g_CmdBuf[arg2pos]);
MasashiNomura 27:ff63c23bc689 64 }
MasashiNomura 25:f3a6e7eec9c3 65 }
MasashiNomura 26:732bc37fbefd 66 else if (strcmp(g_CmdBuf , "smf" )==0 ){
MasashiNomura 26:732bc37fbefd 67 gf_MtReq[0].bf.req=true;
MasashiNomura 26:732bc37fbefd 68 gf_MtReq[0].bf.val=(UINT16)atoi(&g_CmdBuf[arg2pos]);
MasashiNomura 26:732bc37fbefd 69 gf_MtReq[1].bf.req=true;
MasashiNomura 26:732bc37fbefd 70 gf_MtReq[1].bf.val=(UINT16)atoi(&g_CmdBuf[arg2pos]);}
MasashiNomura 26:732bc37fbefd 71 else if (strcmp(g_CmdBuf , "smr" )==0 ){
MasashiNomura 26:732bc37fbefd 72 gf_MtReq[2].bf.req=true;
MasashiNomura 26:732bc37fbefd 73 gf_MtReq[2].bf.val=(UINT16)atoi(&g_CmdBuf[arg2pos]);
MasashiNomura 26:732bc37fbefd 74 gf_MtReq[3].bf.req=true;
MasashiNomura 26:732bc37fbefd 75 gf_MtReq[3].bf.val=(UINT16)atoi(&g_CmdBuf[arg2pos]); }
MasashiNomura 27:ff63c23bc689 76 else if (strcmp(g_CmdBuf , "smo1" )==0 ){gf_MtReqOfs[0].bf.req=true; gf_MtReqOfs[0].bf.val=(UINT16)atoi(&g_CmdBuf[arg2pos]);}
MasashiNomura 27:ff63c23bc689 77 else if (strcmp(g_CmdBuf , "smo2" )==0 ){gf_MtReqOfs[1].bf.req=true; gf_MtReqOfs[1].bf.val=(UINT16)atoi(&g_CmdBuf[arg2pos]);}
MasashiNomura 27:ff63c23bc689 78 else if (strcmp(g_CmdBuf , "smo3" )==0 ){gf_MtReqOfs[2].bf.req=true; gf_MtReqOfs[2].bf.val=(UINT16)atoi(&g_CmdBuf[arg2pos]);}
MasashiNomura 27:ff63c23bc689 79 else if (strcmp(g_CmdBuf , "smo4" )==0 ){gf_MtReqOfs[3].bf.req=true; gf_MtReqOfs[3].bf.val=(UINT16)atoi(&g_CmdBuf[arg2pos]);}
MasashiNomura 27:ff63c23bc689 80 else if (strcmp(g_CmdBuf , "smo" )==0 ){
MasashiNomura 27:ff63c23bc689 81 for(int i = 0; i < 4; ++i){
MasashiNomura 27:ff63c23bc689 82 gf_MtReqOfs[i].bf.req=true;
MasashiNomura 27:ff63c23bc689 83 gf_MtReqOfs[i].bf.val=(UINT16)atoi(&g_CmdBuf[arg2pos]);
MasashiNomura 27:ff63c23bc689 84 }
MasashiNomura 27:ff63c23bc689 85 }
MasashiNomura 27:ff63c23bc689 86 else if (strcmp(g_CmdBuf , "smof" )==0 ){
MasashiNomura 27:ff63c23bc689 87 gf_MtReqOfs[0].bf.req=true;
MasashiNomura 27:ff63c23bc689 88 gf_MtReqOfs[0].bf.val=(UINT16)atoi(&g_CmdBuf[arg2pos]);
MasashiNomura 27:ff63c23bc689 89 gf_MtReqOfs[1].bf.req=true;
MasashiNomura 27:ff63c23bc689 90 gf_MtReqOfs[1].bf.val=(UINT16)atoi(&g_CmdBuf[arg2pos]);}
MasashiNomura 27:ff63c23bc689 91 else if (strcmp(g_CmdBuf , "smor" )==0 ){
MasashiNomura 27:ff63c23bc689 92 gf_MtReqOfs[2].bf.req=true;
MasashiNomura 27:ff63c23bc689 93 gf_MtReqOfs[2].bf.val=(UINT16)atoi(&g_CmdBuf[arg2pos]);
MasashiNomura 27:ff63c23bc689 94 gf_MtReqOfs[3].bf.req=true;
MasashiNomura 27:ff63c23bc689 95 gf_MtReqOfs[3].bf.val=(UINT16)atoi(&g_CmdBuf[arg2pos]); }
MasashiNomura 27:ff63c23bc689 96 //else if (strcmp(g_CmdBuf , "smfpga1" )==0 ){gf_MtReqDct[0].bf.req=true; gf_MtReqDct[0].bf.val=(UINT16)atoi(&g_CmdBuf[arg2pos]);}
MasashiNomura 27:ff63c23bc689 97 //else if (strcmp(g_CmdBuf , "smfpga2" )==0 ){gf_MtReqDct[1].bf.req=true; gf_MtReqDct[1].bf.val=(UINT16)atoi(&g_CmdBuf[arg2pos]);}
MasashiNomura 27:ff63c23bc689 98 //else if (strcmp(g_CmdBuf , "smfpga3" )==0 ){gf_MtReqDct[2].bf.req=true; gf_MtReqDct[2].bf.val=(UINT16)atoi(&g_CmdBuf[arg2pos]);}
MasashiNomura 27:ff63c23bc689 99 //else if (strcmp(g_CmdBuf , "smfpga4" )==0 ){gf_MtReqDct[3].bf.req=true; gf_MtReqDct[3].bf.val=(UINT16)atoi(&g_CmdBuf[arg2pos]);}
MasashiNomura 27:ff63c23bc689 100 //else if (strcmp(g_CmdBuf , "smfpga5" )==0 ){gf_MtReqDct[4].bf.req=true; gf_MtReqDct[4].bf.val=(UINT16)atoi(&g_CmdBuf[arg2pos]);}
MasashiNomura 27:ff63c23bc689 101 //else if (strcmp(g_CmdBuf , "smfpga6" )==0 ){gf_MtReqDct[5].bf.req=true; gf_MtReqDct[5].bf.val=(UINT16)atoi(&g_CmdBuf[arg2pos]);}
MasashiNomura 27:ff63c23bc689 102 //else if (strcmp(g_CmdBuf , "smfpga7" )==0 ){gf_MtReqDct[6].bf.req=true; gf_MtReqDct[6].bf.val=(UINT16)atoi(&g_CmdBuf[arg2pos]);}
MasashiNomura 27:ff63c23bc689 103 //else if (strcmp(g_CmdBuf , "smfpga8" )==0 ){gf_MtReqDct[7].bf.req=true; gf_MtReqDct[7].bf.val=(UINT16)atoi(&g_CmdBuf[arg2pos]);}
MasashiNomura 27:ff63c23bc689 104
MasashiNomura 22:24c9c2dedca9 105 //キャリブレーション
MasashiNomura 22:24c9c2dedca9 106 else if (strcmp(g_CmdBuf , "calyaw")==0 ){gf_Cal.bf.yaw=true;}//
MasashiNomura 22:24c9c2dedca9 107 else if (strcmp(g_CmdBuf , "calgyro")==0 ){gf_Cal.bf.gy=true;}//
MasashiNomura 22:24c9c2dedca9 108 else if (strcmp(g_CmdBuf , "calimu")==0 ){gf_Cal.bf.gy=true;gf_Cal.bf.yaw=true;}//
MasashiNomura 22:24c9c2dedca9 109
MasashiNomura 22:24c9c2dedca9 110 //チェック系
MasashiNomura 22:24c9c2dedca9 111
MasashiNomura 22:24c9c2dedca9 112 //連続モニタ
MasashiNomura 22:24c9c2dedca9 113 else if (strcmp(g_CmdBuf , "mg" )==0 ){gf_Print.bf.gy=true;gf_Mon.bf.gy=true;}//リードジャイロ
MasashiNomura 22:24c9c2dedca9 114 else if (strcmp(g_CmdBuf , "my" )==0 ){gf_Print.bf.yaw=true;gf_Mon.bf.yaw=true;}//リード姿勢角ヨー
MasashiNomura 22:24c9c2dedca9 115 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
MasashiNomura 22:24c9c2dedca9 116 else if (strcmp(g_CmdBuf , "mfb" )==0 ){gf_Print.bf.fb=true;gf_Mon.bf.fb=true;}//PID制御フィードバック結果
MasashiNomura 25:f3a6e7eec9c3 117 else if (strcmp(g_CmdBuf , "mm1" ) == 0 ){gf_Mon.bf.m1=true;}
MasashiNomura 25:f3a6e7eec9c3 118 else if (strcmp(g_CmdBuf , "mm2" ) == 0 ){gf_Mon.bf.m2=true;}
MasashiNomura 25:f3a6e7eec9c3 119 else if (strcmp(g_CmdBuf , "mm3" ) == 0 ){gf_Mon.bf.m3=true;}
MasashiNomura 25:f3a6e7eec9c3 120 else if (strcmp(g_CmdBuf , "mm4" ) == 0 ){gf_Mon.bf.m4=true;}
MasashiNomura 22:24c9c2dedca9 121 else if (strcmp(g_CmdBuf , "kill")==0 ){gf_Mon.flg=0;}//モニタ停止
MasashiNomura 22:24c9c2dedca9 122
MasashiNomura 24:c5945aaae777 123 // 開発、デバッグ用コマンド
MasashiNomura 23:79e20be4bc5b 124 else if (strcmp(g_CmdBuf , "stat")==0 ){gf_Print.bf.stat=true;}//ステート表示
MasashiNomura 23:79e20be4bc5b 125 else if (strcmp(g_CmdBuf , "sstatf")==0 ){setStateF((enmHbState)atoi(&g_CmdBuf[arg2pos]));}//ステート強制遷移
MasashiNomura 25:f3a6e7eec9c3 126
MasashiNomura 25:f3a6e7eec9c3 127 else if (strcmp(g_CmdBuf , "stopmot")==0 ){gf_StopMot = true;}//モーター強制停止
MasashiNomura 25:f3a6e7eec9c3 128
MasashiNomura 24:c5945aaae777 129 else if (strcmp(g_CmdBuf , "spidpp")==0 ){g_PidPara.PP = atof(&g_CmdBuf[arg2pos]);gf_PidParaUpdate=true;}//PIDの係数 Pp設定
MasashiNomura 24:c5945aaae777 130 else if (strcmp(g_CmdBuf , "spidp")==0 ){g_PidPara.P = atof(&g_CmdBuf[arg2pos]);gf_PidParaUpdate=true;}//PIDの係数 P設定
MasashiNomura 24:c5945aaae777 131 else if (strcmp(g_CmdBuf , "spidi")==0 ){g_PidPara.I = atof(&g_CmdBuf[arg2pos]);gf_PidParaUpdate=true;}//PIDの係数 I設定
MasashiNomura 24:c5945aaae777 132 else if (strcmp(g_CmdBuf , "spidd")==0 ){g_PidPara.D = atof(&g_CmdBuf[arg2pos]);gf_PidParaUpdate=true;}//PIDの係数 D設定
MasashiNomura 24:c5945aaae777 133 else if (strcmp(g_CmdBuf , "spidimax")==0 ){g_PidPara.IMax = atof(&g_CmdBuf[arg2pos]);gf_PidParaUpdate=true;}//PIDの係数 積分上限値設定
MasashiNomura 24:c5945aaae777 134 else if (strcmp(g_CmdBuf , "spidimin")==0 ){g_PidPara.IMin = atof(&g_CmdBuf[arg2pos]);gf_PidParaUpdate=true;}//PIDの係数 積分下限値設定
MasashiNomura 25:f3a6e7eec9c3 135
MasashiNomura 25:f3a6e7eec9c3 136 //else if (strcmp(g_CmdBuf , "smot1ofs")==0 ){g_MotPara[0].offset = atoi(&g_CmdBuf[arg2pos]);gf_MotParaUpdate[0]=true;}//モーター１オフセット設定
MasashiNomura 25:f3a6e7eec9c3 137 else if (strcmp(g_CmdBuf , "smot1hi")==0 ){g_MotPara[0].limit_hi = atoi(&g_CmdBuf[arg2pos]);gf_MotParaUpdate[0]=true;}//モーター１上限値設定
MasashiNomura 25:f3a6e7eec9c3 138 else if (strcmp(g_CmdBuf , "smot1low")==0 ){g_MotPara[0].limit_low = atoi(&g_CmdBuf[arg2pos]);gf_MotParaUpdate[0]=true;}//モーター１下限値設定
MasashiNomura 25:f3a6e7eec9c3 139 //else if (strcmp(g_CmdBuf , "smot2ofs")==0 ){g_MotPara[1].offset = atoi(&g_CmdBuf[arg2pos]);gf_MotParaUpdate[1]=true;}//モーター２オフセット設定
MasashiNomura 25:f3a6e7eec9c3 140 else if (strcmp(g_CmdBuf , "smot2hi")==0 ){g_MotPara[1].limit_hi = atoi(&g_CmdBuf[arg2pos]);gf_MotParaUpdate[1]=true;}//モーター２上限値設定
MasashiNomura 25:f3a6e7eec9c3 141 else if (strcmp(g_CmdBuf , "smot2low")==0 ){g_MotPara[1].limit_low = atoi(&g_CmdBuf[arg2pos]);gf_MotParaUpdate[1]=true;}//モーター２下限値設定
MasashiNomura 25:f3a6e7eec9c3 142 //else if (strcmp(g_CmdBuf , "smot3ofs")==0 ){g_MotPara[2].offset = atoi(&g_CmdBuf[arg2pos]);gf_MotParaUpdate[2]=true;}//モーター３オフセット設定
MasashiNomura 25:f3a6e7eec9c3 143 else if (strcmp(g_CmdBuf , "smot3hi")==0 ){g_MotPara[2].limit_hi = atoi(&g_CmdBuf[arg2pos]);gf_MotParaUpdate[2]=true;}//モーター３上限値設定
MasashiNomura 25:f3a6e7eec9c3 144 else if (strcmp(g_CmdBuf , "smot3low")==0 ){g_MotPara[2].limit_low = atoi(&g_CmdBuf[arg2pos]);gf_MotParaUpdate[2]=true;}//モーター３下限値設定
MasashiNomura 25:f3a6e7eec9c3 145 //else if (strcmp(g_CmdBuf , "smot4ofs")==0 ){g_MotPara[3].offset = atoi(&g_CmdBuf[arg2pos]);gf_MotParaUpdate[3]=true;}//モーター４オフセット設定
MasashiNomura 25:f3a6e7eec9c3 146 else if (strcmp(g_CmdBuf , "smot4hi")==0 ){g_MotPara[3].limit_hi = atoi(&g_CmdBuf[arg2pos]);gf_MotParaUpdate[3]=true;}//モーター４上限値設定
MasashiNomura 25:f3a6e7eec9c3 147 else if (strcmp(g_CmdBuf , "smot4low")==0 ){g_MotPara[3].limit_low = atoi(&g_CmdBuf[arg2pos]);gf_MotParaUpdate[3]=true;}//モーター４下限値設定
MasashiNomura 25:f3a6e7eec9c3 148 // else if (strcmp(g_CmdBuf , "smotofs")==0 ){
MasashiNomura 25:f3a6e7eec9c3 149 // g_MotPara[0].offset = atoi(&g_CmdBuf[arg2pos]);gf_MotParaUpdate[0]=true;
MasashiNomura 25:f3a6e7eec9c3 150 // g_MotPara[1].offset = atoi(&g_CmdBuf[arg2pos]);gf_MotParaUpdate[1]=true;
MasashiNomura 25:f3a6e7eec9c3 151 // g_MotPara[2].offset = atoi(&g_CmdBuf[arg2pos]);gf_MotParaUpdate[2]=true;
MasashiNomura 25:f3a6e7eec9c3 152 // g_MotPara[3].offset = atoi(&g_CmdBuf[arg2pos]);gf_MotParaUpdate[3]=true;
MasashiNomura 25:f3a6e7eec9c3 153 // }//モーター１~４オフセット設定
MasashiNomura 25:f3a6e7eec9c3 154 else if (strcmp(g_CmdBuf , "smothi")==0 ){
MasashiNomura 25:f3a6e7eec9c3 155 g_MotPara[0].limit_hi = atoi(&g_CmdBuf[arg2pos]);gf_MotParaUpdate[0]=true;
MasashiNomura 25:f3a6e7eec9c3 156 g_MotPara[1].limit_hi = atoi(&g_CmdBuf[arg2pos]);gf_MotParaUpdate[1]=true;
MasashiNomura 25:f3a6e7eec9c3 157 g_MotPara[2].limit_hi = atoi(&g_CmdBuf[arg2pos]);gf_MotParaUpdate[2]=true;
MasashiNomura 25:f3a6e7eec9c3 158 g_MotPara[3].limit_hi = atoi(&g_CmdBuf[arg2pos]);gf_MotParaUpdate[3]=true;
MasashiNomura 25:f3a6e7eec9c3 159 }//モーター１~４上限値設定
MasashiNomura 25:f3a6e7eec9c3 160 else if (strcmp(g_CmdBuf , "smotlow")==0 ){
MasashiNomura 25:f3a6e7eec9c3 161 g_MotPara[0].limit_low = atoi(&g_CmdBuf[arg2pos]);gf_MotParaUpdate[0]=true;
MasashiNomura 25:f3a6e7eec9c3 162 g_MotPara[1].limit_low = atoi(&g_CmdBuf[arg2pos]);gf_MotParaUpdate[1]=true;
MasashiNomura 25:f3a6e7eec9c3 163 g_MotPara[2].limit_low = atoi(&g_CmdBuf[arg2pos]);gf_MotParaUpdate[2]=true;
MasashiNomura 25:f3a6e7eec9c3 164 g_MotPara[3].limit_low = atoi(&g_CmdBuf[arg2pos]);gf_MotParaUpdate[3]=true;
MasashiNomura 25:f3a6e7eec9c3 165 }//モーター１~４下限値設定
MasashiNomura 26:732bc37fbefd 166 else if (strcmp(g_CmdBuf , "rmo" )==0 ){gf_DbgPrint.bf.mo1=true;gf_DbgPrint.bf.mo2=true;gf_DbgPrint.bf.mo3=true;gf_DbgPrint.bf.mo4=true;}//リードモーターオフセット
MasashiNomura 26:732bc37fbefd 167 else if (strcmp(g_CmdBuf , "rmo1" )==0 ){gf_DbgPrint.bf.mo1=true;}//リードモーターオフセット1
MasashiNomura 26:732bc37fbefd 168 else if (strcmp(g_CmdBuf , "rmo2" )==0 ){gf_DbgPrint.bf.mo2=true;}//リードモーターオフセット2
MasashiNomura 26:732bc37fbefd 169 else if (strcmp(g_CmdBuf , "rmo3" )==0 ){gf_DbgPrint.bf.mo3=true;}//リードモーターオフセット3
MasashiNomura 26:732bc37fbefd 170 else if (strcmp(g_CmdBuf , "rmo4" )==0 ){gf_DbgPrint.bf.mo4=true;}//リードモーターオフセット4
MasashiNomura 25:f3a6e7eec9c3 171
MasashiNomura 29:eb3d72dd94aa 172 //else if (strcmp(g_CmdBuf , "op1" )==0 ){gf_MtReqOP[0].bf.req=true; gf_MtReqOP[0].bf.counter = 0; gf_MtReqOP[0].bf.num = atoi(&g_CmdBuf[arg2pos]);}//モーター1一時的噴射
MasashiNomura 29:eb3d72dd94aa 173 //else if (strcmp(g_CmdBuf , "op2" )==0 ){gf_MtReqOP[1].bf.req=true; gf_MtReqOP[1].bf.counter = 0; gf_MtReqOP[1].bf.num = atoi(&g_CmdBuf[arg2pos]);}//モーター2一時的噴射
MasashiNomura 29:eb3d72dd94aa 174 //else if (strcmp(g_CmdBuf , "op3" )==0 ){gf_MtReqOP[2].bf.req=true; gf_MtReqOP[2].bf.counter = 0; gf_MtReqOP[2].bf.num = atoi(&g_CmdBuf[arg2pos]);}//モーター3一時的噴射
MasashiNomura 29:eb3d72dd94aa 175 //else if (strcmp(g_CmdBuf , "op4" )==0 ){gf_MtReqOP[3].bf.req=true; gf_MtReqOP[3].bf.counter = 0; gf_MtReqOP[3].bf.num = atoi(&g_CmdBuf[arg2pos]);}//モーター4一時的噴射
MasashiNomura 29:eb3d72dd94aa 176 else if (strcmp(g_CmdBuf , "op1" )==0 ){gf_MtReqOP[0].req=true; gf_MtReqOP[0].add_end=false; gf_MtReqOP[0].counter = 0; gf_MtReqOP[0].num = atoi(&g_CmdBuf[arg2pos]);}//モーター1一時的噴射
MasashiNomura 29:eb3d72dd94aa 177 else if (strcmp(g_CmdBuf , "op2" )==0 ){gf_MtReqOP[1].req=true; gf_MtReqOP[1].add_end=false; gf_MtReqOP[1].counter = 0; gf_MtReqOP[1].num = atoi(&g_CmdBuf[arg2pos]);}//モーター2一時的噴射
MasashiNomura 29:eb3d72dd94aa 178 else if (strcmp(g_CmdBuf , "op3" )==0 ){gf_MtReqOP[2].req=true; gf_MtReqOP[2].add_end=false; gf_MtReqOP[2].counter = 0; gf_MtReqOP[2].num = atoi(&g_CmdBuf[arg2pos]);}//モーター3一時的噴射
MasashiNomura 29:eb3d72dd94aa 179 else if (strcmp(g_CmdBuf , "op4" )==0 ){gf_MtReqOP[3].req=true; gf_MtReqOP[3].add_end=false; gf_MtReqOP[3].counter = 0; gf_MtReqOP[3].num = atoi(&g_CmdBuf[arg2pos]);}//モーター4一時的噴射
MasashiNomura 23:79e20be4bc5b 180
MasashiNomura 22:24c9c2dedca9 181 //何にも引っ掛からない
MasashiNomura 22:24c9c2dedca9 182 else {sp.printf("command missmatch\r\n"); };
MasashiNomura 22:24c9c2dedca9 183 } else if(gf_State == SLEEP){
MasashiNomura 25:f3a6e7eec9c3 184 //EMG STOP 現在はモーターの強制停止
MasashiNomura 25:f3a6e7eec9c3 185 if (g_CmdBuf[0] == ESC){gf_StopMot = true;}
MasashiNomura 25:f3a6e7eec9c3 186 else if (strcmp(g_CmdBuf , "sarm" )==0 ){gf_Armed = true;}//アーミングフラグセット テスト用
MasashiNomura 23:79e20be4bc5b 187 else if (strcmp(g_CmdBuf , "dbg" )==0 ){if(gf_Dbg != true) gf_Dbg = true;}//デバッグタスク起動
MasashiNomura 24:c5945aaae777 188 else if (strcmp(g_CmdBuf , "stat")==0 ){gf_Print.bf.stat=true;}//ステート表示
MasashiNomura 23:79e20be4bc5b 189 else if (strcmp(g_CmdBuf , "sstatf")==0 ){setStateF((enmHbState)atoi(&g_CmdBuf[arg2pos]));}//ステート強制遷移
MasashiNomura 25:f3a6e7eec9c3 190 else if (strcmp(g_CmdBuf , "sm1" )==0 ){gf_MtReq[0].bf.req=true; gf_MtReq[0].bf.val=(UINT16)atoi(&g_CmdBuf[arg2pos]);}
MasashiNomura 25:f3a6e7eec9c3 191 else if (strcmp(g_CmdBuf , "sm2" )==0 ){gf_MtReq[1].bf.req=true; gf_MtReq[1].bf.val=(UINT16)atoi(&g_CmdBuf[arg2pos]);}
MasashiNomura 25:f3a6e7eec9c3 192 else if (strcmp(g_CmdBuf , "sm3" )==0 ){gf_MtReq[2].bf.req=true; gf_MtReq[2].bf.val=(UINT16)atoi(&g_CmdBuf[arg2pos]);}
MasashiNomura 25:f3a6e7eec9c3 193 else if (strcmp(g_CmdBuf , "sm4" )==0 ){gf_MtReq[3].bf.req=true; gf_MtReq[3].bf.val=(UINT16)atoi(&g_CmdBuf[arg2pos]);}
MasashiNomura 27:ff63c23bc689 194 else if (strcmp(g_CmdBuf , "sm" )==0 ){
MasashiNomura 27:ff63c23bc689 195 for(int i = 0; i < 4; ++i){
MasashiNomura 27:ff63c23bc689 196 gf_MtReq[i].bf.req=true;
MasashiNomura 27:ff63c23bc689 197 gf_MtReq[i].bf.val=(UINT16)atoi(&g_CmdBuf[arg2pos]);
MasashiNomura 27:ff63c23bc689 198 }
MasashiNomura 25:f3a6e7eec9c3 199 }
MasashiNomura 26:732bc37fbefd 200 else if (strcmp(g_CmdBuf , "smf" )==0 ){
MasashiNomura 26:732bc37fbefd 201 gf_MtReq[0].bf.req=true;
MasashiNomura 26:732bc37fbefd 202 gf_MtReq[0].bf.val=(UINT16)atoi(&g_CmdBuf[arg2pos]);
MasashiNomura 26:732bc37fbefd 203 gf_MtReq[1].bf.req=true;
MasashiNomura 26:732bc37fbefd 204 gf_MtReq[1].bf.val=(UINT16)atoi(&g_CmdBuf[arg2pos]);}
MasashiNomura 26:732bc37fbefd 205 else if (strcmp(g_CmdBuf , "smr" )==0 ){
MasashiNomura 26:732bc37fbefd 206 gf_MtReq[2].bf.req=true;
MasashiNomura 26:732bc37fbefd 207 gf_MtReq[2].bf.val=(UINT16)atoi(&g_CmdBuf[arg2pos]);
MasashiNomura 26:732bc37fbefd 208 gf_MtReq[3].bf.req=true;
MasashiNomura 26:732bc37fbefd 209 gf_MtReq[3].bf.val=(UINT16)atoi(&g_CmdBuf[arg2pos]); }
MasashiNomura 27:ff63c23bc689 210 else if (strcmp(g_CmdBuf , "smo1" )==0 ){gf_MtReqOfs[0].bf.req=true; gf_MtReqOfs[0].bf.val=(UINT16)atoi(&g_CmdBuf[arg2pos]);}
MasashiNomura 27:ff63c23bc689 211 else if (strcmp(g_CmdBuf , "smo2" )==0 ){gf_MtReqOfs[1].bf.req=true; gf_MtReqOfs[1].bf.val=(UINT16)atoi(&g_CmdBuf[arg2pos]);}
MasashiNomura 27:ff63c23bc689 212 else if (strcmp(g_CmdBuf , "smo3" )==0 ){gf_MtReqOfs[2].bf.req=true; gf_MtReqOfs[2].bf.val=(UINT16)atoi(&g_CmdBuf[arg2pos]);}
MasashiNomura 27:ff63c23bc689 213 else if (strcmp(g_CmdBuf , "smo4" )==0 ){gf_MtReqOfs[3].bf.req=true; gf_MtReqOfs[3].bf.val=(UINT16)atoi(&g_CmdBuf[arg2pos]);}
MasashiNomura 27:ff63c23bc689 214 else if (strcmp(g_CmdBuf , "smo" )==0 ){
MasashiNomura 27:ff63c23bc689 215 for(int i = 0; i < 4; ++i){
MasashiNomura 27:ff63c23bc689 216 gf_MtReqOfs[i].bf.req=true;
MasashiNomura 27:ff63c23bc689 217 gf_MtReqOfs[i].bf.val=(UINT16)atoi(&g_CmdBuf[arg2pos]);
MasashiNomura 27:ff63c23bc689 218 }
MasashiNomura 27:ff63c23bc689 219 }
MasashiNomura 27:ff63c23bc689 220 else if (strcmp(g_CmdBuf , "smof" )==0 ){
MasashiNomura 27:ff63c23bc689 221 gf_MtReqOfs[0].bf.req=true;
MasashiNomura 27:ff63c23bc689 222 gf_MtReqOfs[0].bf.val=(UINT16)atoi(&g_CmdBuf[arg2pos]);
MasashiNomura 27:ff63c23bc689 223 gf_MtReqOfs[1].bf.req=true;
MasashiNomura 27:ff63c23bc689 224 gf_MtReqOfs[1].bf.val=(UINT16)atoi(&g_CmdBuf[arg2pos]);}
MasashiNomura 27:ff63c23bc689 225 else if (strcmp(g_CmdBuf , "smor" )==0 ){
MasashiNomura 27:ff63c23bc689 226 gf_MtReqOfs[2].bf.req=true;
MasashiNomura 27:ff63c23bc689 227 gf_MtReqOfs[2].bf.val=(UINT16)atoi(&g_CmdBuf[arg2pos]);
MasashiNomura 27:ff63c23bc689 228 gf_MtReqOfs[3].bf.req=true;
MasashiNomura 27:ff63c23bc689 229 gf_MtReqOfs[3].bf.val=(UINT16)atoi(&g_CmdBuf[arg2pos]); }
MasashiNomura 27:ff63c23bc689 230 else if (strcmp(g_CmdBuf , "smfpga1" )==0 ){gf_MtReqDct[0].bf.req=true; gf_MtReqDct[0].bf.val=(UINT16)atoi(&g_CmdBuf[arg2pos]);}
MasashiNomura 27:ff63c23bc689 231 else if (strcmp(g_CmdBuf , "smfpga2" )==0 ){gf_MtReqDct[1].bf.req=true; gf_MtReqDct[1].bf.val=(UINT16)atoi(&g_CmdBuf[arg2pos]);}
MasashiNomura 27:ff63c23bc689 232 else if (strcmp(g_CmdBuf , "smfpga3" )==0 ){gf_MtReqDct[2].bf.req=true; gf_MtReqDct[2].bf.val=(UINT16)atoi(&g_CmdBuf[arg2pos]);}
MasashiNomura 27:ff63c23bc689 233 else if (strcmp(g_CmdBuf , "smfpga4" )==0 ){gf_MtReqDct[3].bf.req=true; gf_MtReqDct[3].bf.val=(UINT16)atoi(&g_CmdBuf[arg2pos]);}
MasashiNomura 27:ff63c23bc689 234 else if (strcmp(g_CmdBuf , "smfpga5" )==0 ){gf_MtReqDct[4].bf.req=true; gf_MtReqDct[4].bf.val=(UINT16)atoi(&g_CmdBuf[arg2pos]);}
MasashiNomura 27:ff63c23bc689 235 else if (strcmp(g_CmdBuf , "smfpga6" )==0 ){gf_MtReqDct[5].bf.req=true; gf_MtReqDct[5].bf.val=(UINT16)atoi(&g_CmdBuf[arg2pos]);}
MasashiNomura 27:ff63c23bc689 236 else if (strcmp(g_CmdBuf , "smfpga7" )==0 ){gf_MtReqDct[6].bf.req=true; gf_MtReqDct[6].bf.val=(UINT16)atoi(&g_CmdBuf[arg2pos]);}
MasashiNomura 27:ff63c23bc689 237 else if (strcmp(g_CmdBuf , "smfpga8" )==0 ){gf_MtReqDct[7].bf.req=true; gf_MtReqDct[7].bf.val=(UINT16)atoi(&g_CmdBuf[arg2pos]);}
MasashiNomura 25:f3a6e7eec9c3 238
MasashiNomura 25:f3a6e7eec9c3 239 //キャリブレーション
MasashiNomura 25:f3a6e7eec9c3 240 else if (strcmp(g_CmdBuf , "calyaw")==0 ){gf_Cal.bf.yaw=true;}//
MasashiNomura 25:f3a6e7eec9c3 241 else if (strcmp(g_CmdBuf , "calgyro")==0 ){gf_Cal.bf.gy=true;}//
MasashiNomura 25:f3a6e7eec9c3 242 else if (strcmp(g_CmdBuf , "calimu")==0 ){gf_Cal.bf.gy=true;gf_Cal.bf.yaw=true;}//
MasashiNomura 25:f3a6e7eec9c3 243
MasashiNomura 25:f3a6e7eec9c3 244 else if (strcmp(g_CmdBuf , "spidpp")==0 ){g_PidPara.PP = atof(&g_CmdBuf[arg2pos]);gf_PidParaUpdate=true;}//PIDの係数 Pp設定
MasashiNomura 25:f3a6e7eec9c3 245 else if (strcmp(g_CmdBuf , "spidp")==0 ){g_PidPara.P = atof(&g_CmdBuf[arg2pos]);gf_PidParaUpdate=true;}//PIDの係数 P設定
MasashiNomura 25:f3a6e7eec9c3 246 else if (strcmp(g_CmdBuf , "spidi")==0 ){g_PidPara.I = atof(&g_CmdBuf[arg2pos]);gf_PidParaUpdate=true;}//PIDの係数 I設定
MasashiNomura 25:f3a6e7eec9c3 247 else if (strcmp(g_CmdBuf , "spidd")==0 ){g_PidPara.D = atof(&g_CmdBuf[arg2pos]);gf_PidParaUpdate=true;}//PIDの係数 D設定
MasashiNomura 25:f3a6e7eec9c3 248 else if (strcmp(g_CmdBuf , "spidimax")==0 ){g_PidPara.IMax = atof(&g_CmdBuf[arg2pos]);gf_PidParaUpdate=true;}//PIDの係数 積分上限値設定
MasashiNomura 25:f3a6e7eec9c3 249 else if (strcmp(g_CmdBuf , "spidimin")==0 ){g_PidPara.IMin = atof(&g_CmdBuf[arg2pos]);gf_PidParaUpdate=true;}//PIDの係数 積分下限値設定
MasashiNomura 29:eb3d72dd94aa 250
MasashiNomura 29:eb3d72dd94aa 251 //else if (strcmp(g_CmdBuf , "op1" )==0 ){gf_MtReqOP[0].bf.req=true; gf_MtReqOP[0].bf.counter = 0; gf_MtReqOP[0].bf.num = atoi(&g_CmdBuf[arg2pos]);}//モーター1一時的噴射
MasashiNomura 29:eb3d72dd94aa 252 //else if (strcmp(g_CmdBuf , "op2" )==0 ){gf_MtReqOP[1].bf.req=true; gf_MtReqOP[1].bf.counter = 0; gf_MtReqOP[1].bf.num = atoi(&g_CmdBuf[arg2pos]);}//モーター2一時的噴射
MasashiNomura 29:eb3d72dd94aa 253 //else if (strcmp(g_CmdBuf , "op3" )==0 ){gf_MtReqOP[2].bf.req=true; gf_MtReqOP[2].bf.counter = 0; gf_MtReqOP[2].bf.num = atoi(&g_CmdBuf[arg2pos]);}//モーター3一時的噴射
MasashiNomura 29:eb3d72dd94aa 254 //else if (strcmp(g_CmdBuf , "op4" )==0 ){gf_MtReqOP[3].bf.req=true; gf_MtReqOP[3].bf.counter = 0; gf_MtReqOP[3].bf.num = atoi(&g_CmdBuf[arg2pos]);}//モーター4一時的噴射
MasashiNomura 29:eb3d72dd94aa 255 else if (strcmp(g_CmdBuf , "op1" )==0 ){gf_MtReqOP[0].req=true; gf_MtReqOP[0].add_end=false; gf_MtReqOP[0].counter = 0; gf_MtReqOP[0].num = atoi(&g_CmdBuf[arg2pos]);}//モーター1一時的噴射
MasashiNomura 29:eb3d72dd94aa 256 else if (strcmp(g_CmdBuf , "op2" )==0 ){gf_MtReqOP[1].req=true; gf_MtReqOP[1].add_end=false; gf_MtReqOP[1].counter = 0; gf_MtReqOP[1].num = atoi(&g_CmdBuf[arg2pos]);}//モーター2一時的噴射
MasashiNomura 29:eb3d72dd94aa 257 else if (strcmp(g_CmdBuf , "op3" )==0 ){gf_MtReqOP[2].req=true; gf_MtReqOP[2].add_end=false; gf_MtReqOP[2].counter = 0; gf_MtReqOP[2].num = atoi(&g_CmdBuf[arg2pos]);}//モーター3一時的噴射
MasashiNomura 29:eb3d72dd94aa 258 else if (strcmp(g_CmdBuf , "op4" )==0 ){gf_MtReqOP[3].req=true; gf_MtReqOP[3].add_end=false; gf_MtReqOP[3].counter = 0; gf_MtReqOP[3].num = atoi(&g_CmdBuf[arg2pos]);}//モーター4一時的噴射
MasashiNomura 22:24c9c2dedca9 259 //何にも引っ掛からない
MasashiNomura 22:24c9c2dedca9 260 else {sp.printf("command missmatch\r\n"); };
MasashiNomura 22:24c9c2dedca9 261 }
takeru0x1103 18:5aa48aec9cae 262
takeru0x1103 18:5aa48aec9cae 263 //バッファクリア
takeru0x1103 19:4b0fe9a5ec38 264 memset(&g_CmdBuf[0] , 0x00 , sizeof(g_CmdBuf) );
takeru0x1103 19:4b0fe9a5ec38 265
takeru0x1103 18:5aa48aec9cae 266 //フラグ下す
takeru0x1103 18:5aa48aec9cae 267 gf_CmdPrs = false;
takeru0x1103 18:5aa48aec9cae 268 }
takeru0x1103 18:5aa48aec9cae 269
takeru0x1103 18:5aa48aec9cae 270