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

globalFlags.cpp@40:debe99e228d3, 2019-01-21 (annotated)

Committer:
MasashiNomura
Date:
Mon Jan 21 06:32:24 2019 +0000
Revision:
40:debe99e228d3
Parent:
39:1b76f7df8804
Child:
41:45c982b1c5b6
20190121 Modify MotorCtrl etc

Who changed what in which revision?

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