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@50:76413e8e073d, 2019-02-26 (annotated)

Committer:
MasashiNomura
Date:
Tue Feb 26 08:44:06 2019 +0000
Revision:
50:76413e8e073d
Parent:
48:71aec693a7dc
Child:
53:b09c062cc31c
2019/02/26 for share. Add PID Offset and set-command.

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 41:45c982b1c5b6 34 else if (strcmp(g_CmdBuf , "re" )==0 ){gf_Print.d1.bf.e1=true; gf_Print.d1.bf.e2=true;}
MasashiNomura 41:45c982b1c5b6 35 else if (strcmp(g_CmdBuf , "re1" )==0 ){gf_Print.d1.bf.e1=true;}
MasashiNomura 41:45c982b1c5b6 36 else if (strcmp(g_CmdBuf , "re2" )==0 ){gf_Print.d1.bf.e2=true;}
MasashiNomura 42:cc8501b824ba 37 else if (strcmp(g_CmdBuf , "rep" )==0 ){gf_Print.d2.bf.ep1=true; gf_Print.d2.bf.ep2=true;}//エンジン用サーボにセットされた値
MasashiNomura 42:cc8501b824ba 38 else if (strcmp(g_CmdBuf , "rep1" )==0 ){gf_Print.d2.bf.ep1=true;}//前エンジン用サーボにセットされた値
MasashiNomura 42:cc8501b824ba 39 else if (strcmp(g_CmdBuf , "rep2" )==0 ){gf_Print.d2.bf.ep2=true;}//後エンジン用サーボにセットされた値
MasashiNomura 41:45c982b1c5b6 40 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;}//リードモーター
MasashiNomura 41:45c982b1c5b6 41 else if (strcmp(g_CmdBuf , "rm1" )==0 ){gf_Print.d1.bf.m1=true;}//リードモーター1
MasashiNomura 41:45c982b1c5b6 42 else if (strcmp(g_CmdBuf , "rm2" )==0 ){gf_Print.d1.bf.m2=true;}//リードモーター2
MasashiNomura 41:45c982b1c5b6 43 else if (strcmp(g_CmdBuf , "rm3" )==0 ){gf_Print.d1.bf.m3=true;}//リードモーター3
MasashiNomura 41:45c982b1c5b6 44 else if (strcmp(g_CmdBuf , "rm4" )==0 ){gf_Print.d1.bf.m4=true;}//リードモーター4
MasashiNomura 41:45c982b1c5b6 45 else if (strcmp(g_CmdBuf , "rg" )==0 ){gf_Print.d1.bf.gy=true;}//リードジャイロ
MasashiNomura 41:45c982b1c5b6 46 else if (strcmp(g_CmdBuf , "ry" )==0 ){gf_Print.d1.bf.yaw=true;}//リード姿勢角ヨー
MasashiNomura 41:45c982b1c5b6 47 else if (strcmp(g_CmdBuf , "rimu")==0 ){gf_Print.d1.bf.yaw=true;gf_Print.d1.bf.gy=true;}//リードIMU
MasashiNomura 41:45c982b1c5b6 48 else if (strcmp(g_CmdBuf , "rs" )==0 ){gf_Print.d2.bf.sw=true;}//リードスイッチ
MasashiNomura 41:45c982b1c5b6 49 else if (strcmp(g_CmdBuf , "rfb" )==0 ){gf_Print.d1.bf.fb=true;}//PID制御フィードバック結果
MasashiNomura 41:45c982b1c5b6 50 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;}//パラメータ全表示
MasashiNomura 41:45c982b1c5b6 51 else if (strcmp(g_CmdBuf , "rpP" )==0 ){gf_Print.d1.bf.pp=true;}//パラメータアウターPゲイン表示
MasashiNomura 41:45c982b1c5b6 52 else if (strcmp(g_CmdBuf , "rpp" )==0 ){gf_Print.d1.bf.p=true;}//パラメータPゲイン表示
MasashiNomura 41:45c982b1c5b6 53 else if (strcmp(g_CmdBuf , "rpi" )==0 ){gf_Print.d1.bf.i=true;}//パラメータIゲイン表示
MasashiNomura 41:45c982b1c5b6 54 else if (strcmp(g_CmdBuf , "rpd" )==0 ){gf_Print.d1.bf.d=true;}//パラメータDゲイン表示
MasashiNomura 48:71aec693a7dc 55 else if (strcmp(g_CmdBuf , "rmrpm" )==0 ){gf_Print.d2.bf.mrpm=true;}//Motorに支持されたRPM
MasashiNomura 22:24c9c2dedca9 56
MasashiNomura 22:24c9c2dedca9 57 //設定系
MasashiNomura 22:24c9c2dedca9 58 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 59 else if (strcmp(g_CmdBuf , "se2" )==0 ){gf_AxReq[1].bf.req=true; gf_AxReq[1].bf.val=(UINT16)atoi(&g_CmdBuf[arg2pos]);}
MasashiNomura 31:56c554c560c1 60 else if (strcmp(g_CmdBuf , "sep1" )==0 ){gf_AxReq[0].bf.val=(UINT16)atoi(&g_CmdBuf[arg2pos]);}
MasashiNomura 31:56c554c560c1 61 else if (strcmp(g_CmdBuf , "sep2" )==0 ){gf_AxReq[1].bf.val=(UINT16)atoi(&g_CmdBuf[arg2pos]);}
MasashiNomura 42:cc8501b824ba 62 else if (strcmp(g_CmdBuf , "seh1" )==0 ){gf_AxReqH[0].bf.req=true; gf_AxReqH[0].bf.val=(UINT16)atoi(&g_CmdBuf[arg2pos]);}
MasashiNomura 42:cc8501b824ba 63 else if (strcmp(g_CmdBuf , "seh2" )==0 ){gf_AxReqH[1].bf.req=true; gf_AxReqH[1].bf.val=(UINT16)atoi(&g_CmdBuf[arg2pos]);}
MasashiNomura 43:156199c2f9b6 64 else if (strcmp(g_CmdBuf , "ses1" )==0 ){gf_AxStepReq[0].bf.req=true; gf_AxStepReq[0].bf.val=(UINT16)atoi(&g_CmdBuf[arg2pos]);}
MasashiNomura 43:156199c2f9b6 65 else if (strcmp(g_CmdBuf , "ses2" )==0 ){gf_AxStepReq[1].bf.req=true; gf_AxStepReq[1].bf.val=(UINT16)atoi(&g_CmdBuf[arg2pos]);}
MasashiNomura 32:7f4145cc3551 66 else if (strcmp(g_CmdBuf , "sm1" )==0 ){gf_MtReq[0].req=true; gf_MtReq[0].val=(UINT16)atoi(&g_CmdBuf[arg2pos]);}
MasashiNomura 32:7f4145cc3551 67 else if (strcmp(g_CmdBuf , "sm2" )==0 ){gf_MtReq[1].req=true; gf_MtReq[1].val=(UINT16)atoi(&g_CmdBuf[arg2pos]);}
MasashiNomura 32:7f4145cc3551 68 else if (strcmp(g_CmdBuf , "sm3" )==0 ){gf_MtReq[2].req=true; gf_MtReq[2].val=(UINT16)atoi(&g_CmdBuf[arg2pos]);}
MasashiNomura 32:7f4145cc3551 69 else if (strcmp(g_CmdBuf , "sm4" )==0 ){gf_MtReq[3].req=true; gf_MtReq[3].val=(UINT16)atoi(&g_CmdBuf[arg2pos]);}
MasashiNomura 27:ff63c23bc689 70 else if (strcmp(g_CmdBuf , "sm" )==0 ){
MasashiNomura 27:ff63c23bc689 71 for(int i = 0; i < 4; ++i){
MasashiNomura 32:7f4145cc3551 72 gf_MtReq[i].req=true;
MasashiNomura 32:7f4145cc3551 73 gf_MtReq[i].val=(UINT16)atoi(&g_CmdBuf[arg2pos]);
MasashiNomura 27:ff63c23bc689 74 }
MasashiNomura 25:f3a6e7eec9c3 75 }
MasashiNomura 26:732bc37fbefd 76 else if (strcmp(g_CmdBuf , "smf" )==0 ){
MasashiNomura 32:7f4145cc3551 77 gf_MtReq[0].req=true;
MasashiNomura 32:7f4145cc3551 78 gf_MtReq[0].val=(UINT16)atoi(&g_CmdBuf[arg2pos]);
MasashiNomura 32:7f4145cc3551 79 gf_MtReq[1].req=true;
MasashiNomura 32:7f4145cc3551 80 gf_MtReq[1].val=(UINT16)atoi(&g_CmdBuf[arg2pos]);}
MasashiNomura 26:732bc37fbefd 81 else if (strcmp(g_CmdBuf , "smr" )==0 ){
MasashiNomura 32:7f4145cc3551 82 gf_MtReq[2].req=true;
MasashiNomura 32:7f4145cc3551 83 gf_MtReq[2].val=(UINT16)atoi(&g_CmdBuf[arg2pos]);
MasashiNomura 32:7f4145cc3551 84 gf_MtReq[3].req=true;
MasashiNomura 32:7f4145cc3551 85 gf_MtReq[3].val=(UINT16)atoi(&g_CmdBuf[arg2pos]); }
MasashiNomura 50:76413e8e073d 86 else if (strcmp(g_CmdBuf , "smao1" )==0 ){gf_MtAttOfs[0].req=true; gf_MtAttOfs[0].val=(UINT16)atoi(&g_CmdBuf[arg2pos]);}
MasashiNomura 50:76413e8e073d 87 else if (strcmp(g_CmdBuf , "smao2" )==0 ){gf_MtAttOfs[1].req=true; gf_MtAttOfs[1].val=(UINT16)atoi(&g_CmdBuf[arg2pos]);}
MasashiNomura 50:76413e8e073d 88 else if (strcmp(g_CmdBuf , "smao3" )==0 ){gf_MtAttOfs[2].req=true; gf_MtAttOfs[2].val=(UINT16)atoi(&g_CmdBuf[arg2pos]);}
MasashiNomura 50:76413e8e073d 89 else if (strcmp(g_CmdBuf , "smao4" )==0 ){gf_MtAttOfs[3].req=true; gf_MtAttOfs[3].val=(UINT16)atoi(&g_CmdBuf[arg2pos]);}
MasashiNomura 50:76413e8e073d 90 else if (strcmp(g_CmdBuf , "smao" )==0 ){
MasashiNomura 50:76413e8e073d 91 for(int i = 0; i < 4; ++i){
MasashiNomura 50:76413e8e073d 92 gf_MtAttOfs[i].req=true;
MasashiNomura 50:76413e8e073d 93 gf_MtAttOfs[i].val=(UINT16)atoi(&g_CmdBuf[arg2pos]);
MasashiNomura 50:76413e8e073d 94 }
MasashiNomura 50:76413e8e073d 95 }
MasashiNomura 32:7f4145cc3551 96 else if (strcmp(g_CmdBuf , "smo1" )==0 ){gf_MtReqOfs[0].req=true; gf_MtReqOfs[0].val=(UINT16)atoi(&g_CmdBuf[arg2pos]);}
MasashiNomura 32:7f4145cc3551 97 else if (strcmp(g_CmdBuf , "smo2" )==0 ){gf_MtReqOfs[1].req=true; gf_MtReqOfs[1].val=(UINT16)atoi(&g_CmdBuf[arg2pos]);}
MasashiNomura 32:7f4145cc3551 98 else if (strcmp(g_CmdBuf , "smo3" )==0 ){gf_MtReqOfs[2].req=true; gf_MtReqOfs[2].val=(UINT16)atoi(&g_CmdBuf[arg2pos]);}
MasashiNomura 32:7f4145cc3551 99 else if (strcmp(g_CmdBuf , "smo4" )==0 ){gf_MtReqOfs[3].req=true; gf_MtReqOfs[3].val=(UINT16)atoi(&g_CmdBuf[arg2pos]);}
MasashiNomura 27:ff63c23bc689 100 else if (strcmp(g_CmdBuf , "smo" )==0 ){
MasashiNomura 27:ff63c23bc689 101 for(int i = 0; i < 4; ++i){
MasashiNomura 32:7f4145cc3551 102 gf_MtReqOfs[i].req=true;
MasashiNomura 32:7f4145cc3551 103 gf_MtReqOfs[i].val=(UINT16)atoi(&g_CmdBuf[arg2pos]);
MasashiNomura 27:ff63c23bc689 104 }
MasashiNomura 27:ff63c23bc689 105 }
MasashiNomura 27:ff63c23bc689 106 else if (strcmp(g_CmdBuf , "smof" )==0 ){
MasashiNomura 32:7f4145cc3551 107 gf_MtReqOfs[0].req=true;
MasashiNomura 32:7f4145cc3551 108 gf_MtReqOfs[0].val=(UINT16)atoi(&g_CmdBuf[arg2pos]);
MasashiNomura 32:7f4145cc3551 109 gf_MtReqOfs[1].req=true;
MasashiNomura 32:7f4145cc3551 110 gf_MtReqOfs[1].val=(UINT16)atoi(&g_CmdBuf[arg2pos]);}
MasashiNomura 27:ff63c23bc689 111 else if (strcmp(g_CmdBuf , "smor" )==0 ){
MasashiNomura 32:7f4145cc3551 112 gf_MtReqOfs[2].req=true;
MasashiNomura 32:7f4145cc3551 113 gf_MtReqOfs[2].val=(UINT16)atoi(&g_CmdBuf[arg2pos]);
MasashiNomura 32:7f4145cc3551 114 gf_MtReqOfs[3].req=true;
MasashiNomura 32:7f4145cc3551 115 gf_MtReqOfs[3].val=(UINT16)atoi(&g_CmdBuf[arg2pos]); }
MasashiNomura 27:ff63c23bc689 116
MasashiNomura 22:24c9c2dedca9 117 //キャリブレーション
MasashiNomura 22:24c9c2dedca9 118 else if (strcmp(g_CmdBuf , "calyaw")==0 ){gf_Cal.bf.yaw=true;}//
MasashiNomura 22:24c9c2dedca9 119 else if (strcmp(g_CmdBuf , "calgyro")==0 ){gf_Cal.bf.gy=true;}//
MasashiNomura 22:24c9c2dedca9 120 else if (strcmp(g_CmdBuf , "calimu")==0 ){gf_Cal.bf.gy=true;gf_Cal.bf.yaw=true;}//
MasashiNomura 22:24c9c2dedca9 121
MasashiNomura 22:24c9c2dedca9 122 //チェック系
MasashiNomura 22:24c9c2dedca9 123
MasashiNomura 22:24c9c2dedca9 124 //連続モニタ
MasashiNomura 41:45c982b1c5b6 125 else if (strcmp(g_CmdBuf , "mg" )==0 ){gf_Print.d1.bf.gy=true;gf_Mon.d1.bf.gy=true;}//リードジャイロ
MasashiNomura 41:45c982b1c5b6 126 else if (strcmp(g_CmdBuf , "my" )==0 ){gf_Print.d1.bf.yaw=true;gf_Mon.d1.bf.yaw=true;}//リード姿勢角ヨー
MasashiNomura 41:45c982b1c5b6 127 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
MasashiNomura 41:45c982b1c5b6 128 else if (strcmp(g_CmdBuf , "mfb" )==0 ){gf_Print.d1.bf.fb=true;gf_Mon.d1.bf.fb=true;}//PID制御フィードバック結果
MasashiNomura 41:45c982b1c5b6 129 else if (strcmp(g_CmdBuf , "mm1" ) == 0 ){gf_Mon.d1.bf.m1=true;}
MasashiNomura 41:45c982b1c5b6 130 else if (strcmp(g_CmdBuf , "mm2" ) == 0 ){gf_Mon.d1.bf.m2=true;}
MasashiNomura 41:45c982b1c5b6 131 else if (strcmp(g_CmdBuf , "mm3" ) == 0 ){gf_Mon.d1.bf.m3=true;}
MasashiNomura 41:45c982b1c5b6 132 else if (strcmp(g_CmdBuf , "mm4" ) == 0 ){gf_Mon.d1.bf.m4=true;}
MasashiNomura 41:45c982b1c5b6 133 else if (strcmp(g_CmdBuf , "msw" ) == 0 ){gf_Print.d2.bf.sw=true;gf_Mon.d2.bf.sw=true;}
MasashiNomura 41:45c982b1c5b6 134 else if (strcmp(g_CmdBuf , "mana1" ) == 0 ){gf_Print.d2.bf.ain=true;gf_Mon.d2.bf.ain=true;}
MasashiNomura 48:71aec693a7dc 135 else if (strcmp(g_CmdBuf , "mmrpm" ) == 0 ){gf_Print.d2.bf.mrpm=true;gf_Mon.d2.bf.mrpm=true;}
MasashiNomura 41:45c982b1c5b6 136 else if (strcmp(g_CmdBuf , "kill")==0 ){gf_Mon.d1.flg=0;gf_Mon.d2.flg=0;}//モニタ停止
MasashiNomura 22:24c9c2dedca9 137
MasashiNomura 24:c5945aaae777 138 // 開発、デバッグ用コマンド
MasashiNomura 41:45c982b1c5b6 139 else if (strcmp(g_CmdBuf , "stat")==0 ){gf_Print.d1.bf.stat=true;}//ステート表示
MasashiNomura 23:79e20be4bc5b 140 else if (strcmp(g_CmdBuf , "sstatf")==0 ){setStateF((enmHbState)atoi(&g_CmdBuf[arg2pos]));}//ステート強制遷移
MasashiNomura 25:f3a6e7eec9c3 141
MasashiNomura 42:cc8501b824ba 142 else if (strcmp(g_CmdBuf , "sw0")==0 ){gf_SwCmd.bf.req=true;gf_SwCmd.bf.val=0;}//SW 0bit目をON
MasashiNomura 42:cc8501b824ba 143 else if (strcmp(g_CmdBuf , "sw1")==0 ){gf_SwCmd.bf.req=true;gf_SwCmd.bf.val=1;}//SW 1bit目をON
MasashiNomura 42:cc8501b824ba 144 else if (strcmp(g_CmdBuf , "sw2")==0 ){gf_SwCmd.bf.req=true;gf_SwCmd.bf.val=2;}//SW 2bit目をON
MasashiNomura 42:cc8501b824ba 145 else if (strcmp(g_CmdBuf , "sw3")==0 ){gf_SwCmd.bf.req=true;gf_SwCmd.bf.val=3;}//SW 3bit目をON
MasashiNomura 42:cc8501b824ba 146 else if (strcmp(g_CmdBuf , "sw4")==0 ){gf_SwCmd.bf.req=true;gf_SwCmd.bf.val=4;}//SW 4bit目をON
MasashiNomura 42:cc8501b824ba 147 else if (strcmp(g_CmdBuf , "sw5")==0 ){gf_SwCmd.bf.req=true;gf_SwCmd.bf.val=5;}//SW 5bit目をON
MasashiNomura 42:cc8501b824ba 148 else if (strcmp(g_CmdBuf , "sw6")==0 ){gf_SwCmd.bf.req=true;gf_SwCmd.bf.val=6;}//SW 6bit目をON
MasashiNomura 42:cc8501b824ba 149 else if (strcmp(g_CmdBuf , "sw7")==0 ){gf_SwCmd.bf.req=true;gf_SwCmd.bf.val=7;}//SW 7bit目をON
MasashiNomura 42:cc8501b824ba 150 else if (strcmp(g_CmdBuf , "sw8")==0 ){gf_SwCmd.bf.req=true;gf_SwCmd.bf.val=8;}//SW 8bit目をON
MasashiNomura 42:cc8501b824ba 151 else if (strcmp(g_CmdBuf , "sw9")==0 ){gf_SwCmd.bf.req=true;gf_SwCmd.bf.val=9;}//SW 9bit目をON
MasashiNomura 42:cc8501b824ba 152
MasashiNomura 25:f3a6e7eec9c3 153 else if (strcmp(g_CmdBuf , "stopmot")==0 ){gf_StopMot = true;}//モーター強制停止
MasashiNomura 25:f3a6e7eec9c3 154
MasashiNomura 24:c5945aaae777 155 else if (strcmp(g_CmdBuf , "spidpp")==0 ){g_PidPara.PP = atof(&g_CmdBuf[arg2pos]);gf_PidParaUpdate=true;}//PIDの係数 Pp設定
MasashiNomura 24:c5945aaae777 156 else if (strcmp(g_CmdBuf , "spidp")==0 ){g_PidPara.P = atof(&g_CmdBuf[arg2pos]);gf_PidParaUpdate=true;}//PIDの係数 P設定
MasashiNomura 24:c5945aaae777 157 else if (strcmp(g_CmdBuf , "spidi")==0 ){g_PidPara.I = atof(&g_CmdBuf[arg2pos]);gf_PidParaUpdate=true;}//PIDの係数 I設定
MasashiNomura 24:c5945aaae777 158 else if (strcmp(g_CmdBuf , "spidd")==0 ){g_PidPara.D = atof(&g_CmdBuf[arg2pos]);gf_PidParaUpdate=true;}//PIDの係数 D設定
MasashiNomura 24:c5945aaae777 159 else if (strcmp(g_CmdBuf , "spidimax")==0 ){g_PidPara.IMax = atof(&g_CmdBuf[arg2pos]);gf_PidParaUpdate=true;}//PIDの係数 積分上限値設定
MasashiNomura 24:c5945aaae777 160 else if (strcmp(g_CmdBuf , "spidimin")==0 ){g_PidPara.IMin = atof(&g_CmdBuf[arg2pos]);gf_PidParaUpdate=true;}//PIDの係数 積分下限値設定
MasashiNomura 31:56c554c560c1 161 else if (strcmp(g_CmdBuf , "spidv")==0 ){g_PidPara.V = atof(&g_CmdBuf[arg2pos]);gf_PidParaUpdate=true;}//PID2の角速度の係数
MasashiNomura 48:71aec693a7dc 162 else if (strcmp(g_CmdBuf , "spidmode" )==0 ){g_PidPara.mode = (ePIDMode)atoi(&g_CmdBuf[arg2pos]);gf_PidParaUpdate=true;}//PIDのモード
MasashiNomura 42:cc8501b824ba 163
MasashiNomura 41:45c982b1c5b6 164 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;}//リードモーターオフセット
MasashiNomura 41:45c982b1c5b6 165 else if (strcmp(g_CmdBuf , "rmo1" )==0 ){gf_Print.d2.bf.mo1=true;}//リードモーターオフセット1
MasashiNomura 41:45c982b1c5b6 166 else if (strcmp(g_CmdBuf , "rmo2" )==0 ){gf_Print.d2.bf.mo2=true;}//リードモーターオフセット2
MasashiNomura 41:45c982b1c5b6 167 else if (strcmp(g_CmdBuf , "rmo3" )==0 ){gf_Print.d2.bf.mo3=true;}//リードモーターオフセット3
MasashiNomura 41:45c982b1c5b6 168 else if (strcmp(g_CmdBuf , "rmo4" )==0 ){gf_Print.d2.bf.mo4=true;}//リードモーターオフセット4
MasashiNomura 41:45c982b1c5b6 169 else if (strcmp(g_CmdBuf , "rana1" )==0 ){gf_Print.d2.bf.ain=true;}//アナログ入力リード forモーターアクセル
MasashiNomura 48:71aec693a7dc 170
MasashiNomura 48:71aec693a7dc 171
MasashiNomura 32:7f4145cc3551 172
MasashiNomura 22:24c9c2dedca9 173 //何にも引っ掛からない
MasashiNomura 22:24c9c2dedca9 174 else {sp.printf("command missmatch\r\n"); };
MasashiNomura 22:24c9c2dedca9 175 } else if(gf_State == SLEEP){
MasashiNomura 25:f3a6e7eec9c3 176 //EMG STOP 現在はモーターの強制停止
MasashiNomura 25:f3a6e7eec9c3 177 if (g_CmdBuf[0] == ESC){gf_StopMot = true;}
MasashiNomura 25:f3a6e7eec9c3 178 else if (strcmp(g_CmdBuf , "sarm" )==0 ){gf_Armed = true;}//アーミングフラグセット テスト用
MasashiNomura 23:79e20be4bc5b 179 else if (strcmp(g_CmdBuf , "dbg" )==0 ){if(gf_Dbg != true) gf_Dbg = true;}//デバッグタスク起動
MasashiNomura 41:45c982b1c5b6 180 else if (strcmp(g_CmdBuf , "rs" )==0 ){gf_Print.d2.bf.sw=true;}//リードスイッチ
MasashiNomura 41:45c982b1c5b6 181
MasashiNomura 41:45c982b1c5b6 182 else if (strcmp(g_CmdBuf , "stat")==0 ){gf_Print.d1.bf.stat=true;}//ステート表示
MasashiNomura 23:79e20be4bc5b 183 else if (strcmp(g_CmdBuf , "sstatf")==0 ){setStateF((enmHbState)atoi(&g_CmdBuf[arg2pos]));}//ステート強制遷移
MasashiNomura 41:45c982b1c5b6 184
MasashiNomura 45:3b51dd26b579 185 else if (strcmp(g_CmdBuf , "re" )==0 ){gf_Print.d1.bf.e1=true; gf_Print.d1.bf.e2=true;}
MasashiNomura 45:3b51dd26b579 186 else if (strcmp(g_CmdBuf , "re1" )==0 ){gf_Print.d1.bf.e1=true;}
MasashiNomura 45:3b51dd26b579 187 else if (strcmp(g_CmdBuf , "re2" )==0 ){gf_Print.d1.bf.e2=true;}
MasashiNomura 45:3b51dd26b579 188
MasashiNomura 42:cc8501b824ba 189 else if (strcmp(g_CmdBuf , "sw0")==0 ){gf_SwCmd.bf.req=true;gf_SwCmd.bf.val=0;}//SW 0bit目をON
MasashiNomura 42:cc8501b824ba 190 else if (strcmp(g_CmdBuf , "sw1")==0 ){gf_SwCmd.bf.req=true;gf_SwCmd.bf.val=1;}//SW 1bit目をON
MasashiNomura 42:cc8501b824ba 191 else if (strcmp(g_CmdBuf , "sw2")==0 ){gf_SwCmd.bf.req=true;gf_SwCmd.bf.val=2;}//SW 2bit目をON
MasashiNomura 42:cc8501b824ba 192 else if (strcmp(g_CmdBuf , "sw3")==0 ){gf_SwCmd.bf.req=true;gf_SwCmd.bf.val=3;}//SW 3bit目をON
MasashiNomura 42:cc8501b824ba 193 else if (strcmp(g_CmdBuf , "sw4")==0 ){gf_SwCmd.bf.req=true;gf_SwCmd.bf.val=4;}//SW 4bit目をON
MasashiNomura 42:cc8501b824ba 194 else if (strcmp(g_CmdBuf , "sw5")==0 ){gf_SwCmd.bf.req=true;gf_SwCmd.bf.val=5;}//SW 5bit目をON
MasashiNomura 42:cc8501b824ba 195 else if (strcmp(g_CmdBuf , "sw6")==0 ){gf_SwCmd.bf.req=true;gf_SwCmd.bf.val=6;}//SW 6bit目をON
MasashiNomura 42:cc8501b824ba 196 else if (strcmp(g_CmdBuf , "sw7")==0 ){gf_SwCmd.bf.req=true;gf_SwCmd.bf.val=7;}//SW 7bit目をON
MasashiNomura 42:cc8501b824ba 197 else if (strcmp(g_CmdBuf , "sw8")==0 ){gf_SwCmd.bf.req=true;gf_SwCmd.bf.val=8;}//SW 8bit目をON
MasashiNomura 42:cc8501b824ba 198 else if (strcmp(g_CmdBuf , "sw9")==0 ){gf_SwCmd.bf.req=true;gf_SwCmd.bf.val=9;}//SW 9bit目をON
MasashiNomura 42:cc8501b824ba 199
MasashiNomura 43:156199c2f9b6 200 else if (strcmp(g_CmdBuf , "ses1" )==0 ){gf_AxStepReq[0].bf.req=true; gf_AxStepReq[0].bf.val=(UINT16)atoi(&g_CmdBuf[arg2pos]);}
MasashiNomura 43:156199c2f9b6 201 else if (strcmp(g_CmdBuf , "ses2" )==0 ){gf_AxStepReq[1].bf.req=true; gf_AxStepReq[1].bf.val=(UINT16)atoi(&g_CmdBuf[arg2pos]);}
MasashiNomura 43:156199c2f9b6 202
MasashiNomura 32:7f4145cc3551 203 else if (strcmp(g_CmdBuf , "sm1" )==0 ){gf_MtReq[0].req=true; gf_MtReq[0].val=(UINT16)atoi(&g_CmdBuf[arg2pos]);}
MasashiNomura 32:7f4145cc3551 204 else if (strcmp(g_CmdBuf , "sm2" )==0 ){gf_MtReq[1].req=true; gf_MtReq[1].val=(UINT16)atoi(&g_CmdBuf[arg2pos]);}
MasashiNomura 32:7f4145cc3551 205 else if (strcmp(g_CmdBuf , "sm3" )==0 ){gf_MtReq[2].req=true; gf_MtReq[2].val=(UINT16)atoi(&g_CmdBuf[arg2pos]);}
MasashiNomura 32:7f4145cc3551 206 else if (strcmp(g_CmdBuf , "sm4" )==0 ){gf_MtReq[3].req=true; gf_MtReq[3].val=(UINT16)atoi(&g_CmdBuf[arg2pos]);}
MasashiNomura 27:ff63c23bc689 207 else if (strcmp(g_CmdBuf , "sm" )==0 ){
MasashiNomura 27:ff63c23bc689 208 for(int i = 0; i < 4; ++i){
MasashiNomura 32:7f4145cc3551 209 gf_MtReq[i].req=true;
MasashiNomura 32:7f4145cc3551 210 gf_MtReq[i].val=(UINT16)atoi(&g_CmdBuf[arg2pos]);
MasashiNomura 27:ff63c23bc689 211 }
MasashiNomura 25:f3a6e7eec9c3 212 }
MasashiNomura 26:732bc37fbefd 213 else if (strcmp(g_CmdBuf , "smf" )==0 ){
MasashiNomura 32:7f4145cc3551 214 gf_MtReq[0].req=true;
MasashiNomura 32:7f4145cc3551 215 gf_MtReq[0].val=(UINT16)atoi(&g_CmdBuf[arg2pos]);
MasashiNomura 32:7f4145cc3551 216 gf_MtReq[1].req=true;
MasashiNomura 32:7f4145cc3551 217 gf_MtReq[1].val=(UINT16)atoi(&g_CmdBuf[arg2pos]);}
MasashiNomura 26:732bc37fbefd 218 else if (strcmp(g_CmdBuf , "smr" )==0 ){
MasashiNomura 32:7f4145cc3551 219 gf_MtReq[2].req=true;
MasashiNomura 32:7f4145cc3551 220 gf_MtReq[2].val=(UINT16)atoi(&g_CmdBuf[arg2pos]);
MasashiNomura 32:7f4145cc3551 221 gf_MtReq[3].req=true;
MasashiNomura 32:7f4145cc3551 222 gf_MtReq[3].val=(UINT16)atoi(&g_CmdBuf[arg2pos]); }
MasashiNomura 32:7f4145cc3551 223 else if (strcmp(g_CmdBuf , "smo1" )==0 ){gf_MtReqOfs[0].req=true; gf_MtReqOfs[0].val=(UINT16)atoi(&g_CmdBuf[arg2pos]);}
MasashiNomura 32:7f4145cc3551 224 else if (strcmp(g_CmdBuf , "smo2" )==0 ){gf_MtReqOfs[1].req=true; gf_MtReqOfs[1].val=(UINT16)atoi(&g_CmdBuf[arg2pos]);}
MasashiNomura 32:7f4145cc3551 225 else if (strcmp(g_CmdBuf , "smo3" )==0 ){gf_MtReqOfs[2].req=true; gf_MtReqOfs[2].val=(UINT16)atoi(&g_CmdBuf[arg2pos]);}
MasashiNomura 32:7f4145cc3551 226 else if (strcmp(g_CmdBuf , "smo4" )==0 ){gf_MtReqOfs[3].req=true; gf_MtReqOfs[3].val=(UINT16)atoi(&g_CmdBuf[arg2pos]);}
MasashiNomura 27:ff63c23bc689 227 else if (strcmp(g_CmdBuf , "smo" )==0 ){
MasashiNomura 27:ff63c23bc689 228 for(int i = 0; i < 4; ++i){
MasashiNomura 32:7f4145cc3551 229 gf_MtReqOfs[i].req=true;
MasashiNomura 32:7f4145cc3551 230 gf_MtReqOfs[i].val=(UINT16)atoi(&g_CmdBuf[arg2pos]);
MasashiNomura 27:ff63c23bc689 231 }
MasashiNomura 27:ff63c23bc689 232 }
MasashiNomura 27:ff63c23bc689 233 else if (strcmp(g_CmdBuf , "smof" )==0 ){
MasashiNomura 32:7f4145cc3551 234 gf_MtReqOfs[0].req=true;
MasashiNomura 32:7f4145cc3551 235 gf_MtReqOfs[0].val=(UINT16)atoi(&g_CmdBuf[arg2pos]);
MasashiNomura 32:7f4145cc3551 236 gf_MtReqOfs[1].req=true;
MasashiNomura 32:7f4145cc3551 237 gf_MtReqOfs[1].val=(UINT16)atoi(&g_CmdBuf[arg2pos]);}
MasashiNomura 27:ff63c23bc689 238 else if (strcmp(g_CmdBuf , "smor" )==0 ){
MasashiNomura 32:7f4145cc3551 239 gf_MtReqOfs[2].req=true;
MasashiNomura 32:7f4145cc3551 240 gf_MtReqOfs[2].val=(UINT16)atoi(&g_CmdBuf[arg2pos]);
MasashiNomura 32:7f4145cc3551 241 gf_MtReqOfs[3].req=true;
MasashiNomura 40:debe99e228d3 242 gf_MtReqOfs[3].val=(UINT16)atoi(&g_CmdBuf[arg2pos]);}
MasashiNomura 50:76413e8e073d 243 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
MasashiNomura 50:76413e8e073d 244 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
MasashiNomura 50:76413e8e073d 245 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
MasashiNomura 50:76413e8e073d 246 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
MasashiNomura 50:76413e8e073d 247 else if (strcmp(g_CmdBuf , "smao" )==0 ){//!< Set Motor Attitude Offset
MasashiNomura 50:76413e8e073d 248 for(int i = 0; i < 4; ++i){
MasashiNomura 50:76413e8e073d 249 gf_MtAttOfs[i].req=true;
MasashiNomura 50:76413e8e073d 250 gf_MtAttOfs[i].val=(UINT16)atoi(&g_CmdBuf[arg2pos]);
MasashiNomura 50:76413e8e073d 251 }
MasashiNomura 50:76413e8e073d 252 }
MasashiNomura 32:7f4145cc3551 253 else if (strcmp(g_CmdBuf , "smfpga1" )==0 ){gf_MtReqDct[0].req=true; gf_MtReqDct[0].val=(UINT16)atoi(&g_CmdBuf[arg2pos]);}
MasashiNomura 32:7f4145cc3551 254 else if (strcmp(g_CmdBuf , "smfpga2" )==0 ){gf_MtReqDct[1].req=true; gf_MtReqDct[1].val=(UINT16)atoi(&g_CmdBuf[arg2pos]);}
MasashiNomura 32:7f4145cc3551 255 else if (strcmp(g_CmdBuf , "smfpga3" )==0 ){gf_MtReqDct[2].req=true; gf_MtReqDct[2].val=(UINT16)atoi(&g_CmdBuf[arg2pos]);}
MasashiNomura 32:7f4145cc3551 256 else if (strcmp(g_CmdBuf , "smfpga4" )==0 ){gf_MtReqDct[3].req=true; gf_MtReqDct[3].val=(UINT16)atoi(&g_CmdBuf[arg2pos]);}
MasashiNomura 32:7f4145cc3551 257 else if (strcmp(g_CmdBuf , "smfpga5" )==0 ){gf_MtReqDct[4].req=true; gf_MtReqDct[4].val=(UINT16)atoi(&g_CmdBuf[arg2pos]);}
MasashiNomura 32:7f4145cc3551 258 else if (strcmp(g_CmdBuf , "smfpga6" )==0 ){gf_MtReqDct[5].req=true; gf_MtReqDct[5].val=(UINT16)atoi(&g_CmdBuf[arg2pos]);}
MasashiNomura 32:7f4145cc3551 259 else if (strcmp(g_CmdBuf , "smfpga7" )==0 ){gf_MtReqDct[6].req=true; gf_MtReqDct[6].val=(UINT16)atoi(&g_CmdBuf[arg2pos]);}
MasashiNomura 32:7f4145cc3551 260 else if (strcmp(g_CmdBuf , "smfpga8" )==0 ){gf_MtReqDct[7].req=true; gf_MtReqDct[7].val=(UINT16)atoi(&g_CmdBuf[arg2pos]);}
MasashiNomura 25:f3a6e7eec9c3 261
MasashiNomura 25:f3a6e7eec9c3 262 //キャリブレーション
MasashiNomura 25:f3a6e7eec9c3 263 else if (strcmp(g_CmdBuf , "calyaw")==0 ){gf_Cal.bf.yaw=true;}//
MasashiNomura 25:f3a6e7eec9c3 264 else if (strcmp(g_CmdBuf , "calgyro")==0 ){gf_Cal.bf.gy=true;}//
MasashiNomura 25:f3a6e7eec9c3 265 else if (strcmp(g_CmdBuf , "calimu")==0 ){gf_Cal.bf.gy=true;gf_Cal.bf.yaw=true;}//
MasashiNomura 25:f3a6e7eec9c3 266
MasashiNomura 25:f3a6e7eec9c3 267 else if (strcmp(g_CmdBuf , "spidpp")==0 ){g_PidPara.PP = atof(&g_CmdBuf[arg2pos]);gf_PidParaUpdate=true;}//PIDの係数 Pp設定
MasashiNomura 25:f3a6e7eec9c3 268 else if (strcmp(g_CmdBuf , "spidp")==0 ){g_PidPara.P = atof(&g_CmdBuf[arg2pos]);gf_PidParaUpdate=true;}//PIDの係数 P設定
MasashiNomura 25:f3a6e7eec9c3 269 else if (strcmp(g_CmdBuf , "spidi")==0 ){g_PidPara.I = atof(&g_CmdBuf[arg2pos]);gf_PidParaUpdate=true;}//PIDの係数 I設定
MasashiNomura 25:f3a6e7eec9c3 270 else if (strcmp(g_CmdBuf , "spidd")==0 ){g_PidPara.D = atof(&g_CmdBuf[arg2pos]);gf_PidParaUpdate=true;}//PIDの係数 D設定
MasashiNomura 25:f3a6e7eec9c3 271 else if (strcmp(g_CmdBuf , "spidimax")==0 ){g_PidPara.IMax = atof(&g_CmdBuf[arg2pos]);gf_PidParaUpdate=true;}//PIDの係数 積分上限値設定
MasashiNomura 25:f3a6e7eec9c3 272 else if (strcmp(g_CmdBuf , "spidimin")==0 ){g_PidPara.IMin = atof(&g_CmdBuf[arg2pos]);gf_PidParaUpdate=true;}//PIDの係数 積分下限値設定
MasashiNomura 31:56c554c560c1 273 else if (strcmp(g_CmdBuf , "spidv")==0 ){g_PidPara.V = atof(&g_CmdBuf[arg2pos]);gf_PidParaUpdate=true;}//PID2の角速度の係数
MasashiNomura 48:71aec693a7dc 274 else if (strcmp(g_CmdBuf , "spidmode" )==0 ){g_PidPara.mode = (ePIDMode)atoi(&g_CmdBuf[arg2pos]);gf_PidParaUpdate=true;}//PIDのモード
MasashiNomura 29:eb3d72dd94aa 275
MasashiNomura 41:45c982b1c5b6 276 else if (strcmp(g_CmdBuf , "rana1" )==0 ){gf_Print.d2.bf.ain=true;}//アナログ入力リード forモーターアクセル
MasashiNomura 38:24ee50452755 277
MasashiNomura 41:45c982b1c5b6 278 else if (strcmp(g_CmdBuf , "msw" ) == 0 ){gf_Print.d2.bf.sw=true;gf_Mon.d2.bf.sw=true;}
MasashiNomura 41:45c982b1c5b6 279 else if (strcmp(g_CmdBuf , "mana1" ) == 0 ){gf_Print.d2.bf.ain=true;gf_Mon.d2.bf.ain=true;}
MasashiNomura 41:45c982b1c5b6 280 else if (strcmp(g_CmdBuf , "kill")==0 ){gf_Mon.d1.flg=0;gf_Mon.d2.flg=0;}//モニタ停止
MasashiNomura 22:24c9c2dedca9 281 //何にも引っ掛からない
MasashiNomura 22:24c9c2dedca9 282 else {sp.printf("command missmatch\r\n"); };
MasashiNomura 22:24c9c2dedca9 283 }
takeru0x1103 18:5aa48aec9cae 284
takeru0x1103 18:5aa48aec9cae 285 //バッファクリア
takeru0x1103 19:4b0fe9a5ec38 286 memset(&g_CmdBuf[0] , 0x00 , sizeof(g_CmdBuf) );
takeru0x1103 19:4b0fe9a5ec38 287
takeru0x1103 18:5aa48aec9cae 288 //フラグ下す
takeru0x1103 18:5aa48aec9cae 289 gf_CmdPrs = false;
takeru0x1103 18:5aa48aec9cae 290 }
takeru0x1103 18:5aa48aec9cae 291
takeru0x1103 18:5aa48aec9cae 292