File content as of revision 63:aee44afe6363:

/**
* @file globalFlags.cpp
* @brief コマンド解析<-->ホバーバイク制御タスク間及びホバーバイク制御タスク内で使用する共用フラグを使用する変数宣言と関数定義
* @author Shirota, Nomura
* @date 2018/10/01 ?
*/
#include "typedef.h"
#include "globalFlags.h"

DigitalOut led1(LED1);     //!< モニタ用LED1用変数
DigitalOut led2(LED2);     //!< モニタ用LED2用変数
DigitalOut led3(LED3);     //!< モニタ用LED3用変数
DigitalOut led4(LED4);     //!< モニタ用LED4用変数

DigitalOut DO_01(p21);    //!< 状態表示用LED1(R)用変数
DigitalOut DO_02(p22);    //!< 状態表示用LED2(G)用変数
DigitalOut DO_03(p23);    //!< 状態表示用LED3(B)用変数

AnalogIn AinAxl(p20);     //!< モーターアクセル用アナログ入力


char            g_CmdBuf[G_CMD_BUF_SIZ];    //!< コマンド受け渡しバッファ
bool            gf_CmdPrs;                  //!< コマンドパーサー実行要求フラグ

bool            gf_Armed;                   //!< アーミングフラグ
bool            gf_Dbg;                     //!< デバッグタスク起動フラグ

bool            gf_StopMot;                 //!< モーターの強制停止 サーボ全閉
bool            gf_FromActiveStat;          //!< モーターの強制停止 サーボ全閉が動作時に発生したかどうか

bool            gf_BlinkLED;

typPrintFlg    gf_Print;                    //!< デバッグプリントフラグ（１回表示）
typPrintFlg    gf_Mon;                      //!< デバッグモニタフラグ（繰り返し表示）
typCalFlag      gf_Cal;                     //!< キャリブレーションフラグ

typAccel        gf_AxReq[2];                //!< エンジンアクセル値更新フラグ
typAccel        gf_AxReqH[2];               //!< 浮上時エンジンアクセル値更新フラグ
typAccel        gf_AxStepReq[2];            //!< エンジンアクセル用サーボ動作ステップ値更新フラグ
typAccel        gf_AxAdjStepReq[2];         //!< エンジンアクセル調整用サーボ動作ステップ値更新フラグ

typAxlRpm        gf_MtReq[4];               //!< モーター姿勢制御値更新
typAxlRpm        gf_MtAttOfs[4];            //!< モーター姿勢制御オフセット値更新
typAxlRpm        gf_MtReqOfs[4];            //!< モーターオフセット値更新
typAxlRpm        gf_MtReqU[4];              //!< モーターユーザー値更新
typAxlRpm        gf_MtBrk;                  //!< モーターブレーキ時のRPM値更新
typAxlRpm        gf_MtDefaultOffset;        //!< モーターのTAKE_OFFステート時に使用するデフォルトオフセット値更新
typAxlRpm        gf_MtReqDct[8];            //!< ダイレクト(FPGA関数直接呼び出し)モーター値更新フラグ

typAxlRpm        gf_AngBrk;                 //!< ブレーキ時の1秒間の目標角度変化値[deg]更新

typSWCmd     gf_SwCmd;                      //!< スイッチが押されたことを模擬するフラグ

enmHbState      gf_State = SLEEP;           //!< 現在のステートを格納する変数
bool            gf_StateEnt;                //!< 状態遷移後、最初であることを示す

bool            gf_PidParaUpdate;           //!< PID Pp,P,I,Dの係数アップデートフラグ
typPidPara      g_PidPara;                  //!< PID Pp,P,I,Dの係数の外部設定用


void initFlags(){
    memset(g_CmdBuf,0x0,sizeof(g_CmdBuf));
    gf_CmdPrs = false;
    gf_Armed = false;
    gf_Dbg = false;
    gf_StopMot = false;
    
    gf_BlinkLED = false;

//    gf_Print.flg = 0;
//    gf_Mon.flg = 0;
    gf_Print.d1.flg = 0;
    gf_Print.d2.flg = 0;

    gf_Cal.flg = 0;

    //gf_DbgPrint.flg = 0;

    // engine
    for(int i = 0; i < 2; ++i){
        gf_AxReq[i].dt = 0;
        gf_AxReqH[i].dt = 0;
        gf_AxStepReq[i].bf.req = false;
        gf_AxStepReq[i].bf.val = 30;
        gf_AxAdjStepReq[i].bf.req = false;
        gf_AxAdjStepReq[i].bf.val = 50;
    }

    for(int i=0; i<4;++i){
        gf_MtReq[i].req = false;
        gf_MtReq[i].val = 0;
        gf_MtReqOfs[i].req = false;
        gf_MtReqOfs[i].val = 0;
        gf_MtReqU[i].req = false;
        gf_MtReqU[i].val = 0;
        gf_MtAttOfs[i].req = false;
        gf_MtAttOfs[i].val = 0;
    }
    gf_MtBrk.req = false;
    gf_MtBrk.val = DEF_BRK_RPM;
    gf_AngBrk.req = false;
    gf_AngBrk.val = DEF_BRK_ANG;
    
    gf_MtDefaultOffset.req = false;
    gf_MtDefaultOffset.val = DEF_MOT_OFFSET;

    for(int i = 0; i < 8; ++i){
        gf_MtReqDct[i].req = false;
        gf_MtReqDct[i].val = 0;
    }

    gf_SwCmd.dt = 0;

    gf_State = SLEEP;
    gf_StateEnt = false;
    g_PidPara.PP = 2;
    g_PidPara.P = 700000;
    g_PidPara.I = 200000;
    g_PidPara.D = 700000;
    g_PidPara.IMax = 1000000;
    g_PidPara.IMin = -1000000;
    g_PidPara.mode = PID_0_OFF;

    // for(int i = 0; i < 4; ++i){
    //     gf_MotParaUpdate[i] = false;
    //     g_MotPara[i].limit_hi = LIM_MOT_INP_MAX;
    //     g_MotPara[i].limit_low = LIM_MOT_INP_MIN;
    // }
}

void setDOCol(eLedCol col)
{
    switch(col){
        case WHITE:
            DO_01 = 1;
            DO_02 = 1;
            DO_03 = 1;
        break;
        case RED:
            DO_01 = 1;
            DO_02 = 0;
            DO_03 = 0;
        break;
        case GREEN:
            DO_01 = 0;
            DO_02 = 1;
            DO_03 = 0;
        break;
        case BLUE:
            DO_01 = 0;
            DO_02 = 0;
            DO_03 = 1;
        break;
        case YELLOW:
            DO_01 = 1;
            DO_02 = 1;
            DO_03 = 0;
        break;
        case PURPLE:
            DO_01 = 1;
            DO_02 = 0;
            DO_03 = 1;
        break;
        case LIGHT_BLUE:
            DO_01 = 0;
            DO_02 = 1;
            DO_03 = 1;
        break;
        case BLK_WHITE:
            DO_01 = !DO_01;
            DO_02 = !DO_02;
            DO_03 = !DO_03;
        break;
        case BLK_RED:
            DO_01 = !DO_01;
            DO_02 = 0;
            DO_03 = 0;
        break;
        case BLK_GREEN:
            DO_01 = 0;
            DO_02 = !DO_02;
            DO_03 = 0;
        break;
        case BLK_BLUE:
            DO_01 = 0;
            DO_02 = 0;
            DO_03 = !DO_03;
        break;
        case BLK_YELLOW:
            DO_01 = !DO_01;
            DO_02 = !DO_02;
            DO_03 = 0;
        break;
        case BLK_PURPLE:
            DO_01 = !DO_01;
            DO_02 = 0;
            DO_03 = !DO_03;
        break;
        case BLK_LIGHT_BLUE:
            DO_01 = 0;
            DO_02 = !DO_02;
            DO_03 = !DO_03;
        break;
        case OFF:
        default:
            DO_01 = 0;//R
            DO_02 = 0;//G
            DO_03 = 0;//B
        break;
    }
}

void setDO4LED(enmHbState stat)
{
    if(stat == NONE)
    {// 消灯
        setDOCol(OFF);
    }
    else if(stat == SLEEP)
    {//YELLOW
        setDOCol(YELLOW);
    }
    else if(stat == WAKEUP || stat == STANDBY)
    {//BLUE
        setDOCol(BLUE);
    }
    else if(stat == IDLE)
    {//GREEN
        setDOCol(GREEN);
    }
    else if(stat == UPPER_IDLE)
    {//WHITE
        setDOCol(WHITE);
    }
    else if(stat == TAKE_OFF || stat == HOVER || stat == DRIVE || stat == GROUND || stat == EMGGND)
    {//PURPLE
        if(gf_BlinkLED){
            setDOCol(BLK_PURPLE);
            gf_BlinkLED = false;
        }
        else{
            setDOCol(PURPLE);
        }
    }
    else if(stat == CHK_EG_ENT || stat == CHK_EG_MID || stat == CHK_EG_EXIT)
    {//LIGHT BLUE
        setDOCol(LIGHT_BLUE);
    }
    else if(stat == CHK_EG_F || stat == CHK_EG_R )
    {//BLK_LIGHT BLUE
        setDOCol(BLK_LIGHT_BLUE);
    }
    else if(stat == CHK_ENT || stat == CHK_MOT || stat == CHK_AXL || stat == CHK_ATT || stat == CHK_EXIT)
    {//WHITE
        setDOCol(WHITE);
    }
    else/* if(stat == MOT_STOP)*/
    {
        setDOCol(RED);
    }
}

void setState(enmHbState stat){
    // ありえない遷移を排除
    if(gf_State == SLEEP){if(stat != WAKEUP) return;}
    if(gf_State == WAKEUP){if(stat != STANDBY) return;}
    if(gf_State == STANDBY){if(stat !=IDLE) return;}
    if(gf_State == IDLE){if(stat != TAKE_OFF && stat != SLEEP) return;}
    if(gf_State == TAKE_OFF){if(stat != HOVER && stat != GROUND) return;}
    if(gf_State == GROUND){if(stat != IDLE) return;}
    if(gf_State == HOVER){if(stat != DRIVE && stat != GROUND) return;}
    if(gf_State == DRIVE){if(stat != HOVER && stat != EMGGND) return;}
    if(gf_State == EMGGND){if(stat != IDLE) return;}

    sp.printf("state: %d\r\n",stat);
    gf_State = stat;
    gf_StateEnt = true;
}
void setStateF(enmHbState stat){
    sp.printf("state: %d\r\n",stat);
    enmHbState tmp;
    if((UINT16)stat > MOT_STOP){
        tmp = SLEEP;
    }
    else
    {
        tmp = stat;
    }
    gf_State = tmp;
    gf_StateEnt = true;
}

bool isActiveState(){
    if(gf_State  >= IDLE && gf_State <= EMGGND){
        return true;
    }
    else {
        return false;
    }
}