teamALI / Mbed 2 deprecated HB2018

Dependencies:   mbed FreeRTOS

Committer:
MasashiNomura
Date:
Wed Dec 19 12:22:22 2018 +0000
Revision:
31:56c554c560c1
Parent:
30:13ada1a24c59
Child:
32:7f4145cc3551
12/19 test modify PID etc

Who changed what in which revision?

UserRevisionLine numberNew contents of line
takeru0x1103 21:78302ecdb661 1 //RTOS関連------------------
takeru0x1103 1:15ab74f0d0f1 2 #include "FreeRTOS.h"
takeru0x1103 1:15ab74f0d0f1 3 #include "task.h"
takeru0x1103 21:78302ecdb661 4 //#include "queue.h"
takeru0x1103 21:78302ecdb661 5 //------------------RTOS関連
takeru0x1103 21:78302ecdb661 6
takeru0x1103 17:f9610f3cfa1b 7 #include "globalFlags.h"
takeru0x1103 17:f9610f3cfa1b 8 #include "HbManager.h"
takeru0x1103 18:5aa48aec9cae 9 #include "hbCommand.h"
takeru0x1103 18:5aa48aec9cae 10 #include "uart.h"
MasashiNomura 24:c5945aaae777 11 #include "fpga.h"
takeru0x1103 1:15ab74f0d0f1 12
takeru0x1103 18:5aa48aec9cae 13 //タスクハンドル(停止とか再開に必要)
takeru0x1103 18:5aa48aec9cae 14 static xTaskHandle tskHandle[3]={NULL,};
takeru0x1103 1:15ab74f0d0f1 15
takeru0x1103 21:78302ecdb661 16 //------------------
takeru0x1103 18:5aa48aec9cae 17 //タスク停止
takeru0x1103 18:5aa48aec9cae 18 //------------------
takeru0x1103 18:5aa48aec9cae 19 static void taskStop(int iId){
takeru0x1103 18:5aa48aec9cae 20 sp.printf("Task[%d] Stop\r\n" , iId);
takeru0x1103 18:5aa48aec9cae 21 vTaskSuspend(tskHandle[iId]);//タスクを止める
takeru0x1103 18:5aa48aec9cae 22 }
takeru0x1103 18:5aa48aec9cae 23
takeru0x1103 21:78302ecdb661 24 //------------------
takeru0x1103 18:5aa48aec9cae 25 //タスク再開
takeru0x1103 18:5aa48aec9cae 26 //------------------
takeru0x1103 18:5aa48aec9cae 27 static void taskStart(int iId){
takeru0x1103 18:5aa48aec9cae 28 sp.printf("Task[%d] Start!!\r\n" , iId);
takeru0x1103 18:5aa48aec9cae 29 vTaskResume(tskHandle[iId]);//タスク再開
takeru0x1103 18:5aa48aec9cae 30 }
takeru0x1103 18:5aa48aec9cae 31
takeru0x1103 18:5aa48aec9cae 32 //========================================================
takeru0x1103 21:78302ecdb661 33 //コマンド解析タスク
takeru0x1103 21:78302ecdb661 34 //========================================================
takeru0x1103 21:78302ecdb661 35 void taskCmdParser(void *pvParameters){
takeru0x1103 21:78302ecdb661 36 // int8_t *pcTaskName = (int8_t *) pvParameters;;
takeru0x1103 21:78302ecdb661 37 portTickType xLastWakeTime = xTaskGetTickCount();
takeru0x1103 21:78302ecdb661 38
takeru0x1103 21:78302ecdb661 39 // vTaskDelay(300);//制御タスクを先に走らせたいので待たす
takeru0x1103 21:78302ecdb661 40
takeru0x1103 21:78302ecdb661 41 while(1){
takeru0x1103 21:78302ecdb661 42 led2=!led2;
takeru0x1103 21:78302ecdb661 43 //コマンド解釈
takeru0x1103 21:78302ecdb661 44 commandParse();
takeru0x1103 21:78302ecdb661 45
takeru0x1103 21:78302ecdb661 46 //
MasashiNomura 24:c5945aaae777 47 // if(gf_Armed){
MasashiNomura 24:c5945aaae777 48 // setState(WAKEUP);
MasashiNomura 24:c5945aaae777 49 // gf_Armed = false;
MasashiNomura 24:c5945aaae777 50 // // taskStop(0);//制御タスクを止める
MasashiNomura 24:c5945aaae777 51 // // taskStart(2);//デバッグタスク再開
MasashiNomura 24:c5945aaae777 52 // }
MasashiNomura 22:24c9c2dedca9 53 // else{
MasashiNomura 22:24c9c2dedca9 54 // taskStop(2);//デバッグタスクを止める
MasashiNomura 22:24c9c2dedca9 55 // taskStart(0);//制御タスク再開
MasashiNomura 22:24c9c2dedca9 56 // }//switch
takeru0x1103 21:78302ecdb661 57
takeru0x1103 21:78302ecdb661 58 //次の周期まで待つ
takeru0x1103 21:78302ecdb661 59 vTaskDelayUntil(&xLastWakeTime, 50 / portTICK_RATE_MS );
takeru0x1103 21:78302ecdb661 60 }
takeru0x1103 21:78302ecdb661 61 }
takeru0x1103 21:78302ecdb661 62 //========================================================
takeru0x1103 17:f9610f3cfa1b 63 //ホバーバイク制御タスク
takeru0x1103 18:5aa48aec9cae 64 //========================================================
takeru0x1103 17:f9610f3cfa1b 65 void taskHbControl(void *pvParameters){
takeru0x1103 21:78302ecdb661 66 // int8_t *pcTaskName = (int8_t *) pvParameters;;
takeru0x1103 21:78302ecdb661 67 portTickType xLastWakeTime = xTaskGetTickCount();
takeru0x1103 21:78302ecdb661 68
takeru0x1103 17:f9610f3cfa1b 69 HbManager hb;
takeru0x1103 8:1ca49cb18290 70
MasashiNomura 23:79e20be4bc5b 71 bool bDoCtrlAtt;
MasashiNomura 23:79e20be4bc5b 72 bool bDoCtrlMot;
MasashiNomura 31:56c554c560c1 73 bool bDoCtrlEng;
MasashiNomura 31:56c554c560c1 74 //INT16 tmpRpm, difRpm;
takeru0x1103 18:5aa48aec9cae 75 //
takeru0x1103 1:15ab74f0d0f1 76 while(1){
takeru0x1103 16:05b9e44889f1 77 led1=!led1;
MasashiNomura 23:79e20be4bc5b 78 bDoCtrlAtt = false;
MasashiNomura 23:79e20be4bc5b 79 bDoCtrlMot = false;
MasashiNomura 31:56c554c560c1 80 bDoCtrlEng = false;
MasashiNomura 24:c5945aaae777 81 // PID係数アップデートチェック
MasashiNomura 24:c5945aaae777 82 if(gf_PidParaUpdate){
MasashiNomura 24:c5945aaae777 83 hb.setAttPara(g_PidPara);
MasashiNomura 24:c5945aaae777 84 sp.printf("Pp : [%f]\r\n",g_PidPara.PP);
MasashiNomura 24:c5945aaae777 85 sp.printf("P : [%f]\r\n",g_PidPara.P);
MasashiNomura 24:c5945aaae777 86 sp.printf("I : [%f]\r\n",g_PidPara.I);
MasashiNomura 24:c5945aaae777 87 sp.printf("D : [%f]\r\n",g_PidPara.D);
MasashiNomura 24:c5945aaae777 88 sp.printf("IMAX: [%f]\r\n",g_PidPara.IMax);
MasashiNomura 24:c5945aaae777 89 sp.printf("IMIN: [%f]\r\n",g_PidPara.IMin);
MasashiNomura 31:56c554c560c1 90 sp.printf("V : [%f]\r\n",g_PidPara.V);
MasashiNomura 24:c5945aaae777 91 gf_PidParaUpdate = false;
MasashiNomura 24:c5945aaae777 92 }
MasashiNomura 25:f3a6e7eec9c3 93 for(int i = 0; i < 4; ++i){
MasashiNomura 25:f3a6e7eec9c3 94 if(gf_MotParaUpdate[i]){
MasashiNomura 25:f3a6e7eec9c3 95 hb.setMotPara((UCHAR)i, g_MotPara[i]);
MasashiNomura 25:f3a6e7eec9c3 96 sp.printf("num : [%d]\r\n",i);
MasashiNomura 25:f3a6e7eec9c3 97 //sp.printf("ofs : [%d]\r\n",g_MotPara[i].offset);
MasashiNomura 25:f3a6e7eec9c3 98 sp.printf("low : [%d]\r\n",g_MotPara[i].limit_low);
MasashiNomura 25:f3a6e7eec9c3 99 sp.printf("hi : [%d]\r\n",g_MotPara[i].limit_hi);
MasashiNomura 25:f3a6e7eec9c3 100 gf_MotParaUpdate[i] = false;
MasashiNomura 25:f3a6e7eec9c3 101 }
MasashiNomura 25:f3a6e7eec9c3 102 }
MasashiNomura 25:f3a6e7eec9c3 103 if(gf_StopMot){
MasashiNomura 29:eb3d72dd94aa 104 //setStateF(MOT_STOP);
MasashiNomura 29:eb3d72dd94aa 105 //hb.getCurMotVal();
MasashiNomura 29:eb3d72dd94aa 106 for(int i = 0; i < 4; ++i){
MasashiNomura 29:eb3d72dd94aa 107 gf_MtReq[i].bf.req = true;
MasashiNomura 29:eb3d72dd94aa 108 gf_MtReq[i].bf.val = 0;
MasashiNomura 29:eb3d72dd94aa 109 gf_MtReqOfs[i].bf.req = true;
MasashiNomura 29:eb3d72dd94aa 110 gf_MtReqOfs[i].bf.val = 0;
MasashiNomura 29:eb3d72dd94aa 111 }
MasashiNomura 25:f3a6e7eec9c3 112 gf_StopMot = false;
MasashiNomura 25:f3a6e7eec9c3 113 }
MasashiNomura 23:79e20be4bc5b 114
MasashiNomura 29:eb3d72dd94aa 115
takeru0x1103 21:78302ecdb661 116 //▼①状態読み出し系
MasashiNomura 22:24c9c2dedca9 117 hb.getAttitude(); //現在角度読み出し
MasashiNomura 22:24c9c2dedca9 118 hb.controlEngine(); //エンジン回転数読み出し
MasashiNomura 22:24c9c2dedca9 119 hb.getUserCommand(); //操作ボタン状態読み出し
takeru0x1103 18:5aa48aec9cae 120
MasashiNomura 29:eb3d72dd94aa 121
takeru0x1103 21:78302ecdb661 122 //▼②ステート遷移
MasashiNomura 22:24c9c2dedca9 123 switch(gf_State){
MasashiNomura 22:24c9c2dedca9 124 case SLEEP:
MasashiNomura 22:24c9c2dedca9 125 if(gf_Dbg){
MasashiNomura 24:c5945aaae777 126 gf_Dbg = false;
MasashiNomura 22:24c9c2dedca9 127 taskStart(2);
MasashiNomura 22:24c9c2dedca9 128 }
MasashiNomura 22:24c9c2dedca9 129 break;
MasashiNomura 22:24c9c2dedca9 130 case WAKEUP:
MasashiNomura 24:c5945aaae777 131 //各種モーター
MasashiNomura 24:c5945aaae777 132 //機材のチェック
MasashiNomura 24:c5945aaae777 133 // if(hb.chkMotor() == true){
MasashiNomura 25:f3a6e7eec9c3 134 hb.calAtt();
MasashiNomura 23:79e20be4bc5b 135 setState(STANDBY);
MasashiNomura 24:c5945aaae777 136 // }
MasashiNomura 22:24c9c2dedca9 137 break;
MasashiNomura 22:24c9c2dedca9 138 case STANDBY:
MasashiNomura 25:f3a6e7eec9c3 139 // モーター回転数のチェック→モーターの回転数をオフセット値まで上げる
MasashiNomura 25:f3a6e7eec9c3 140 //nxtStatFlg = true;
MasashiNomura 25:f3a6e7eec9c3 141 //for(int i = 0; i < 4; ++i){
MasashiNomura 25:f3a6e7eec9c3 142 // if(gf_MtReq[i].bf.req){
MasashiNomura 25:f3a6e7eec9c3 143 // bDoCtrlMot = true;
MasashiNomura 25:f3a6e7eec9c3 144 // }
MasashiNomura 25:f3a6e7eec9c3 145 //}
MasashiNomura 25:f3a6e7eec9c3 146 if(!bDoCtrlMot){
MasashiNomura 25:f3a6e7eec9c3 147 if(hb.chkInRangeIDLE()){
MasashiNomura 25:f3a6e7eec9c3 148 setState(IDLE);
MasashiNomura 25:f3a6e7eec9c3 149 }
MasashiNomura 25:f3a6e7eec9c3 150 else{
MasashiNomura 25:f3a6e7eec9c3 151 // デバッグのためここもスルー
MasashiNomura 25:f3a6e7eec9c3 152 setState(IDLE);
MasashiNomura 25:f3a6e7eec9c3 153 }
MasashiNomura 23:79e20be4bc5b 154 }
MasashiNomura 22:24c9c2dedca9 155 break;
MasashiNomura 22:24c9c2dedca9 156 case IDLE:
MasashiNomura 23:79e20be4bc5b 157 //SWのチェック
MasashiNomura 31:56c554c560c1 158 hb.chkSW(IDLE);
MasashiNomura 31:56c554c560c1 159 bDoCtrlEng = true;
MasashiNomura 25:f3a6e7eec9c3 160 bDoCtrlMot = true;
MasashiNomura 31:56c554c560c1 161 bDoCtrlAtt = true;
MasashiNomura 22:24c9c2dedca9 162 break;
MasashiNomura 22:24c9c2dedca9 163 case TAKE_OFF:
MasashiNomura 23:79e20be4bc5b 164 // エンジン回転数のチェック
MasashiNomura 23:79e20be4bc5b 165 // 着陸ボタンが優先度が高いので先に判定、たが、まだ作ってないのでコメントアウト
MasashiNomura 23:79e20be4bc5b 166 // if(hb.chkSWUserOpe(HbUserOpe::STOP)){
MasashiNomura 23:79e20be4bc5b 167 // setState(GROUND);
MasashiNomura 23:79e20be4bc5b 168 // }
MasashiNomura 23:79e20be4bc5b 169 //else
MasashiNomura 31:56c554c560c1 170 bDoCtrlEng = true;
MasashiNomura 23:79e20be4bc5b 171 bDoCtrlMot = true;
MasashiNomura 23:79e20be4bc5b 172 bDoCtrlAtt = true;
MasashiNomura 23:79e20be4bc5b 173 //if(hb.chkSWUserOpe(HbUserOpe::STOP)){
MasashiNomura 23:79e20be4bc5b 174 // setState(HOVER);
MasashiNomura 23:79e20be4bc5b 175 //}
MasashiNomura 23:79e20be4bc5b 176
MasashiNomura 22:24c9c2dedca9 177 break;
MasashiNomura 22:24c9c2dedca9 178 case GROUND:
MasashiNomura 23:79e20be4bc5b 179 // そのままスルー
MasashiNomura 23:79e20be4bc5b 180 setState(IDLE);
MasashiNomura 22:24c9c2dedca9 181 break;
MasashiNomura 22:24c9c2dedca9 182 case HOVER:
MasashiNomura 23:79e20be4bc5b 183 //
MasashiNomura 22:24c9c2dedca9 184 break;
MasashiNomura 22:24c9c2dedca9 185 case DRIVE:
MasashiNomura 22:24c9c2dedca9 186 break;
MasashiNomura 22:24c9c2dedca9 187 case EMGGND:
MasashiNomura 22:24c9c2dedca9 188 break;
MasashiNomura 23:79e20be4bc5b 189 case CHK_ENT: //チェックエンター
MasashiNomura 23:79e20be4bc5b 190 sp.printf("Check enter\r\n");
MasashiNomura 23:79e20be4bc5b 191 break;
MasashiNomura 23:79e20be4bc5b 192 case CHK_MOT: //モーターチェック
MasashiNomura 23:79e20be4bc5b 193 sp.printf("Check Motor\r\n");
MasashiNomura 23:79e20be4bc5b 194 bDoCtrlMot = true;
MasashiNomura 23:79e20be4bc5b 195 break;
MasashiNomura 23:79e20be4bc5b 196 case CHK_AXL: //アクセルサーボチェック
MasashiNomura 23:79e20be4bc5b 197 sp.printf("Check Accel Servo\r\n");
MasashiNomura 23:79e20be4bc5b 198 break;
MasashiNomura 23:79e20be4bc5b 199 case CHK_ATT: //姿勢制御チェック
MasashiNomura 23:79e20be4bc5b 200 sp.printf("Check Attitude\r\n");
MasashiNomura 23:79e20be4bc5b 201 bDoCtrlAtt = true;
MasashiNomura 23:79e20be4bc5b 202 break;
MasashiNomura 23:79e20be4bc5b 203 case CHK_EXIT: //チェックステート脱出
MasashiNomura 23:79e20be4bc5b 204 sp.printf("Check Exit\r\n");
MasashiNomura 23:79e20be4bc5b 205 break;
MasashiNomura 25:f3a6e7eec9c3 206 case MOT_STOP:
MasashiNomura 25:f3a6e7eec9c3 207 if(hb.stopMotor() == true){
MasashiNomura 25:f3a6e7eec9c3 208 hb.initMotVal();
MasashiNomura 25:f3a6e7eec9c3 209 setState(SLEEP);
MasashiNomura 25:f3a6e7eec9c3 210 sp.printf("MOTOR STOPPED!\r\n");
MasashiNomura 25:f3a6e7eec9c3 211 }
MasashiNomura 25:f3a6e7eec9c3 212 else{
MasashiNomura 25:f3a6e7eec9c3 213 //bDoCtrlMot = true;
MasashiNomura 25:f3a6e7eec9c3 214 sp.printf(".");
MasashiNomura 25:f3a6e7eec9c3 215 }
MasashiNomura 25:f3a6e7eec9c3 216 break;
MasashiNomura 22:24c9c2dedca9 217 }
takeru0x1103 21:78302ecdb661 218
takeru0x1103 21:78302ecdb661 219 //▼③各種設定
takeru0x1103 21:78302ecdb661 220 //hb.controlAttitude(); //姿勢制御
takeru0x1103 21:78302ecdb661 221 //hb.controlMotor();//モーター指令出し
MasashiNomura 23:79e20be4bc5b 222 if(bDoCtrlAtt)hb.controlAttitude(); //姿勢制御
MasashiNomura 23:79e20be4bc5b 223 if(bDoCtrlMot)hb.controlMotor();//モーター指令出し
MasashiNomura 31:56c554c560c1 224 //if(bDoCtrlEng)hb.controlEngine();//エンジン指令出し
MasashiNomura 31:56c554c560c1 225
MasashiNomura 23:79e20be4bc5b 226 if(gf_Print.bf.stat){
MasashiNomura 23:79e20be4bc5b 227 sp.printf("stat : [%X]\r\n",gf_State);
MasashiNomura 23:79e20be4bc5b 228 }
takeru0x1103 18:5aa48aec9cae 229 //表示フラグを落とす(けどモニタフラグが立ってる箇所は残る)
takeru0x1103 18:5aa48aec9cae 230 if(gf_Print.flg!=0){
takeru0x1103 18:5aa48aec9cae 231 gf_Print.flg=gf_Mon.flg;
takeru0x1103 18:5aa48aec9cae 232 sp.printf("\r\n");
takeru0x1103 18:5aa48aec9cae 233 }
MasashiNomura 26:732bc37fbefd 234 if(gf_DbgPrint.flg != 0){
MasashiNomura 26:732bc37fbefd 235 gf_DbgPrint.flg = 0;
MasashiNomura 26:732bc37fbefd 236 sp.printf("\r\n");
MasashiNomura 26:732bc37fbefd 237 }
takeru0x1103 1:15ab74f0d0f1 238
takeru0x1103 1:15ab74f0d0f1 239 //次の周期まで待つ
takeru0x1103 1:15ab74f0d0f1 240 vTaskDelayUntil(&xLastWakeTime, 20 / portTICK_RATE_MS );
takeru0x1103 1:15ab74f0d0f1 241 }
takeru0x1103 1:15ab74f0d0f1 242 }
takeru0x1103 18:5aa48aec9cae 243 //========================================================
takeru0x1103 21:78302ecdb661 244 //タスク2:デバッグ用タスク
takeru0x1103 18:5aa48aec9cae 245 //========================================================
takeru0x1103 18:5aa48aec9cae 246 void taskDebug(void *pvParameters){
takeru0x1103 21:78302ecdb661 247
takeru0x1103 21:78302ecdb661 248 taskStop(2);//自ら止めてレジュームされるまで待つ
MasashiNomura 24:c5945aaae777 249 UCHAR Num;
MasashiNomura 24:c5945aaae777 250 INT16 iVal;
MasashiNomura 25:f3a6e7eec9c3 251 //bool rvFlg;
MasashiNomura 24:c5945aaae777 252
takeru0x1103 21:78302ecdb661 253 //この関数をリターンしてしまうと他のタスクも止まるのでwhileで回すこと!
takeru0x1103 18:5aa48aec9cae 254 while(1){
MasashiNomura 24:c5945aaae777 255 for(Num = 0; Num < 8; ++Num){
MasashiNomura 24:c5945aaae777 256 for(iVal = 50; iVal < 300; ++iVal){
MasashiNomura 24:c5945aaae777 257 fpgaMotor(Num, iVal);
MasashiNomura 25:f3a6e7eec9c3 258 wait(0.002);
MasashiNomura 24:c5945aaae777 259 }
MasashiNomura 24:c5945aaae777 260 for(iVal = 300; iVal >= 50; --iVal){
MasashiNomura 24:c5945aaae777 261 fpgaMotor(Num, iVal);
MasashiNomura 25:f3a6e7eec9c3 262 wait(0.002);
MasashiNomura 24:c5945aaae777 263 }
MasashiNomura 24:c5945aaae777 264 }
MasashiNomura 23:79e20be4bc5b 265 //自タスク停止させて次の周期まで待つ
MasashiNomura 22:24c9c2dedca9 266 gf_Dbg = false;
MasashiNomura 23:79e20be4bc5b 267 sp.printf("Do DebugTask!");
takeru0x1103 18:5aa48aec9cae 268 taskStop(2);
takeru0x1103 18:5aa48aec9cae 269 }
takeru0x1103 18:5aa48aec9cae 270 }
takeru0x1103 17:f9610f3cfa1b 271
takeru0x1103 1:15ab74f0d0f1 272 //-------------------------------------------------------------
takeru0x1103 17:f9610f3cfa1b 273 //初期化:タスク登録
takeru0x1103 1:15ab74f0d0f1 274 //-------------------------------------------------------------
takeru0x1103 1:15ab74f0d0f1 275 void taskInit(){
takeru0x1103 21:78302ecdb661 276 portBASE_TYPE TaskRtn;//タスククリエイトの結果を受け取る型
takeru0x1103 18:5aa48aec9cae 277
takeru0x1103 21:78302ecdb661 278 //タスク0:コマンド解析タスク
MasashiNomura 26:732bc37fbefd 279 TaskRtn= xTaskCreate(taskCmdParser, (signed portCHAR *)"TaskCmd", 512, NULL, 1, &tskHandle[0]);
takeru0x1103 21:78302ecdb661 280 if(TaskRtn==pdTRUE){sp.printf("Task0 Set : Command parser\r\n");}
takeru0x1103 21:78302ecdb661 281
takeru0x1103 21:78302ecdb661 282 //タスク1:制御タスク
MasashiNomura 28:fdb3b144e342 283 TaskRtn= xTaskCreate(taskHbControl, (signed portCHAR *)"TaskHbCont", 1024, NULL, 2, &tskHandle[1]);
takeru0x1103 21:78302ecdb661 284 if(TaskRtn==pdTRUE){sp.printf("Task1 Set : Hoverbike Control\r\n");}
takeru0x1103 21:78302ecdb661 285
takeru0x1103 21:78302ecdb661 286 //タスク2:デバッグタスク
MasashiNomura 23:79e20be4bc5b 287 TaskRtn= xTaskCreate(taskDebug, (signed portCHAR *)"TaskDebug", 192, NULL, 0, &tskHandle[2]);
takeru0x1103 21:78302ecdb661 288 if(TaskRtn==pdTRUE){sp.printf("Task2 Set : Debug\r\n");}
takeru0x1103 8:1ca49cb18290 289
takeru0x1103 21:78302ecdb661 290 //RTOSカーネル起動(タスクが走り出す)
takeru0x1103 1:15ab74f0d0f1 291 vTaskStartScheduler();
takeru0x1103 1:15ab74f0d0f1 292 }
takeru0x1103 1:15ab74f0d0f1 293
takeru0x1103 1:15ab74f0d0f1 294