teamALI / Mbed 2 deprecated HB2018

Dependencies:   mbed FreeRTOS

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?

UserRevisionLine numberNew 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