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