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@48:71aec693a7dc, 2019-02-24 (annotated)

Committer:
MasashiNomura
Date:
Sun Feb 24 10:33:34 2019 +0000
Revision:
48:71aec693a7dc
Parent:
45:3b51dd26b579
Child:
50:76413e8e073d
20190224 add cmd pid mode etc.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
MasashiNomura 45:3b51dd26b579 1 /**
MasashiNomura 45:3b51dd26b579 2 * @file globalFlags.cpp
MasashiNomura 45:3b51dd26b579 3 * @brief コマンド解析<-->ホバーバイク制御タスク間及びホバーバイク制御タスク内で使用する共用フラグを使用する変数宣言と関数定義
MasashiNomura 45:3b51dd26b579 4 * @author Shirota, Nomura
MasashiNomura 45:3b51dd26b579 5 * @date 2018/10/01 ?
MasashiNomura 45:3b51dd26b579 6 */
takeru0x1103 19:4b0fe9a5ec38 7
MasashiNomura 48:71aec693a7dc 8 #include "typedef.h"
takeru0x1103 16:05b9e44889f1 9 #include "globalFlags.h"
takeru0x1103 16:05b9e44889f1 10
MasashiNomura 45:3b51dd26b579 11 DigitalOut led1(LED1); //!< モニタ用LED1用変数
MasashiNomura 45:3b51dd26b579 12 DigitalOut led2(LED2); //!< モニタ用LED2用変数
MasashiNomura 45:3b51dd26b579 13 DigitalOut led3(LED3); //!< モニタ用LED3用変数
MasashiNomura 45:3b51dd26b579 14 DigitalOut led4(LED4); //!< モニタ用LED4用変数
takeru0x1103 8:1ca49cb18290 15
MasashiNomura 45:3b51dd26b579 16 DigitalOut DO_01(p21); //!< 状態表示用LED1(R)用変数
MasashiNomura 45:3b51dd26b579 17 DigitalOut DO_02(p22); //!< 状態表示用LED2(G)用変数
MasashiNomura 45:3b51dd26b579 18 DigitalOut DO_03(p23); //!< 状態表示用LED3(B)用変数
MasashiNomura 40:debe99e228d3 19
MasashiNomura 45:3b51dd26b579 20 AnalogIn AinAxl(p20); //!< モーターアクセル用アナログ入力
MasashiNomura 45:3b51dd26b579 21
takeru0x1103 21:78302ecdb661 22
MasashiNomura 45:3b51dd26b579 23 char g_CmdBuf[G_CMD_BUF_SIZ]; //!< コマンド受け渡しバッファ
MasashiNomura 45:3b51dd26b579 24 bool gf_CmdPrs; //!< コマンドパーサー実行要求フラグ
MasashiNomura 36:2cc739c7e4cb 25
MasashiNomura 45:3b51dd26b579 26 bool gf_Armed; //!< アーミングフラグ
MasashiNomura 45:3b51dd26b579 27 bool gf_Dbg; //!< デバッグタスク起動フラグ
takeru0x1103 19:4b0fe9a5ec38 28
MasashiNomura 45:3b51dd26b579 29 bool gf_StopMot; //!< モーターの強制停止 サーボ全閉
MasashiNomura 45:3b51dd26b579 30 bool gf_FromActiveStat; //!< モーターの強制停止 サーボ全閉が動作時に発生したかどうか
MasashiNomura 25:f3a6e7eec9c3 31
MasashiNomura 43:156199c2f9b6 32 bool gf_BlinkLED;
MasashiNomura 43:156199c2f9b6 33
MasashiNomura 45:3b51dd26b579 34 typPrintFlg gf_Print; //!< デバッグプリントフラグ（１回表示）
MasashiNomura 45:3b51dd26b579 35 typPrintFlg gf_Mon; //!< デバッグモニタフラグ（繰り返し表示）
MasashiNomura 45:3b51dd26b579 36 typCalFlag gf_Cal; //!< キャリブレーションフラグ
MasashiNomura 32:7f4145cc3551 37
MasashiNomura 45:3b51dd26b579 38 typAccel gf_AxReq[2]; //!< エンジンアクセル値更新フラグ
MasashiNomura 45:3b51dd26b579 39 typAccel gf_AxReqH[2]; //!< 浮上時エンジンアクセル値更新フラグ
MasashiNomura 45:3b51dd26b579 40 typAccel gf_AxStepReq[2]; //!< エンジンアクセル用サーボ動作ステップ値更新フラグ
MasashiNomura 45:3b51dd26b579 41 typAxlRpm gf_MtReq[4]; //!< モーター姿勢制御値更新
MasashiNomura 45:3b51dd26b579 42 typAxlRpm gf_MtReqOfs[4]; //!< モーターオフセット値更新
MasashiNomura 45:3b51dd26b579 43 typAxlRpm gf_MtReqU[4]; //!< モーターユーザー値更新
MasashiNomura 45:3b51dd26b579 44 typAxlRpm gf_MtReqDct[8]; //!< ダイレクト(FPGA関数直接呼び出し)モーター値更新フラグ
takeru0x1103 19:4b0fe9a5ec38 45
MasashiNomura 45:3b51dd26b579 46 typSWCmd gf_SwCmd; //!< スイッチが押されたことを模擬するフラグ
MasashiNomura 45:3b51dd26b579 47
MasashiNomura 45:3b51dd26b579 48 enmHbState gf_State = SLEEP; //!< 現在のステートを格納する変数
takeru0x1103 20:0394e15412c3 49
MasashiNomura 45:3b51dd26b579 50 bool gf_StateEnt; //!< 状態遷移後、最初であることを示す
MasashiNomura 45:3b51dd26b579 51
MasashiNomura 45:3b51dd26b579 52 bool gf_PidParaUpdate; //!< PID Pp,P,I,Dの係数アップデートフラグ
MasashiNomura 45:3b51dd26b579 53 typPidPara g_PidPara; //!< PID Pp,P,I,Dの係数の外部設定用
MasashiNomura 45:3b51dd26b579 54
MasashiNomura 25:f3a6e7eec9c3 55
MasashiNomura 22:24c9c2dedca9 56 void initFlags(){
MasashiNomura 22:24c9c2dedca9 57 memset(g_CmdBuf,0x0,sizeof(g_CmdBuf));
MasashiNomura 22:24c9c2dedca9 58 gf_CmdPrs = false;
MasashiNomura 22:24c9c2dedca9 59 gf_Armed = false;
MasashiNomura 22:24c9c2dedca9 60 gf_Dbg = false;
MasashiNomura 25:f3a6e7eec9c3 61 gf_StopMot = false;
MasashiNomura 43:156199c2f9b6 62
MasashiNomura 43:156199c2f9b6 63 gf_BlinkLED = false;
MasashiNomura 25:f3a6e7eec9c3 64
MasashiNomura 41:45c982b1c5b6 65 // gf_Print.flg = 0;
MasashiNomura 41:45c982b1c5b6 66 // gf_Mon.flg = 0;
MasashiNomura 41:45c982b1c5b6 67 gf_Print.d1.flg = 0;
MasashiNomura 41:45c982b1c5b6 68 gf_Print.d2.flg = 0;
MasashiNomura 39:1b76f7df8804 69
MasashiNomura 22:24c9c2dedca9 70 gf_Cal.flg = 0;
MasashiNomura 26:732bc37fbefd 71
MasashiNomura 41:45c982b1c5b6 72 //gf_DbgPrint.flg = 0;
MasashiNomura 26:732bc37fbefd 73
MasashiNomura 43:156199c2f9b6 74 // engine
MasashiNomura 43:156199c2f9b6 75 for(int i = 0; i < 2; ++i){
MasashiNomura 43:156199c2f9b6 76 gf_AxReq[i].dt = 0;
MasashiNomura 43:156199c2f9b6 77 gf_AxReqH[i].dt = 0;
MasashiNomura 43:156199c2f9b6 78 gf_AxStepReq[i].bf.req = false;
MasashiNomura 43:156199c2f9b6 79 gf_AxStepReq[i].bf.val = 100;
MasashiNomura 43:156199c2f9b6 80 }
MasashiNomura 43:156199c2f9b6 81
MasashiNomura 27:ff63c23bc689 82 for(int i=0; i<4;++i){
MasashiNomura 32:7f4145cc3551 83 gf_MtReq[i].req = false;
MasashiNomura 32:7f4145cc3551 84 gf_MtReq[i].val = 0;
MasashiNomura 32:7f4145cc3551 85 gf_MtReqOfs[i].req = false;
MasashiNomura 32:7f4145cc3551 86 gf_MtReqOfs[i].val = 0;
MasashiNomura 32:7f4145cc3551 87 gf_MtReqU[i].req = false;
MasashiNomura 32:7f4145cc3551 88 gf_MtReqU[i].val = 0;
MasashiNomura 27:ff63c23bc689 89 }
MasashiNomura 27:ff63c23bc689 90 for(int i = 0; i < 8; ++i){
MasashiNomura 32:7f4145cc3551 91 gf_MtReqDct[i].req = false;
MasashiNomura 32:7f4145cc3551 92 gf_MtReqDct[i].val = 0;
MasashiNomura 27:ff63c23bc689 93 }
MasashiNomura 42:cc8501b824ba 94
MasashiNomura 42:cc8501b824ba 95 gf_SwCmd.dt = 0;
MasashiNomura 42:cc8501b824ba 96
MasashiNomura 22:24c9c2dedca9 97 gf_State = SLEEP;
MasashiNomura 36:2cc739c7e4cb 98 gf_StateEnt = false;
MasashiNomura 27:ff63c23bc689 99 g_PidPara.PP = 2;
MasashiNomura 32:7f4145cc3551 100 g_PidPara.P = 0;
MasashiNomura 25:f3a6e7eec9c3 101 g_PidPara.I = 0;
MasashiNomura 24:c5945aaae777 102 g_PidPara.D = 0;
MasashiNomura 24:c5945aaae777 103 g_PidPara.IMax = 2000;
MasashiNomura 24:c5945aaae777 104 g_PidPara.IMin = -2000;
MasashiNomura 36:2cc739c7e4cb 105 // for(int i = 0; i < 4; ++i){
MasashiNomura 36:2cc739c7e4cb 106 // gf_MotParaUpdate[i] = false;
MasashiNomura 36:2cc739c7e4cb 107 // g_MotPara[i].limit_hi = LIM_MOT_INP_MAX;
MasashiNomura 36:2cc739c7e4cb 108 // g_MotPara[i].limit_low = LIM_MOT_INP_MIN;
MasashiNomura 36:2cc739c7e4cb 109 // }
MasashiNomura 22:24c9c2dedca9 110 }
MasashiNomura 35:3779201b4c73 111
MasashiNomura 41:45c982b1c5b6 112 void setDOCol(eLedCol col)
MasashiNomura 41:45c982b1c5b6 113 {
MasashiNomura 41:45c982b1c5b6 114 switch(col){
MasashiNomura 41:45c982b1c5b6 115 case WHITE:
MasashiNomura 42:cc8501b824ba 116 DO_01 = 1;
MasashiNomura 42:cc8501b824ba 117 DO_02 = 1;
MasashiNomura 42:cc8501b824ba 118 DO_03 = 1;
MasashiNomura 41:45c982b1c5b6 119 break;
MasashiNomura 41:45c982b1c5b6 120 case RED:
MasashiNomura 42:cc8501b824ba 121 DO_01 = 1;
MasashiNomura 41:45c982b1c5b6 122 DO_02 = 0;
MasashiNomura 41:45c982b1c5b6 123 DO_03 = 0;
MasashiNomura 41:45c982b1c5b6 124 break;
MasashiNomura 41:45c982b1c5b6 125 case GREEN:
MasashiNomura 41:45c982b1c5b6 126 DO_01 = 0;
MasashiNomura 42:cc8501b824ba 127 DO_02 = 1;
MasashiNomura 41:45c982b1c5b6 128 DO_03 = 0;
MasashiNomura 41:45c982b1c5b6 129 break;
MasashiNomura 41:45c982b1c5b6 130 case BLUE:
MasashiNomura 41:45c982b1c5b6 131 DO_01 = 0;
MasashiNomura 41:45c982b1c5b6 132 DO_02 = 0;
MasashiNomura 42:cc8501b824ba 133 DO_03 = 1;
MasashiNomura 42:cc8501b824ba 134 break;
MasashiNomura 42:cc8501b824ba 135 case YELLOW:
MasashiNomura 42:cc8501b824ba 136 DO_01 = 1;
MasashiNomura 42:cc8501b824ba 137 DO_02 = 1;
MasashiNomura 42:cc8501b824ba 138 DO_03 = 0;
MasashiNomura 42:cc8501b824ba 139 break;
MasashiNomura 42:cc8501b824ba 140 case PURPLE:
MasashiNomura 42:cc8501b824ba 141 DO_01 = 1;
MasashiNomura 42:cc8501b824ba 142 DO_02 = 0;
MasashiNomura 42:cc8501b824ba 143 DO_03 = 1;
MasashiNomura 42:cc8501b824ba 144 break;
MasashiNomura 42:cc8501b824ba 145 case LIGHT_BLUE:
MasashiNomura 42:cc8501b824ba 146 DO_01 = 0;
MasashiNomura 42:cc8501b824ba 147 DO_02 = 1;
MasashiNomura 42:cc8501b824ba 148 DO_03 = 1;
MasashiNomura 42:cc8501b824ba 149 break;
MasashiNomura 42:cc8501b824ba 150 case BLK_WHITE:
MasashiNomura 42:cc8501b824ba 151 DO_01 = !DO_01;
MasashiNomura 42:cc8501b824ba 152 DO_02 = !DO_02;
MasashiNomura 41:45c982b1c5b6 153 DO_03 = !DO_03;
MasashiNomura 41:45c982b1c5b6 154 break;
MasashiNomura 42:cc8501b824ba 155 case BLK_RED:
MasashiNomura 42:cc8501b824ba 156 DO_01 = !DO_01;
MasashiNomura 42:cc8501b824ba 157 DO_02 = 0;
MasashiNomura 42:cc8501b824ba 158 DO_03 = 0;
MasashiNomura 42:cc8501b824ba 159 break;
MasashiNomura 42:cc8501b824ba 160 case BLK_GREEN:
MasashiNomura 42:cc8501b824ba 161 DO_01 = 0;
MasashiNomura 42:cc8501b824ba 162 DO_02 = !DO_02;
MasashiNomura 42:cc8501b824ba 163 DO_03 = 0;
MasashiNomura 42:cc8501b824ba 164 break;
MasashiNomura 42:cc8501b824ba 165 case BLK_BLUE:
MasashiNomura 42:cc8501b824ba 166 DO_01 = 0;
MasashiNomura 42:cc8501b824ba 167 DO_02 = 0;
MasashiNomura 42:cc8501b824ba 168 DO_03 = !DO_03;
MasashiNomura 42:cc8501b824ba 169 break;
MasashiNomura 42:cc8501b824ba 170 case BLK_YELLOW:
MasashiNomura 41:45c982b1c5b6 171 DO_01 = !DO_01;
MasashiNomura 41:45c982b1c5b6 172 DO_02 = !DO_02;
MasashiNomura 41:45c982b1c5b6 173 DO_03 = 0;
MasashiNomura 41:45c982b1c5b6 174 break;
MasashiNomura 42:cc8501b824ba 175 case BLK_PURPLE:
MasashiNomura 41:45c982b1c5b6 176 DO_01 = !DO_01;
MasashiNomura 41:45c982b1c5b6 177 DO_02 = 0;
MasashiNomura 41:45c982b1c5b6 178 DO_03 = !DO_03;
MasashiNomura 41:45c982b1c5b6 179 break;
MasashiNomura 42:cc8501b824ba 180 case BLK_LIGHT_BLUE:
MasashiNomura 41:45c982b1c5b6 181 DO_01 = 0;
MasashiNomura 41:45c982b1c5b6 182 DO_02 = !DO_02;
MasashiNomura 41:45c982b1c5b6 183 DO_03 = !DO_03;
MasashiNomura 41:45c982b1c5b6 184 break;
MasashiNomura 41:45c982b1c5b6 185 case OFF:
MasashiNomura 41:45c982b1c5b6 186 default:
MasashiNomura 41:45c982b1c5b6 187 DO_01 = 0;//R
MasashiNomura 41:45c982b1c5b6 188 DO_02 = 0;//G
MasashiNomura 41:45c982b1c5b6 189 DO_03 = 0;//B
MasashiNomura 41:45c982b1c5b6 190 break;
MasashiNomura 41:45c982b1c5b6 191 }
MasashiNomura 41:45c982b1c5b6 192 }
MasashiNomura 41:45c982b1c5b6 193
MasashiNomura 41:45c982b1c5b6 194 void setDO4LED(enmHbState stat)
MasashiNomura 41:45c982b1c5b6 195 {
MasashiNomura 41:45c982b1c5b6 196 if(stat == NONE)
MasashiNomura 41:45c982b1c5b6 197 {// 消灯
MasashiNomura 41:45c982b1c5b6 198 setDOCol(OFF);
MasashiNomura 41:45c982b1c5b6 199 }
MasashiNomura 41:45c982b1c5b6 200 else if(stat == SLEEP)
MasashiNomura 41:45c982b1c5b6 201 {//YELLOW
MasashiNomura 41:45c982b1c5b6 202 setDOCol(YELLOW);
MasashiNomura 41:45c982b1c5b6 203 }
MasashiNomura 41:45c982b1c5b6 204 else if(stat == WAKEUP || stat == STANDBY)
MasashiNomura 41:45c982b1c5b6 205 {//BLUE
MasashiNomura 41:45c982b1c5b6 206 setDOCol(BLUE);
MasashiNomura 41:45c982b1c5b6 207 }
MasashiNomura 41:45c982b1c5b6 208 else if(stat == IDLE)
MasashiNomura 41:45c982b1c5b6 209 {//GREEN
MasashiNomura 41:45c982b1c5b6 210 setDOCol(GREEN);
MasashiNomura 41:45c982b1c5b6 211 }
MasashiNomura 48:71aec693a7dc 212 else if(stat == UPPER_IDLE)
MasashiNomura 48:71aec693a7dc 213 {//WHITE
MasashiNomura 48:71aec693a7dc 214 setDOCol(WHITE);
MasashiNomura 48:71aec693a7dc 215 }
MasashiNomura 41:45c982b1c5b6 216 else if(stat == TAKE_OFF || stat == HOVER || stat == DRIVE || stat == GROUND || stat == EMGGND)
MasashiNomura 41:45c982b1c5b6 217 {//PURPLE
MasashiNomura 43:156199c2f9b6 218 if(gf_BlinkLED){
MasashiNomura 43:156199c2f9b6 219 setDOCol(BLK_PURPLE);
MasashiNomura 43:156199c2f9b6 220 gf_BlinkLED = false;
MasashiNomura 43:156199c2f9b6 221 }
MasashiNomura 43:156199c2f9b6 222 else{
MasashiNomura 43:156199c2f9b6 223 setDOCol(PURPLE);
MasashiNomura 43:156199c2f9b6 224 }
MasashiNomura 41:45c982b1c5b6 225 }
MasashiNomura 42:cc8501b824ba 226 else if(stat == CHK_EG_ENT || stat == CHK_EG_MID || stat == CHK_EG_EXIT)
MasashiNomura 41:45c982b1c5b6 227 {//LIGHT BLUE
MasashiNomura 41:45c982b1c5b6 228 setDOCol(LIGHT_BLUE);
MasashiNomura 41:45c982b1c5b6 229 }
MasashiNomura 42:cc8501b824ba 230 else if(stat == CHK_EG_F || stat == CHK_EG_R )
MasashiNomura 42:cc8501b824ba 231 {//BLK_LIGHT BLUE
MasashiNomura 42:cc8501b824ba 232 setDOCol(BLK_LIGHT_BLUE);
MasashiNomura 42:cc8501b824ba 233 }
MasashiNomura 41:45c982b1c5b6 234 else if(stat == CHK_ENT || stat == CHK_MOT || stat == CHK_AXL || stat == CHK_ATT || stat == CHK_EXIT)
MasashiNomura 41:45c982b1c5b6 235 {//WHITE
MasashiNomura 41:45c982b1c5b6 236 setDOCol(WHITE);
MasashiNomura 41:45c982b1c5b6 237 }
MasashiNomura 41:45c982b1c5b6 238 else/* if(stat == MOT_STOP)*/
MasashiNomura 41:45c982b1c5b6 239 {
MasashiNomura 41:45c982b1c5b6 240 setDOCol(RED);
MasashiNomura 41:45c982b1c5b6 241 }
MasashiNomura 41:45c982b1c5b6 242 }
MasashiNomura 41:45c982b1c5b6 243
MasashiNomura 22:24c9c2dedca9 244 void setState(enmHbState stat){
MasashiNomura 22:24c9c2dedca9 245 // ありえない遷移を排除
MasashiNomura 22:24c9c2dedca9 246 if(gf_State == SLEEP){if(stat != WAKEUP) return;}
MasashiNomura 22:24c9c2dedca9 247 if(gf_State == WAKEUP){if(stat != STANDBY) return;}
MasashiNomura 22:24c9c2dedca9 248 if(gf_State == STANDBY){if(stat !=IDLE) return;}
MasashiNomura 23:79e20be4bc5b 249 if(gf_State == IDLE){if(stat != TAKE_OFF && stat != SLEEP) return;}
MasashiNomura 23:79e20be4bc5b 250 if(gf_State == TAKE_OFF){if(stat != HOVER && stat != GROUND) return;}
MasashiNomura 22:24c9c2dedca9 251 if(gf_State == GROUND){if(stat != IDLE) return;}
MasashiNomura 22:24c9c2dedca9 252 if(gf_State == HOVER){if(stat != DRIVE && stat != GROUND) return;}
MasashiNomura 22:24c9c2dedca9 253 if(gf_State == DRIVE){if(stat != HOVER && stat != EMGGND) return;}
MasashiNomura 22:24c9c2dedca9 254 if(gf_State == EMGGND){if(stat != IDLE) return;}
MasashiNomura 22:24c9c2dedca9 255
MasashiNomura 23:79e20be4bc5b 256 sp.printf("state: %d\r\n",stat);
MasashiNomura 22:24c9c2dedca9 257 gf_State = stat;
MasashiNomura 36:2cc739c7e4cb 258 gf_StateEnt = true;
MasashiNomura 22:24c9c2dedca9 259 }
MasashiNomura 23:79e20be4bc5b 260 void setStateF(enmHbState stat){
MasashiNomura 23:79e20be4bc5b 261 sp.printf("state: %d\r\n",stat);
MasashiNomura 23:79e20be4bc5b 262 enmHbState tmp;
MasashiNomura 25:f3a6e7eec9c3 263 if((UINT16)stat > MOT_STOP){
MasashiNomura 23:79e20be4bc5b 264 tmp = SLEEP;
MasashiNomura 23:79e20be4bc5b 265 }
MasashiNomura 23:79e20be4bc5b 266 else
MasashiNomura 23:79e20be4bc5b 267 {
MasashiNomura 23:79e20be4bc5b 268 tmp = stat;
MasashiNomura 23:79e20be4bc5b 269 }
MasashiNomura 23:79e20be4bc5b 270 gf_State = tmp;
MasashiNomura 36:2cc739c7e4cb 271 gf_StateEnt = true;
MasashiNomura 23:79e20be4bc5b 272 }
MasashiNomura 37:d51dacb4c30f 273
MasashiNomura 37:d51dacb4c30f 274 bool isActiveState(){
MasashiNomura 37:d51dacb4c30f 275 if(gf_State >= IDLE && gf_State <= EMGGND){
MasashiNomura 37:d51dacb4c30f 276 return true;
MasashiNomura 37:d51dacb4c30f 277 }
MasashiNomura 37:d51dacb4c30f 278 else {
MasashiNomura 37:d51dacb4c30f 279 return false;
MasashiNomura 37:d51dacb4c30f 280 }
MasashiNomura 37:d51dacb4c30f 281 }
MasashiNomura 37:d51dacb4c30f 282
MasashiNomura 33:eb260dbfc22a 283