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@18:5aa48aec9cae, 2018-12-01 (annotated)

Committer:
takeru0x1103
Date:
Sat Dec 01 14:03:08 2018 +0000
Revision:
18:5aa48aec9cae
Child:
19:4b0fe9a5ec38
??????????PID????????????

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>
takeru0x1103 18:5aa48aec9cae 3
takeru0x1103 18:5aa48aec9cae 4 #include "globalFlags.h"
takeru0x1103 18:5aa48aec9cae 5 #include "hbCommand.h"
takeru0x1103 18:5aa48aec9cae 6
takeru0x1103 18:5aa48aec9cae 7 //受信文字を貯めるコマンドバッファのサイズ
takeru0x1103 18:5aa48aec9cae 8 #define BUFFER_INDEX_END 31
takeru0x1103 18:5aa48aec9cae 9
takeru0x1103 18:5aa48aec9cae 10 //-------------------------------------------------------------
takeru0x1103 18:5aa48aec9cae 11 //グローバル変数
takeru0x1103 18:5aa48aec9cae 12 //-------------------------------------------------------------
takeru0x1103 18:5aa48aec9cae 13 static char cmdBuf[BUFFER_INDEX_END] ={0,} ;// コマンドバッファ
takeru0x1103 18:5aa48aec9cae 14 static UCHAR bp =0 ;// バッファポインタ
takeru0x1103 18:5aa48aec9cae 15
takeru0x1103 18:5aa48aec9cae 16 //-------------------------------------------------------------
takeru0x1103 18:5aa48aec9cae 17 //受信コマンドバッファクリア
takeru0x1103 18:5aa48aec9cae 18 //-------------------------------------------------------------
takeru0x1103 18:5aa48aec9cae 19 static void xBufClear(){
takeru0x1103 18:5aa48aec9cae 20 memset(&cmdBuf[0] , 0x00 , sizeof(cmdBuf) ); //コマンドバッファクリア
takeru0x1103 18:5aa48aec9cae 21 bp=0; //バッファポインタを先頭に戻す
takeru0x1103 18:5aa48aec9cae 22 }
takeru0x1103 18:5aa48aec9cae 23
takeru0x1103 18:5aa48aec9cae 24 //-------------------------------------------------------------
takeru0x1103 18:5aa48aec9cae 25 //バックスペースが来た時の処理
takeru0x1103 18:5aa48aec9cae 26 //-------------------------------------------------------------
takeru0x1103 18:5aa48aec9cae 27 static void xBs(){
takeru0x1103 18:5aa48aec9cae 28 cmdBuf[bp] = 0x0; //現在位置をNULLにする
takeru0x1103 18:5aa48aec9cae 29 if(bp>0) bp--; //インデックスを戻す(先頭でなければ)
takeru0x1103 18:5aa48aec9cae 30 }
takeru0x1103 18:5aa48aec9cae 31
takeru0x1103 18:5aa48aec9cae 32 //-------------------------------------------------------------
takeru0x1103 18:5aa48aec9cae 33 //コマンドバッファへ格納
takeru0x1103 18:5aa48aec9cae 34 //-------------------------------------------------------------
takeru0x1103 18:5aa48aec9cae 35 static void xPush(char cBuf){
takeru0x1103 18:5aa48aec9cae 36 //バッファ終端だったらエラー
takeru0x1103 18:5aa48aec9cae 37 if(bp >= BUFFER_INDEX_END){
takeru0x1103 18:5aa48aec9cae 38 return;
takeru0x1103 18:5aa48aec9cae 39 }
takeru0x1103 18:5aa48aec9cae 40
takeru0x1103 18:5aa48aec9cae 41 cmdBuf[bp] = cBuf;//受信文字をバッファへ格納
takeru0x1103 18:5aa48aec9cae 42 bp++;//次へ進める
takeru0x1103 18:5aa48aec9cae 43 }
takeru0x1103 18:5aa48aec9cae 44
takeru0x1103 18:5aa48aec9cae 45 //=============================================================
takeru0x1103 18:5aa48aec9cae 46 // 初期化
takeru0x1103 18:5aa48aec9cae 47 //=============================================================
takeru0x1103 18:5aa48aec9cae 48 void commandInit(){
takeru0x1103 18:5aa48aec9cae 49 //バッファクリア
takeru0x1103 18:5aa48aec9cae 50 xBufClear();
takeru0x1103 18:5aa48aec9cae 51 }
takeru0x1103 18:5aa48aec9cae 52
takeru0x1103 18:5aa48aec9cae 53 //-------------------------------------------------------------
takeru0x1103 18:5aa48aec9cae 54 //
takeru0x1103 18:5aa48aec9cae 55 //-------------------------------------------------------------
takeru0x1103 18:5aa48aec9cae 56 void commandParse(){
takeru0x1103 18:5aa48aec9cae 57 //フラグが立ってなければ何もしないで終わる
takeru0x1103 18:5aa48aec9cae 58 if(gf_CmdPrs==false){
takeru0x1103 18:5aa48aec9cae 59 return;
takeru0x1103 18:5aa48aec9cae 60 }
takeru0x1103 18:5aa48aec9cae 61 //フラグを落として以降の処理をする
takeru0x1103 18:5aa48aec9cae 62 gf_CmdPrs=false;
takeru0x1103 18:5aa48aec9cae 63
takeru0x1103 18:5aa48aec9cae 64 // string str((char*)cmdBuf);
takeru0x1103 18:5aa48aec9cae 65 // std::vector<string> list = f_Split(str,SPC);//スペースで切る
takeru0x1103 18:5aa48aec9cae 66 // string arg2;
takeru0x1103 18:5aa48aec9cae 67 // if(list.size() > 0){
takeru0x1103 18:5aa48aec9cae 68 // arg2 = list.at(1).c_str();//
takeru0x1103 18:5aa48aec9cae 69 // }
takeru0x1103 18:5aa48aec9cae 70
takeru0x1103 18:5aa48aec9cae 71 //第二引数の開始位置
takeru0x1103 18:5aa48aec9cae 72 UCHAR arg2pos=0;
takeru0x1103 18:5aa48aec9cae 73 while (1) {
takeru0x1103 18:5aa48aec9cae 74 if(cmdBuf[arg2pos]==NULL){
takeru0x1103 18:5aa48aec9cae 75 arg2pos++;
takeru0x1103 18:5aa48aec9cae 76 break;
takeru0x1103 18:5aa48aec9cae 77 }else{
takeru0x1103 18:5aa48aec9cae 78 arg2pos++;
takeru0x1103 18:5aa48aec9cae 79 }
takeru0x1103 18:5aa48aec9cae 80 }
takeru0x1103 18:5aa48aec9cae 81
takeru0x1103 18:5aa48aec9cae 82 //リード系
takeru0x1103 18:5aa48aec9cae 83 if (strcmp(cmdBuf , "re" )==0 ){gf_Print.bf.e1=true; gf_Print.bf.e2=true;}
takeru0x1103 18:5aa48aec9cae 84 else if (strcmp(cmdBuf , "re1" )==0 ){gf_Print.bf.e1=true; }
takeru0x1103 18:5aa48aec9cae 85 else if (strcmp(cmdBuf , "re2" )==0 ){ gf_Print.bf.e2=true;}
takeru0x1103 18:5aa48aec9cae 86 else if (strcmp(cmdBuf , "rm" )==0 ){ }//リードモーター
takeru0x1103 18:5aa48aec9cae 87 else if (strcmp(cmdBuf , "rm1" )==0 ){ }//リードモーター1
takeru0x1103 18:5aa48aec9cae 88 else if (strcmp(cmdBuf , "rm2" )==0 ){ }//リードモーター2
takeru0x1103 18:5aa48aec9cae 89 else if (strcmp(cmdBuf , "rm3" )==0 ){ }//リードモーター3
takeru0x1103 18:5aa48aec9cae 90 else if (strcmp(cmdBuf , "rm4" )==0 ){ }//リードモーター4
takeru0x1103 18:5aa48aec9cae 91 else if (strcmp(cmdBuf , "rg" )==0 ){gf_Print.bf.gy=true;}//リードジャイロ
takeru0x1103 18:5aa48aec9cae 92 else if (strcmp(cmdBuf , "ry" )==0 ){gf_Print.bf.yaw=true;}//リード姿勢角ヨー
takeru0x1103 18:5aa48aec9cae 93 else if (strcmp(cmdBuf , "rimu")==0 ){gf_Print.bf.yaw=true;gf_Print.bf.gy=true;}//リードIMU
takeru0x1103 18:5aa48aec9cae 94 else if (strcmp(cmdBuf , "rs" )==0 ){ }//リードスイッチ
takeru0x1103 18:5aa48aec9cae 95
takeru0x1103 18:5aa48aec9cae 96 else if (strcmp(cmdBuf , "rfb" )==0 ){gf_Print.bf.fb=true;}//PID制御フィードバック結果
takeru0x1103 18:5aa48aec9cae 97 else if (strcmp(cmdBuf , "rp" )==0 ){gf_Print.bf.pp=true;gf_Print.bf.p=true;gf_Print.bf.i=true;gf_Print.bf.d=true;}//パラメータ全表示
takeru0x1103 18:5aa48aec9cae 98 else if (strcmp(cmdBuf , "rpP" )==0 ){gf_Print.bf.pp=true;}//パラメータアウターPゲイン表示
takeru0x1103 18:5aa48aec9cae 99 else if (strcmp(cmdBuf , "rpp" )==0 ){gf_Print.bf.p=true;}//パラメータPゲイン表示
takeru0x1103 18:5aa48aec9cae 100 else if (strcmp(cmdBuf , "rpi" )==0 ){gf_Print.bf.i=true;}//パラメータIゲイン表示
takeru0x1103 18:5aa48aec9cae 101 else if (strcmp(cmdBuf , "rpd" )==0 ){gf_Print.bf.d=true;}//パラメータDゲイン表示
takeru0x1103 18:5aa48aec9cae 102
takeru0x1103 18:5aa48aec9cae 103 //設定系
takeru0x1103 18:5aa48aec9cae 104 else if (strcmp(cmdBuf , "se1" )==0 ){gf_AxReq[0].bf.req=true; gf_AxReq[0].bf.val=(UINT16)atoi(&cmdBuf[arg2pos]);}
takeru0x1103 18:5aa48aec9cae 105 else if (strcmp(cmdBuf , "se2" )==0 ){gf_AxReq[1].bf.req=true; gf_AxReq[1].bf.val=(UINT16)atoi(&cmdBuf[arg2pos]);}
takeru0x1103 18:5aa48aec9cae 106 else if (strcmp(cmdBuf , "sm1" )==0 ){gf_MtReq[0].bf.req=true; gf_MtReq[0].bf.val=(UINT16)atoi(&cmdBuf[arg2pos]);}
takeru0x1103 18:5aa48aec9cae 107 else if (strcmp(cmdBuf , "sm2" )==0 ){gf_MtReq[1].bf.req=true; gf_MtReq[1].bf.val=(UINT16)atoi(&cmdBuf[arg2pos]);}
takeru0x1103 18:5aa48aec9cae 108 else if (strcmp(cmdBuf , "sm3" )==0 ){gf_MtReq[2].bf.req=true; gf_MtReq[2].bf.val=(UINT16)atoi(&cmdBuf[arg2pos]);}
takeru0x1103 18:5aa48aec9cae 109 else if (strcmp(cmdBuf , "sm4" )==0 ){gf_MtReq[3].bf.req=true; gf_MtReq[3].bf.val=(UINT16)atoi(&cmdBuf[arg2pos]);}
takeru0x1103 18:5aa48aec9cae 110
takeru0x1103 18:5aa48aec9cae 111 //キャリブレーション
takeru0x1103 18:5aa48aec9cae 112 else if (strcmp(cmdBuf , "calyaw")==0 ){gf_Cal.bf.yaw=true;}//
takeru0x1103 18:5aa48aec9cae 113 else if (strcmp(cmdBuf , "calgyro")==0 ){gf_Cal.bf.gy=true;}//
takeru0x1103 18:5aa48aec9cae 114 else if (strcmp(cmdBuf , "calimu")==0 ){gf_Cal.bf.gy=true;gf_Cal.bf.yaw=true;}//
takeru0x1103 18:5aa48aec9cae 115
takeru0x1103 18:5aa48aec9cae 116 //チェック系
takeru0x1103 18:5aa48aec9cae 117 else if (strcmp(cmdBuf , "chkmot")==0 ){gf_Chk.bf.mot=true;}//モーターチェック
takeru0x1103 18:5aa48aec9cae 118
takeru0x1103 18:5aa48aec9cae 119 //連続モニタ
takeru0x1103 18:5aa48aec9cae 120 else if (strcmp(cmdBuf , "mg" )==0 ){gf_Print.bf.gy=true;gf_Mon.bf.gy=true;}//リードジャイロ
takeru0x1103 18:5aa48aec9cae 121 else if (strcmp(cmdBuf , "my" )==0 ){gf_Print.bf.yaw=true;gf_Mon.bf.yaw=true;}//リード姿勢角ヨー
takeru0x1103 18:5aa48aec9cae 122 else if (strcmp(cmdBuf , "mimu")==0 ){gf_Print.bf.yaw=true;gf_Print.bf.gy=true;gf_Mon.bf.yaw=true;gf_Mon.bf.gy=true;}//リードIMU
takeru0x1103 18:5aa48aec9cae 123 else if (strcmp(cmdBuf , "mfb" )==0 ){gf_Print.bf.fb=true;gf_Mon.bf.fb=true;}//PID制御フィードバック結果
takeru0x1103 18:5aa48aec9cae 124 else if (strcmp(cmdBuf , "kill")==0 ){gf_Mon.flg=0;}//モニタ停止
takeru0x1103 18:5aa48aec9cae 125
takeru0x1103 18:5aa48aec9cae 126 //バッファクリア
takeru0x1103 18:5aa48aec9cae 127 xBufClear();
takeru0x1103 18:5aa48aec9cae 128 //フラグ下す
takeru0x1103 18:5aa48aec9cae 129 gf_CmdPrs = false;
takeru0x1103 18:5aa48aec9cae 130 }
takeru0x1103 18:5aa48aec9cae 131
takeru0x1103 18:5aa48aec9cae 132 //=============================================================
takeru0x1103 18:5aa48aec9cae 133 // コマンドバッファへ受信キャラを格納
takeru0x1103 18:5aa48aec9cae 134 //=============================================================
takeru0x1103 18:5aa48aec9cae 135 void commandPush(UCHAR iRxChar){
takeru0x1103 18:5aa48aec9cae 136 switch(iRxChar){
takeru0x1103 18:5aa48aec9cae 137 case ESC:
takeru0x1103 18:5aa48aec9cae 138 /*何もしない*/
takeru0x1103 18:5aa48aec9cae 139 break;
takeru0x1103 18:5aa48aec9cae 140 case TAB: //タブで姿勢制御ON/OFF
takeru0x1103 18:5aa48aec9cae 141 gf_AttCntEna = !gf_AttCntEna;
takeru0x1103 18:5aa48aec9cae 142 sp.printf("Attitude control %d\r\n",gf_AttCntEna);
takeru0x1103 18:5aa48aec9cae 143 break;
takeru0x1103 18:5aa48aec9cae 144 case BS :
takeru0x1103 18:5aa48aec9cae 145 xBs();//バックスペース
takeru0x1103 18:5aa48aec9cae 146 break;
takeru0x1103 18:5aa48aec9cae 147 case CR :
takeru0x1103 18:5aa48aec9cae 148 /*何もしない*/
takeru0x1103 18:5aa48aec9cae 149 break;
takeru0x1103 18:5aa48aec9cae 150 case LF :
takeru0x1103 18:5aa48aec9cae 151 gf_CmdPrs = true;
takeru0x1103 18:5aa48aec9cae 152 break;
takeru0x1103 18:5aa48aec9cae 153 case SPC:
takeru0x1103 18:5aa48aec9cae 154 xPush(0x0);//Wite space はNullにする
takeru0x1103 18:5aa48aec9cae 155 break;
takeru0x1103 18:5aa48aec9cae 156 default :
takeru0x1103 18:5aa48aec9cae 157 xPush(iRxChar);//その他はコマンドバッファへプッシュ
takeru0x1103 18:5aa48aec9cae 158 break;
takeru0x1103 18:5aa48aec9cae 159 }//switch
takeru0x1103 18:5aa48aec9cae 160 }
takeru0x1103 18:5aa48aec9cae 161