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.
globalFlags.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?
User | Revision | Line number | New contents of line |
---|---|---|---|
takeru0x1103 | 19:4b0fe9a5ec38 | 1 | |
takeru0x1103 | 16:05b9e44889f1 | 2 | #include "globalFlags.h" |
MasashiNomura | 22:24c9c2dedca9 | 3 | //#define STATE_BUF_SIZ 8 |
takeru0x1103 | 16:05b9e44889f1 | 4 | |
MasashiNomura | 22:24c9c2dedca9 | 5 | //モニタ用LED |
takeru0x1103 | 16:05b9e44889f1 | 6 | DigitalOut led1(LED1); |
takeru0x1103 | 16:05b9e44889f1 | 7 | DigitalOut led2(LED2); |
takeru0x1103 | 16:05b9e44889f1 | 8 | DigitalOut led3(LED3); |
takeru0x1103 | 16:05b9e44889f1 | 9 | DigitalOut led4(LED4); |
takeru0x1103 | 8:1ca49cb18290 | 10 | |
takeru0x1103 | 21:78302ecdb661 | 11 | //GlobalFlags gf; |
takeru0x1103 | 21:78302ecdb661 | 12 | |
takeru0x1103 | 19:4b0fe9a5ec38 | 13 | char g_CmdBuf[G_CMD_BUF_SIZ] ;// コマンド受け渡しバッファ |
MasashiNomura | 22:24c9c2dedca9 | 14 | bool gf_CmdPrs; |
takeru0x1103 | 19:4b0fe9a5ec38 | 15 | |
MasashiNomura | 22:24c9c2dedca9 | 16 | bool gf_Armed ;//アーミングフラグ |
MasashiNomura | 23:79e20be4bc5b | 17 | bool gf_Dbg ;//デバッグタスク起動フラグ |
MasashiNomura | 23:79e20be4bc5b | 18 | bool gf_fs_stat ;//強制遷移フラグ |
MasashiNomura | 25:f3a6e7eec9c3 | 19 | bool gf_StopMot ;//モーターの強制停止 |
MasashiNomura | 25:f3a6e7eec9c3 | 20 | |
takeru0x1103 | 19:4b0fe9a5ec38 | 21 | typPrintFlag gf_Print ;// |
takeru0x1103 | 19:4b0fe9a5ec38 | 22 | typPrintFlag gf_Mon ;// |
takeru0x1103 | 19:4b0fe9a5ec38 | 23 | typCalFlag gf_Cal ;// |
MasashiNomura | 26:732bc37fbefd | 24 | typDbgPrintFlg gf_DbgPrint ;//デバッグ用 |
MasashiNomura | 22:24c9c2dedca9 | 25 | //typAccel gf_AxReq[2] ={0,} ;//アクセル更新 |
MasashiNomura | 22:24c9c2dedca9 | 26 | //typAccel gf_MtReq[4] ={0,} ;//モーター更新 |
MasashiNomura | 22:24c9c2dedca9 | 27 | typAccel gf_AxReq[2] ;//アクセル更新フラグ |
MasashiNomura | 29:eb3d72dd94aa | 28 | //typAccel gf_MtReq[4] ;//モーター更新フラグ |
MasashiNomura | 29:eb3d72dd94aa | 29 | //typAccel gf_MtReqOfs[4] ;//モーターオフセット更新フラグ |
MasashiNomura | 29:eb3d72dd94aa | 30 | //typAccel gf_MtReqDct[8] ;//ダイレクト(FPGA関数直接呼び出し)モーター更新フラグ |
MasashiNomura | 29:eb3d72dd94aa | 31 | typAxlRpm gf_MtReq[4] ;//モーター更新フラグ |
MasashiNomura | 29:eb3d72dd94aa | 32 | typAxlRpm gf_MtReqOfs[4] ;//モーターオフセット更新フラグ |
MasashiNomura | 29:eb3d72dd94aa | 33 | typAxlRpm gf_MtReqDct[8] ;//ダイレクト(FPGA関数直接呼び出し)モーター更新フラグ |
MasashiNomura | 29:eb3d72dd94aa | 34 | //typOnePushFlg gf_MtReqOP[4] ;//モーターの回転制御動作用 一時的出力アップダウン |
MasashiNomura | 29:eb3d72dd94aa | 35 | typ1Push gf_MtReqOP[4] ;//モーターの回転制御動作用 一時的出力アップダウン |
MasashiNomura | 22:24c9c2dedca9 | 36 | enmHbState gf_State = SLEEP; |
takeru0x1103 | 19:4b0fe9a5ec38 | 37 | |
MasashiNomura | 24:c5945aaae777 | 38 | bool gf_PidParaUpdate ;//PID Pp,P,I,Dの係数アップデートフラグ |
MasashiNomura | 24:c5945aaae777 | 39 | typPidPara g_PidPara ;//PID Pp,P,I,Dの係数の外部設定用 |
takeru0x1103 | 20:0394e15412c3 | 40 | |
MasashiNomura | 25:f3a6e7eec9c3 | 41 | bool gf_MotParaUpdate[4] ;//モーターのパラメータ値更新フラグ |
MasashiNomura | 25:f3a6e7eec9c3 | 42 | typMotPara g_MotPara[4]; |
MasashiNomura | 25:f3a6e7eec9c3 | 43 | |
MasashiNomura | 22:24c9c2dedca9 | 44 | void initFlags(){ |
MasashiNomura | 22:24c9c2dedca9 | 45 | memset(g_CmdBuf,0x0,sizeof(g_CmdBuf)); |
MasashiNomura | 22:24c9c2dedca9 | 46 | gf_CmdPrs = false; |
MasashiNomura | 22:24c9c2dedca9 | 47 | gf_Armed = false; |
MasashiNomura | 22:24c9c2dedca9 | 48 | gf_Dbg = false; |
MasashiNomura | 25:f3a6e7eec9c3 | 49 | gf_StopMot = false; |
MasashiNomura | 25:f3a6e7eec9c3 | 50 | |
MasashiNomura | 22:24c9c2dedca9 | 51 | gf_Print.flg = 0; |
MasashiNomura | 22:24c9c2dedca9 | 52 | gf_Mon.flg = 0; |
MasashiNomura | 22:24c9c2dedca9 | 53 | gf_Cal.flg = 0; |
MasashiNomura | 26:732bc37fbefd | 54 | |
MasashiNomura | 26:732bc37fbefd | 55 | gf_DbgPrint.flg = 0; |
MasashiNomura | 26:732bc37fbefd | 56 | |
MasashiNomura | 22:24c9c2dedca9 | 57 | gf_AxReq[0].dt = 0; |
MasashiNomura | 22:24c9c2dedca9 | 58 | gf_AxReq[1].dt = 0; |
MasashiNomura | 27:ff63c23bc689 | 59 | for(int i=0; i<4;++i){ |
MasashiNomura | 27:ff63c23bc689 | 60 | gf_MtReq[i].dt = 0; |
MasashiNomura | 27:ff63c23bc689 | 61 | gf_MtReqOfs[i].dt = 0; |
MasashiNomura | 29:eb3d72dd94aa | 62 | init1PushStruct(gf_MtReqOP[i]); |
MasashiNomura | 29:eb3d72dd94aa | 63 | //gf_MtReqOP[i].req = false; |
MasashiNomura | 29:eb3d72dd94aa | 64 | //gf_MtReqOP[i].sFlg = false; |
MasashiNomura | 29:eb3d72dd94aa | 65 | //gf_MtReqOP[i].add_end = false; |
MasashiNomura | 29:eb3d72dd94aa | 66 | //gf_MtReqOP[i].addVal = 0; |
MasashiNomura | 29:eb3d72dd94aa | 67 | //gf_MtReqOP[i].counter = 0; |
MasashiNomura | 29:eb3d72dd94aa | 68 | //gf_MtReqOP[i].num = 0; |
MasashiNomura | 29:eb3d72dd94aa | 69 | //gf_MtReqOP[i].sVal = 0; |
MasashiNomura | 27:ff63c23bc689 | 70 | } |
MasashiNomura | 27:ff63c23bc689 | 71 | for(int i = 0; i < 8; ++i){ |
MasashiNomura | 27:ff63c23bc689 | 72 | gf_MtReqDct[i].dt = 0; |
MasashiNomura | 27:ff63c23bc689 | 73 | } |
MasashiNomura | 22:24c9c2dedca9 | 74 | gf_State = SLEEP; |
MasashiNomura | 27:ff63c23bc689 | 75 | g_PidPara.PP = 2; |
MasashiNomura | 27:ff63c23bc689 | 76 | g_PidPara.P = 2; |
MasashiNomura | 25:f3a6e7eec9c3 | 77 | g_PidPara.I = 0; |
MasashiNomura | 24:c5945aaae777 | 78 | g_PidPara.D = 0; |
MasashiNomura | 24:c5945aaae777 | 79 | g_PidPara.IMax = 2000; |
MasashiNomura | 24:c5945aaae777 | 80 | g_PidPara.IMin = -2000; |
MasashiNomura | 25:f3a6e7eec9c3 | 81 | for(int i = 0; i < 4; ++i){ |
MasashiNomura | 25:f3a6e7eec9c3 | 82 | gf_MotParaUpdate[i] = false; |
MasashiNomura | 25:f3a6e7eec9c3 | 83 | //g_MotPara[i].offset = 0; |
MasashiNomura | 25:f3a6e7eec9c3 | 84 | g_MotPara[i].limit_hi = LIM_MOT_INP_MAX; |
MasashiNomura | 25:f3a6e7eec9c3 | 85 | g_MotPara[i].limit_low = LIM_MOT_INP_MIN; |
MasashiNomura | 25:f3a6e7eec9c3 | 86 | } |
MasashiNomura | 22:24c9c2dedca9 | 87 | } |
MasashiNomura | 29:eb3d72dd94aa | 88 | void init1PushStruct(typ1Push& item){ |
MasashiNomura | 29:eb3d72dd94aa | 89 | item.req = false; |
MasashiNomura | 29:eb3d72dd94aa | 90 | item.sFlg = false; |
MasashiNomura | 29:eb3d72dd94aa | 91 | item.add_end = false; |
MasashiNomura | 29:eb3d72dd94aa | 92 | item.addVal = 0; |
MasashiNomura | 29:eb3d72dd94aa | 93 | item.counter = 0; |
MasashiNomura | 29:eb3d72dd94aa | 94 | item.num = 0; |
MasashiNomura | 29:eb3d72dd94aa | 95 | item.sVal = 0; |
MasashiNomura | 29:eb3d72dd94aa | 96 | } |
MasashiNomura | 22:24c9c2dedca9 | 97 | void setState(enmHbState stat){ |
MasashiNomura | 22:24c9c2dedca9 | 98 | // ありえない遷移を排除 |
MasashiNomura | 22:24c9c2dedca9 | 99 | if(gf_State == SLEEP){if(stat != WAKEUP) return;} |
MasashiNomura | 22:24c9c2dedca9 | 100 | if(gf_State == WAKEUP){if(stat != STANDBY) return;} |
MasashiNomura | 22:24c9c2dedca9 | 101 | if(gf_State == STANDBY){if(stat !=IDLE) return;} |
MasashiNomura | 23:79e20be4bc5b | 102 | if(gf_State == IDLE){if(stat != TAKE_OFF && stat != SLEEP) return;} |
MasashiNomura | 23:79e20be4bc5b | 103 | if(gf_State == TAKE_OFF){if(stat != HOVER && stat != GROUND) return;} |
MasashiNomura | 22:24c9c2dedca9 | 104 | if(gf_State == GROUND){if(stat != IDLE) return;} |
MasashiNomura | 22:24c9c2dedca9 | 105 | if(gf_State == HOVER){if(stat != DRIVE && stat != GROUND) return;} |
MasashiNomura | 22:24c9c2dedca9 | 106 | if(gf_State == DRIVE){if(stat != HOVER && stat != EMGGND) return;} |
MasashiNomura | 22:24c9c2dedca9 | 107 | if(gf_State == EMGGND){if(stat != IDLE) return;} |
MasashiNomura | 22:24c9c2dedca9 | 108 | |
MasashiNomura | 23:79e20be4bc5b | 109 | sp.printf("state: %d\r\n",stat); |
MasashiNomura | 22:24c9c2dedca9 | 110 | gf_State = stat; |
MasashiNomura | 22:24c9c2dedca9 | 111 | } |
MasashiNomura | 23:79e20be4bc5b | 112 | void setStateF(enmHbState stat){ |
MasashiNomura | 23:79e20be4bc5b | 113 | sp.printf("state: %d\r\n",stat); |
MasashiNomura | 23:79e20be4bc5b | 114 | enmHbState tmp; |
MasashiNomura | 25:f3a6e7eec9c3 | 115 | if((UINT16)stat > MOT_STOP){ |
MasashiNomura | 23:79e20be4bc5b | 116 | tmp = SLEEP; |
MasashiNomura | 23:79e20be4bc5b | 117 | } |
MasashiNomura | 23:79e20be4bc5b | 118 | else |
MasashiNomura | 23:79e20be4bc5b | 119 | { |
MasashiNomura | 23:79e20be4bc5b | 120 | tmp = stat; |
MasashiNomura | 23:79e20be4bc5b | 121 | } |
MasashiNomura | 23:79e20be4bc5b | 122 | gf_State = tmp; |
MasashiNomura | 23:79e20be4bc5b | 123 | } |
takeru0x1103 | 21:78302ecdb661 | 124 | /* |
takeru0x1103 | 19:4b0fe9a5ec38 | 125 | //=========================================== |
takeru0x1103 | 19:4b0fe9a5ec38 | 126 | //コンストラクタ |
takeru0x1103 | 19:4b0fe9a5ec38 | 127 | //=========================================== |
takeru0x1103 | 19:4b0fe9a5ec38 | 128 | GlobalFlags::GlobalFlags(){ |
takeru0x1103 | 19:4b0fe9a5ec38 | 129 | //初期ステート |
takeru0x1103 | 21:78302ecdb661 | 130 | state = SLEEP; |
takeru0x1103 | 19:4b0fe9a5ec38 | 131 | //ステートキュー初期化 |
takeru0x1103 | 19:4b0fe9a5ec38 | 132 | for(int i=0; i<STATE_QUE_SIZ; i++){ |
takeru0x1103 | 19:4b0fe9a5ec38 | 133 | stateQueue[i] = NONE; |
takeru0x1103 | 19:4b0fe9a5ec38 | 134 | } |
takeru0x1103 | 19:4b0fe9a5ec38 | 135 | rp = wp = 0;//先頭を指す |
takeru0x1103 | 19:4b0fe9a5ec38 | 136 | full = false;//最初はエンプティ |
takeru0x1103 | 19:4b0fe9a5ec38 | 137 | empty = true; |
takeru0x1103 | 19:4b0fe9a5ec38 | 138 | } |
takeru0x1103 | 19:4b0fe9a5ec38 | 139 | |
takeru0x1103 | 21:78302ecdb661 | 140 | |
takeru0x1103 | 19:4b0fe9a5ec38 | 141 | //----------------------------------- |
takeru0x1103 | 19:4b0fe9a5ec38 | 142 | //ステートのFIFOにプッシュする |
takeru0x1103 | 19:4b0fe9a5ec38 | 143 | //----------------------------------- |
takeru0x1103 | 19:4b0fe9a5ec38 | 144 | bool GlobalFlags::push(enmHbState iState){ |
takeru0x1103 | 19:4b0fe9a5ec38 | 145 | if(full){ |
takeru0x1103 | 19:4b0fe9a5ec38 | 146 | return false;//FFULLフラグが立ってら入力を受け付けない |
takeru0x1103 | 19:4b0fe9a5ec38 | 147 | }else{ |
takeru0x1103 | 19:4b0fe9a5ec38 | 148 | stateQueue[wp] = iState;//キューに突っ込んで |
takeru0x1103 | 19:4b0fe9a5ec38 | 149 | wp = (wp==STATE_QUE_SIZ) ? 0 : wp+1;//ライトポインタを進める |
takeru0x1103 | 19:4b0fe9a5ec38 | 150 | full = (wp == rp)? true : false; //次ライト予定の場所がリードポインタに追いついたらFULLを立てる |
takeru0x1103 | 19:4b0fe9a5ec38 | 151 | empty = false; |
takeru0x1103 | 19:4b0fe9a5ec38 | 152 | return true; |
takeru0x1103 | 19:4b0fe9a5ec38 | 153 | } |
takeru0x1103 | 19:4b0fe9a5ec38 | 154 | } |
takeru0x1103 | 19:4b0fe9a5ec38 | 155 | |
takeru0x1103 | 19:4b0fe9a5ec38 | 156 | //----------------------------------- |
takeru0x1103 | 19:4b0fe9a5ec38 | 157 | //ステートFIFOから次の予約ステートを引き抜く |
takeru0x1103 | 19:4b0fe9a5ec38 | 158 | //----------------------------------- |
takeru0x1103 | 19:4b0fe9a5ec38 | 159 | bool GlobalFlags::pull(){ |
takeru0x1103 | 19:4b0fe9a5ec38 | 160 | if(empty){ |
takeru0x1103 | 19:4b0fe9a5ec38 | 161 | return false;//EMPTYだったら引き抜けない |
takeru0x1103 | 19:4b0fe9a5ec38 | 162 | }else{ |
takeru0x1103 | 19:4b0fe9a5ec38 | 163 | state = stateQueue[rp];//リードポインタの指すデータをステートに移す |
takeru0x1103 | 19:4b0fe9a5ec38 | 164 | stateQueue[rp] = NONE;//バッファを消す |
takeru0x1103 | 19:4b0fe9a5ec38 | 165 | rp = (rp==STATE_QUE_SIZ) ? 0 : rp+1;//リードポインタを進める |
takeru0x1103 | 19:4b0fe9a5ec38 | 166 | full = false; //フルではなくなる |
takeru0x1103 | 19:4b0fe9a5ec38 | 167 | empty = (stateQueue[rp] == NONE) ? true : false;//次にリードする場所がNONEだったらEMPTYを立てる |
takeru0x1103 | 19:4b0fe9a5ec38 | 168 | return true; |
takeru0x1103 | 19:4b0fe9a5ec38 | 169 | } |
takeru0x1103 | 19:4b0fe9a5ec38 | 170 | } |
takeru0x1103 | 21:78302ecdb661 | 171 | */ |
takeru0x1103 | 19:4b0fe9a5ec38 | 172 | |
takeru0x1103 | 19:4b0fe9a5ec38 | 173 |