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@41:45c982b1c5b6, 2019-01-21 (annotated)

Committer:
MasashiNomura
Date:
Mon Jan 21 11:57:35 2019 +0000
Revision:
41:45c982b1c5b6
Parent:
40:debe99e228d3
Child:
42:cc8501b824ba
20190121 Modify for 45inc. HbMotor,HbManager,GF 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 41:45c982b1c5b6 11 DigitalOut DO_01(p21);//R
MasashiNomura 41:45c982b1c5b6 12 DigitalOut DO_02(p22);//G
MasashiNomura 41:45c982b1c5b6 13 DigitalOut DO_03(p23);//B
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
MasashiNomura 41:45c982b1c5b6 30 typPrintFlg gf_Print ;//
MasashiNomura 41:45c982b1c5b6 31 typPrintFlg gf_Mon ;//
takeru0x1103 19:4b0fe9a5ec38 32 typCalFlag gf_Cal ;//
MasashiNomura 41:45c982b1c5b6 33 //typDbgPrintFlg gf_DbgPrint ;//デバッグ用
MasashiNomura 22:24c9c2dedca9 34 typAccel gf_AxReq[2] ;//アクセル更新フラグ
MasashiNomura 32:7f4145cc3551 35 typAxlRpm gf_MtReq[4] ;//モーター姿勢制御更新フラグ
MasashiNomura 29:eb3d72dd94aa 36 typAxlRpm gf_MtReqOfs[4] ;//モーターオフセット更新フラグ
MasashiNomura 29:eb3d72dd94aa 37 typAxlRpm gf_MtReqDct[8] ;//ダイレクト(FPGA関数直接呼び出し)モーター更新フラグ
MasashiNomura 32:7f4145cc3551 38 typAxlRpm gf_MtReqU[4] ;//モーターユーザー更新フラグ
MasashiNomura 32:7f4145cc3551 39
MasashiNomura 22:24c9c2dedca9 40 enmHbState gf_State = SLEEP;
MasashiNomura 36:2cc739c7e4cb 41 bool gf_StateEnt ;//状態遷移後、最初であることを示す
takeru0x1103 19:4b0fe9a5ec38 42
MasashiNomura 24:c5945aaae777 43 bool gf_PidParaUpdate ;//PID Pp,P,I,Dの係数アップデートフラグ
MasashiNomura 24:c5945aaae777 44 typPidPara g_PidPara ;//PID Pp,P,I,Dの係数の外部設定用
takeru0x1103 20:0394e15412c3 45
MasashiNomura 36:2cc739c7e4cb 46 // bool gf_MotParaUpdate[4] ;//モーターのパラメータ値更新フラグ
MasashiNomura 36:2cc739c7e4cb 47 // typMotPara g_MotPara[4];
MasashiNomura 25:f3a6e7eec9c3 48
MasashiNomura 22:24c9c2dedca9 49 void initFlags(){
MasashiNomura 22:24c9c2dedca9 50 memset(g_CmdBuf,0x0,sizeof(g_CmdBuf));
MasashiNomura 22:24c9c2dedca9 51 gf_CmdPrs = false;
MasashiNomura 22:24c9c2dedca9 52 gf_Armed = false;
MasashiNomura 22:24c9c2dedca9 53 gf_Dbg = false;
MasashiNomura 25:f3a6e7eec9c3 54 gf_StopMot = false;
MasashiNomura 25:f3a6e7eec9c3 55
MasashiNomura 41:45c982b1c5b6 56 // gf_Print.flg = 0;
MasashiNomura 41:45c982b1c5b6 57 // gf_Mon.flg = 0;
MasashiNomura 41:45c982b1c5b6 58 gf_Print.d1.flg = 0;
MasashiNomura 41:45c982b1c5b6 59 gf_Print.d2.flg = 0;
MasashiNomura 39:1b76f7df8804 60
MasashiNomura 22:24c9c2dedca9 61 gf_Cal.flg = 0;
MasashiNomura 26:732bc37fbefd 62
MasashiNomura 41:45c982b1c5b6 63 //gf_DbgPrint.flg = 0;
MasashiNomura 26:732bc37fbefd 64
MasashiNomura 22:24c9c2dedca9 65 gf_AxReq[0].dt = 0;
MasashiNomura 22:24c9c2dedca9 66 gf_AxReq[1].dt = 0;
MasashiNomura 27:ff63c23bc689 67 for(int i=0; i<4;++i){
MasashiNomura 32:7f4145cc3551 68 gf_MtReq[i].req = false;
MasashiNomura 32:7f4145cc3551 69 gf_MtReq[i].val = 0;
MasashiNomura 32:7f4145cc3551 70 gf_MtReqOfs[i].req = false;
MasashiNomura 32:7f4145cc3551 71 gf_MtReqOfs[i].val = 0;
MasashiNomura 32:7f4145cc3551 72 gf_MtReqU[i].req = false;
MasashiNomura 32:7f4145cc3551 73 gf_MtReqU[i].val = 0;
MasashiNomura 27:ff63c23bc689 74 }
MasashiNomura 27:ff63c23bc689 75 for(int i = 0; i < 8; ++i){
MasashiNomura 32:7f4145cc3551 76 gf_MtReqDct[i].req = false;
MasashiNomura 32:7f4145cc3551 77 gf_MtReqDct[i].val = 0;
MasashiNomura 27:ff63c23bc689 78 }
MasashiNomura 22:24c9c2dedca9 79 gf_State = SLEEP;
MasashiNomura 36:2cc739c7e4cb 80 gf_StateEnt = false;
MasashiNomura 27:ff63c23bc689 81 g_PidPara.PP = 2;
MasashiNomura 32:7f4145cc3551 82 g_PidPara.P = 0;
MasashiNomura 25:f3a6e7eec9c3 83 g_PidPara.I = 0;
MasashiNomura 24:c5945aaae777 84 g_PidPara.D = 0;
MasashiNomura 24:c5945aaae777 85 g_PidPara.IMax = 2000;
MasashiNomura 24:c5945aaae777 86 g_PidPara.IMin = -2000;
MasashiNomura 36:2cc739c7e4cb 87 // for(int i = 0; i < 4; ++i){
MasashiNomura 36:2cc739c7e4cb 88 // gf_MotParaUpdate[i] = false;
MasashiNomura 36:2cc739c7e4cb 89 // g_MotPara[i].limit_hi = LIM_MOT_INP_MAX;
MasashiNomura 36:2cc739c7e4cb 90 // g_MotPara[i].limit_low = LIM_MOT_INP_MIN;
MasashiNomura 36:2cc739c7e4cb 91 // }
MasashiNomura 22:24c9c2dedca9 92 }
MasashiNomura 35:3779201b4c73 93
MasashiNomura 41:45c982b1c5b6 94 void setDOCol(eLedCol col)
MasashiNomura 41:45c982b1c5b6 95 {
MasashiNomura 41:45c982b1c5b6 96 switch(col){
MasashiNomura 41:45c982b1c5b6 97 case WHITE:
MasashiNomura 41:45c982b1c5b6 98 DO_01 = !DO_01;
MasashiNomura 41:45c982b1c5b6 99 DO_02 = !DO_02;
MasashiNomura 41:45c982b1c5b6 100 DO_03 = !DO_03;
MasashiNomura 41:45c982b1c5b6 101 break;
MasashiNomura 41:45c982b1c5b6 102 case RED:
MasashiNomura 41:45c982b1c5b6 103 DO_01 = !DO_01;
MasashiNomura 41:45c982b1c5b6 104 DO_02 = 0;
MasashiNomura 41:45c982b1c5b6 105 DO_03 = 0;
MasashiNomura 41:45c982b1c5b6 106 break;
MasashiNomura 41:45c982b1c5b6 107 case GREEN:
MasashiNomura 41:45c982b1c5b6 108 DO_01 = 0;
MasashiNomura 41:45c982b1c5b6 109 DO_02 = !DO_02;
MasashiNomura 41:45c982b1c5b6 110 DO_03 = 0;
MasashiNomura 41:45c982b1c5b6 111 break;
MasashiNomura 41:45c982b1c5b6 112 case BLUE:
MasashiNomura 41:45c982b1c5b6 113 DO_01 = 0;
MasashiNomura 41:45c982b1c5b6 114 DO_02 = 0;
MasashiNomura 41:45c982b1c5b6 115 DO_03 = !DO_03;
MasashiNomura 41:45c982b1c5b6 116 break;
MasashiNomura 41:45c982b1c5b6 117 case YELLOW:
MasashiNomura 41:45c982b1c5b6 118 DO_01 = !DO_01;
MasashiNomura 41:45c982b1c5b6 119 DO_02 = !DO_02;
MasashiNomura 41:45c982b1c5b6 120 DO_03 = 0;
MasashiNomura 41:45c982b1c5b6 121 break;
MasashiNomura 41:45c982b1c5b6 122 case PURPLE:
MasashiNomura 41:45c982b1c5b6 123 DO_01 = !DO_01;
MasashiNomura 41:45c982b1c5b6 124 DO_02 = 0;
MasashiNomura 41:45c982b1c5b6 125 DO_03 = !DO_03;
MasashiNomura 41:45c982b1c5b6 126 break;
MasashiNomura 41:45c982b1c5b6 127 case LIGHT_BLUE:
MasashiNomura 41:45c982b1c5b6 128 DO_01 = 0;
MasashiNomura 41:45c982b1c5b6 129 DO_02 = !DO_02;
MasashiNomura 41:45c982b1c5b6 130 DO_03 = !DO_03;
MasashiNomura 41:45c982b1c5b6 131 break;
MasashiNomura 41:45c982b1c5b6 132 case OFF:
MasashiNomura 41:45c982b1c5b6 133 default:
MasashiNomura 41:45c982b1c5b6 134 DO_01 = 0;//R
MasashiNomura 41:45c982b1c5b6 135 DO_02 = 0;//G
MasashiNomura 41:45c982b1c5b6 136 DO_03 = 0;//B
MasashiNomura 41:45c982b1c5b6 137 break;
MasashiNomura 41:45c982b1c5b6 138 }
MasashiNomura 41:45c982b1c5b6 139 }
MasashiNomura 41:45c982b1c5b6 140
MasashiNomura 41:45c982b1c5b6 141 void setDO4LED(enmHbState stat)
MasashiNomura 41:45c982b1c5b6 142 {
MasashiNomura 41:45c982b1c5b6 143 if(stat == NONE)
MasashiNomura 41:45c982b1c5b6 144 {// 消灯
MasashiNomura 41:45c982b1c5b6 145 setDOCol(OFF);
MasashiNomura 41:45c982b1c5b6 146 }
MasashiNomura 41:45c982b1c5b6 147 else if(stat == SLEEP)
MasashiNomura 41:45c982b1c5b6 148 {//YELLOW
MasashiNomura 41:45c982b1c5b6 149 setDOCol(YELLOW);
MasashiNomura 41:45c982b1c5b6 150 }
MasashiNomura 41:45c982b1c5b6 151 else if(stat == WAKEUP || stat == STANDBY)
MasashiNomura 41:45c982b1c5b6 152 {//BLUE
MasashiNomura 41:45c982b1c5b6 153 setDOCol(BLUE);
MasashiNomura 41:45c982b1c5b6 154 }
MasashiNomura 41:45c982b1c5b6 155 else if(stat == IDLE)
MasashiNomura 41:45c982b1c5b6 156 {//GREEN
MasashiNomura 41:45c982b1c5b6 157 setDOCol(GREEN);
MasashiNomura 41:45c982b1c5b6 158 }
MasashiNomura 41:45c982b1c5b6 159 else if(stat == TAKE_OFF || stat == HOVER || stat == DRIVE || stat == GROUND || stat == EMGGND)
MasashiNomura 41:45c982b1c5b6 160 {//PURPLE
MasashiNomura 41:45c982b1c5b6 161 setDOCol(PURPLE);
MasashiNomura 41:45c982b1c5b6 162 }
MasashiNomura 41:45c982b1c5b6 163 else if(stat == CHK_EG_ENT || stat == CHK_EG_F || stat == CHK_EG_MID || stat == CHK_EG_R || stat == CHK_EG_EXIT)
MasashiNomura 41:45c982b1c5b6 164 {//LIGHT BLUE
MasashiNomura 41:45c982b1c5b6 165 setDOCol(LIGHT_BLUE);
MasashiNomura 41:45c982b1c5b6 166 }
MasashiNomura 41:45c982b1c5b6 167 else if(stat == CHK_ENT || stat == CHK_MOT || stat == CHK_AXL || stat == CHK_ATT || stat == CHK_EXIT)
MasashiNomura 41:45c982b1c5b6 168 {//WHITE
MasashiNomura 41:45c982b1c5b6 169 setDOCol(WHITE);
MasashiNomura 41:45c982b1c5b6 170 }
MasashiNomura 41:45c982b1c5b6 171 else/* if(stat == MOT_STOP)*/
MasashiNomura 41:45c982b1c5b6 172 {
MasashiNomura 41:45c982b1c5b6 173 setDOCol(RED);
MasashiNomura 41:45c982b1c5b6 174 }
MasashiNomura 41:45c982b1c5b6 175 }
MasashiNomura 41:45c982b1c5b6 176
MasashiNomura 22:24c9c2dedca9 177 void setState(enmHbState stat){
MasashiNomura 22:24c9c2dedca9 178 // ありえない遷移を排除
MasashiNomura 22:24c9c2dedca9 179 if(gf_State == SLEEP){if(stat != WAKEUP) return;}
MasashiNomura 22:24c9c2dedca9 180 if(gf_State == WAKEUP){if(stat != STANDBY) return;}
MasashiNomura 22:24c9c2dedca9 181 if(gf_State == STANDBY){if(stat !=IDLE) return;}
MasashiNomura 23:79e20be4bc5b 182 if(gf_State == IDLE){if(stat != TAKE_OFF && stat != SLEEP) return;}
MasashiNomura 23:79e20be4bc5b 183 if(gf_State == TAKE_OFF){if(stat != HOVER && stat != GROUND) return;}
MasashiNomura 22:24c9c2dedca9 184 if(gf_State == GROUND){if(stat != IDLE) return;}
MasashiNomura 22:24c9c2dedca9 185 if(gf_State == HOVER){if(stat != DRIVE && stat != GROUND) return;}
MasashiNomura 22:24c9c2dedca9 186 if(gf_State == DRIVE){if(stat != HOVER && stat != EMGGND) return;}
MasashiNomura 22:24c9c2dedca9 187 if(gf_State == EMGGND){if(stat != IDLE) return;}
MasashiNomura 22:24c9c2dedca9 188
MasashiNomura 23:79e20be4bc5b 189 sp.printf("state: %d\r\n",stat);
MasashiNomura 22:24c9c2dedca9 190 gf_State = stat;
MasashiNomura 36:2cc739c7e4cb 191 gf_StateEnt = true;
MasashiNomura 22:24c9c2dedca9 192 }
MasashiNomura 23:79e20be4bc5b 193 void setStateF(enmHbState stat){
MasashiNomura 23:79e20be4bc5b 194 sp.printf("state: %d\r\n",stat);
MasashiNomura 23:79e20be4bc5b 195 enmHbState tmp;
MasashiNomura 25:f3a6e7eec9c3 196 if((UINT16)stat > MOT_STOP){
MasashiNomura 23:79e20be4bc5b 197 tmp = SLEEP;
MasashiNomura 23:79e20be4bc5b 198 }
MasashiNomura 23:79e20be4bc5b 199 else
MasashiNomura 23:79e20be4bc5b 200 {
MasashiNomura 23:79e20be4bc5b 201 tmp = stat;
MasashiNomura 23:79e20be4bc5b 202 }
MasashiNomura 23:79e20be4bc5b 203 gf_State = tmp;
MasashiNomura 36:2cc739c7e4cb 204 gf_StateEnt = true;
MasashiNomura 23:79e20be4bc5b 205 }
MasashiNomura 37:d51dacb4c30f 206
MasashiNomura 37:d51dacb4c30f 207 bool isActiveState(){
MasashiNomura 37:d51dacb4c30f 208 if(gf_State >= IDLE && gf_State <= EMGGND){
MasashiNomura 37:d51dacb4c30f 209 return true;
MasashiNomura 37:d51dacb4c30f 210 }
MasashiNomura 37:d51dacb4c30f 211 else {
MasashiNomura 37:d51dacb4c30f 212 return false;
MasashiNomura 37:d51dacb4c30f 213 }
MasashiNomura 37:d51dacb4c30f 214 }
MasashiNomura 37:d51dacb4c30f 215
takeru0x1103 21:78302ecdb661 216 /*
takeru0x1103 19:4b0fe9a5ec38 217 //===========================================
takeru0x1103 19:4b0fe9a5ec38 218 //コンストラクタ
takeru0x1103 19:4b0fe9a5ec38 219 //===========================================
takeru0x1103 19:4b0fe9a5ec38 220 GlobalFlags::GlobalFlags(){
takeru0x1103 19:4b0fe9a5ec38 221 //初期ステート
takeru0x1103 21:78302ecdb661 222 state = SLEEP;
takeru0x1103 19:4b0fe9a5ec38 223 //ステートキュー初期化
takeru0x1103 19:4b0fe9a5ec38 224 for(int i=0; i<STATE_QUE_SIZ; i++){
takeru0x1103 19:4b0fe9a5ec38 225 stateQueue[i] = NONE;
MasashiNomura 33:eb260dbfc22a 226 }
takeru0x1103 19:4b0fe9a5ec38 227 rp = wp = 0;//先頭を指す
takeru0x1103 19:4b0fe9a5ec38 228 full = false;//最初はエンプティ
takeru0x1103 19:4b0fe9a5ec38 229 empty = true;
takeru0x1103 19:4b0fe9a5ec38 230 }
takeru0x1103 19:4b0fe9a5ec38 231
takeru0x1103 21:78302ecdb661 232
takeru0x1103 19:4b0fe9a5ec38 233 //-----------------------------------
takeru0x1103 19:4b0fe9a5ec38 234 //ステートのFIFOにプッシュする
takeru0x1103 19:4b0fe9a5ec38 235 //-----------------------------------
takeru0x1103 19:4b0fe9a5ec38 236 bool GlobalFlags::push(enmHbState iState){
takeru0x1103 19:4b0fe9a5ec38 237 if(full){
takeru0x1103 19:4b0fe9a5ec38 238 return false;//FFULLフラグが立ってら入力を受け付けない
takeru0x1103 19:4b0fe9a5ec38 239 }else{
takeru0x1103 19:4b0fe9a5ec38 240 stateQueue[wp] = iState;//キューに突っ込んで
takeru0x1103 19:4b0fe9a5ec38 241 wp = (wp==STATE_QUE_SIZ) ? 0 : wp+1;//ライトポインタを進める
takeru0x1103 19:4b0fe9a5ec38 242 full = (wp == rp)? true : false; //次ライト予定の場所がリードポインタに追いついたらFULLを立てる
takeru0x1103 19:4b0fe9a5ec38 243 empty = false;
takeru0x1103 19:4b0fe9a5ec38 244 return true;
takeru0x1103 19:4b0fe9a5ec38 245 }
takeru0x1103 19:4b0fe9a5ec38 246 }
takeru0x1103 19:4b0fe9a5ec38 247
takeru0x1103 19:4b0fe9a5ec38 248 //-----------------------------------
takeru0x1103 19:4b0fe9a5ec38 249 //ステートFIFOから次の予約ステートを引き抜く
takeru0x1103 19:4b0fe9a5ec38 250 //-----------------------------------
takeru0x1103 19:4b0fe9a5ec38 251 bool GlobalFlags::pull(){
takeru0x1103 19:4b0fe9a5ec38 252 if(empty){
takeru0x1103 19:4b0fe9a5ec38 253 return false;//EMPTYだったら引き抜けない
takeru0x1103 19:4b0fe9a5ec38 254 }else{
takeru0x1103 19:4b0fe9a5ec38 255 state = stateQueue[rp];//リードポインタの指すデータをステートに移す
takeru0x1103 19:4b0fe9a5ec38 256 stateQueue[rp] = NONE;//バッファを消す
takeru0x1103 19:4b0fe9a5ec38 257 rp = (rp==STATE_QUE_SIZ) ? 0 : rp+1;//リードポインタを進める
takeru0x1103 19:4b0fe9a5ec38 258 full = false; //フルではなくなる
takeru0x1103 19:4b0fe9a5ec38 259 empty = (stateQueue[rp] == NONE) ? true : false;//次にリードする場所がNONEだったらEMPTYを立てる
takeru0x1103 19:4b0fe9a5ec38 260 return true;
takeru0x1103 19:4b0fe9a5ec38 261 }
takeru0x1103 19:4b0fe9a5ec38 262 }
takeru0x1103 21:78302ecdb661 263 */
takeru0x1103 19:4b0fe9a5ec38 264
MasashiNomura 33:eb260dbfc22a 265