teamALI / Mbed 2 deprecated HB2018

Dependencies:   mbed FreeRTOS

hbCommand.cpp

Committer:
takeru0x1103
Date:
2018-12-01
Revision:
18:5aa48aec9cae
Child:
19:4b0fe9a5ec38

File content as of revision 18:5aa48aec9cae:

#include <string.h>
#include <stdio.h>

#include "globalFlags.h"
#include "hbCommand.h"

//受信文字を貯めるコマンドバッファのサイズ
#define BUFFER_INDEX_END    31

//-------------------------------------------------------------
//グローバル変数
//-------------------------------------------------------------
static      char    cmdBuf[BUFFER_INDEX_END]    ={0,}   ;// コマンドバッファ
static      UCHAR   bp                          =0      ;// バッファポインタ

//-------------------------------------------------------------
//受信コマンドバッファクリア
//-------------------------------------------------------------
static void xBufClear(){
    memset(&cmdBuf[0] , 0x00 , sizeof(cmdBuf) );    //コマンドバッファクリア
    bp=0;                                           //バッファポインタを先頭に戻す
}

//-------------------------------------------------------------
//バックスペースが来た時の処理
//-------------------------------------------------------------
static void xBs(){
    cmdBuf[bp] = 0x0;   //現在位置をNULLにする
    if(bp>0)    bp--;   //インデックスを戻す(先頭でなければ)
}

//-------------------------------------------------------------
//コマンドバッファへ格納
//-------------------------------------------------------------
static void xPush(char cBuf){
    //バッファ終端だったらエラー
    if(bp >= BUFFER_INDEX_END){
        return;
    }

    cmdBuf[bp] = cBuf;//受信文字をバッファへ格納
    bp++;//次へ進める
}

//=============================================================
// 初期化
//=============================================================
void commandInit(){
    //バッファクリア
    xBufClear();    
}

//-------------------------------------------------------------
//
//-------------------------------------------------------------
void commandParse(){
    //フラグが立ってなければ何もしないで終わる
    if(gf_CmdPrs==false){
        return;
    }
    //フラグを落として以降の処理をする
    gf_CmdPrs=false;
    
//    string                  str((char*)cmdBuf);
//    std::vector<string>     list = f_Split(str,SPC);//スペースで切る
//    string                  arg2;
//    if(list.size() > 0){
//        arg2  = list.at(1).c_str();//
//    }
    
    //第二引数の開始位置
    UCHAR arg2pos=0;
    while (1) {
        if(cmdBuf[arg2pos]==NULL){
            arg2pos++;
            break;
        }else{
            arg2pos++;
        }
    }
    
    //リード系
         if (strcmp(cmdBuf , "re"  )==0 ){gf_Print.bf.e1=true; gf_Print.bf.e2=true;}
    else if (strcmp(cmdBuf , "re1" )==0 ){gf_Print.bf.e1=true;                     }
    else if (strcmp(cmdBuf , "re2" )==0 ){                     gf_Print.bf.e2=true;}
    else if (strcmp(cmdBuf , "rm"  )==0 ){ }//リードモーター  
    else if (strcmp(cmdBuf , "rm1" )==0 ){ }//リードモーター1 
    else if (strcmp(cmdBuf , "rm2" )==0 ){ }//リードモーター2 
    else if (strcmp(cmdBuf , "rm3" )==0 ){ }//リードモーター3 
    else if (strcmp(cmdBuf , "rm4" )==0 ){ }//リードモーター4 
    else if (strcmp(cmdBuf , "rg"  )==0 ){gf_Print.bf.gy=true;}//リードジャイロ  
    else if (strcmp(cmdBuf , "ry"  )==0 ){gf_Print.bf.yaw=true;}//リード姿勢角ヨー
    else if (strcmp(cmdBuf , "rimu")==0 ){gf_Print.bf.yaw=true;gf_Print.bf.gy=true;}//リードIMU
    else if (strcmp(cmdBuf , "rs"  )==0 ){ }//リードスイッチ
    
    else if (strcmp(cmdBuf , "rfb" )==0 ){gf_Print.bf.fb=true;}//PID制御フィードバック結果
    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;}//パラメータ全表示
    else if (strcmp(cmdBuf , "rpP" )==0 ){gf_Print.bf.pp=true;}//パラメータアウターPゲイン表示    
    else if (strcmp(cmdBuf , "rpp" )==0 ){gf_Print.bf.p=true;}//パラメータPゲイン表示 
    else if (strcmp(cmdBuf , "rpi" )==0 ){gf_Print.bf.i=true;}//パラメータIゲイン表示 
    else if (strcmp(cmdBuf , "rpd" )==0 ){gf_Print.bf.d=true;}//パラメータDゲイン表示
    
    //設定系
    else if (strcmp(cmdBuf , "se1"   )==0 ){gf_AxReq[0].bf.req=true; gf_AxReq[0].bf.val=(UINT16)atoi(&cmdBuf[arg2pos]);}
    else if (strcmp(cmdBuf , "se2"   )==0 ){gf_AxReq[1].bf.req=true; gf_AxReq[1].bf.val=(UINT16)atoi(&cmdBuf[arg2pos]);}
    else if (strcmp(cmdBuf , "sm1"   )==0 ){gf_MtReq[0].bf.req=true; gf_MtReq[0].bf.val=(UINT16)atoi(&cmdBuf[arg2pos]);}
    else if (strcmp(cmdBuf , "sm2"   )==0 ){gf_MtReq[1].bf.req=true; gf_MtReq[1].bf.val=(UINT16)atoi(&cmdBuf[arg2pos]);}
    else if (strcmp(cmdBuf , "sm3"   )==0 ){gf_MtReq[2].bf.req=true; gf_MtReq[2].bf.val=(UINT16)atoi(&cmdBuf[arg2pos]);}
    else if (strcmp(cmdBuf , "sm4"   )==0 ){gf_MtReq[3].bf.req=true; gf_MtReq[3].bf.val=(UINT16)atoi(&cmdBuf[arg2pos]);}
    
    //キャリブレーション
    else if (strcmp(cmdBuf , "calyaw")==0 ){gf_Cal.bf.yaw=true;}//
    else if (strcmp(cmdBuf , "calgyro")==0 ){gf_Cal.bf.gy=true;}//
    else if (strcmp(cmdBuf , "calimu")==0 ){gf_Cal.bf.gy=true;gf_Cal.bf.yaw=true;}//
    
    //チェック系
    else if (strcmp(cmdBuf , "chkmot")==0 ){gf_Chk.bf.mot=true;}//モーターチェック

    //連続モニタ
    else if (strcmp(cmdBuf , "mg"  )==0 ){gf_Print.bf.gy=true;gf_Mon.bf.gy=true;}//リードジャイロ  
    else if (strcmp(cmdBuf , "my"  )==0 ){gf_Print.bf.yaw=true;gf_Mon.bf.yaw=true;}//リード姿勢角ヨー
    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
    else if (strcmp(cmdBuf , "mfb" )==0 ){gf_Print.bf.fb=true;gf_Mon.bf.fb=true;}//PID制御フィードバック結果
    else if (strcmp(cmdBuf , "kill")==0 ){gf_Mon.flg=0;}//モニタ停止  
    
    //バッファクリア
    xBufClear();
    //フラグ下す
    gf_CmdPrs = false;
}

//=============================================================
// コマンドバッファへ受信キャラを格納
//=============================================================
void commandPush(UCHAR iRxChar){
    switch(iRxChar){
        case ESC: 
            /*何もしない*/
            break;
        case TAB: //タブで姿勢制御ON/OFF
            gf_AttCntEna = !gf_AttCntEna;
            sp.printf("Attitude control %d\r\n",gf_AttCntEna);
            break;
        case BS :
            xBs();//バックスペース
            break;
        case CR :
            /*何もしない*/
            break; 
        case LF :
            gf_CmdPrs = true;
            break;
      case SPC:
            xPush(0x0);//Wite space はNullにする
            break;
        default :
            xPush(iRxChar);//その他はコマンドバッファへプッシュ
            break; 
    }//switch
}