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

userTask.cpp@24:c5945aaae777, 2018-12-10 (annotated)

Committer:
MasashiNomura
Date:
Mon Dec 10 12:29:37 2018 +0000
Revision:
24:c5945aaae777
Parent:
23:79e20be4bc5b
Child:
25:f3a6e7eec9c3
2018/12/10  dbg 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;
takeru0x1103 18:5aa48aec9cae 73 //
takeru0x1103 1:15ab74f0d0f1 74 while(1){
takeru0x1103 16:05b9e44889f1 75 led1=!led1;
MasashiNomura 23:79e20be4bc5b 76 bDoCtrlAtt = false;
MasashiNomura 23:79e20be4bc5b 77 bDoCtrlMot = false;
MasashiNomura 24:c5945aaae777 78
MasashiNomura 24:c5945aaae777 79 // PID係数アップデートチェック
MasashiNomura 24:c5945aaae777 80 if(gf_PidParaUpdate){
MasashiNomura 24:c5945aaae777 81 hb.setAttPara(g_PidPara);
MasashiNomura 24:c5945aaae777 82 sp.printf("Pp : [%f]\r\n",g_PidPara.PP);
MasashiNomura 24:c5945aaae777 83 sp.printf("P : [%f]\r\n",g_PidPara.P);
MasashiNomura 24:c5945aaae777 84 sp.printf("I : [%f]\r\n",g_PidPara.I);
MasashiNomura 24:c5945aaae777 85 sp.printf("D : [%f]\r\n",g_PidPara.D);
MasashiNomura 24:c5945aaae777 86 sp.printf("IMAX: [%f]\r\n",g_PidPara.IMax);
MasashiNomura 24:c5945aaae777 87 sp.printf("IMIN: [%f]\r\n",g_PidPara.IMin);
MasashiNomura 24:c5945aaae777 88 gf_PidParaUpdate = false;
MasashiNomura 24:c5945aaae777 89 }
MasashiNomura 23:79e20be4bc5b 90
takeru0x1103 21:78302ecdb661 91 //▼①状態読み出し系
MasashiNomura 22:24c9c2dedca9 92 hb.getAttitude(); //現在角度読み出し
MasashiNomura 22:24c9c2dedca9 93 hb.controlEngine(); //エンジン回転数読み出し
MasashiNomura 22:24c9c2dedca9 94 hb.getUserCommand(); //操作ボタン状態読み出し
takeru0x1103 18:5aa48aec9cae 95
takeru0x1103 21:78302ecdb661 96 //▼②ステート遷移
MasashiNomura 22:24c9c2dedca9 97 switch(gf_State){
MasashiNomura 22:24c9c2dedca9 98 case SLEEP:
MasashiNomura 22:24c9c2dedca9 99 if(gf_Dbg){
MasashiNomura 24:c5945aaae777 100 gf_Dbg = false;
MasashiNomura 22:24c9c2dedca9 101 taskStart(2);
MasashiNomura 22:24c9c2dedca9 102 }
MasashiNomura 23:79e20be4bc5b 103 else if(gf_Armed){
MasashiNomura 24:c5945aaae777 104 gf_Armed = false;
MasashiNomura 24:c5945aaae777 105 // hb.initChkMotor();
MasashiNomura 23:79e20be4bc5b 106 setState(WAKEUP);
MasashiNomura 23:79e20be4bc5b 107 }
MasashiNomura 22:24c9c2dedca9 108 break;
MasashiNomura 22:24c9c2dedca9 109 case WAKEUP:
MasashiNomura 24:c5945aaae777 110 //各種モーター
MasashiNomura 24:c5945aaae777 111 //機材のチェック
MasashiNomura 24:c5945aaae777 112 // if(hb.chkMotor() == true){
MasashiNomura 23:79e20be4bc5b 113 setState(STANDBY);
MasashiNomura 24:c5945aaae777 114 // }
MasashiNomura 22:24c9c2dedca9 115 break;
MasashiNomura 22:24c9c2dedca9 116 case STANDBY:
MasashiNomura 23:79e20be4bc5b 117 // モーター回転数のチェック
MasashiNomura 23:79e20be4bc5b 118 if(hb.chkInRangeIDLE()){
MasashiNomura 23:79e20be4bc5b 119 setState(IDLE);
MasashiNomura 23:79e20be4bc5b 120 }
MasashiNomura 23:79e20be4bc5b 121 else{
MasashiNomura 23:79e20be4bc5b 122 // デバッグのためここもスルー
MasashiNomura 23:79e20be4bc5b 123 setState(IDLE);
MasashiNomura 23:79e20be4bc5b 124 }
MasashiNomura 22:24c9c2dedca9 125 break;
MasashiNomura 22:24c9c2dedca9 126 case IDLE:
MasashiNomura 23:79e20be4bc5b 127 //SWのチェック
MasashiNomura 23:79e20be4bc5b 128 //離陸ボタン押下
MasashiNomura 23:79e20be4bc5b 129 //とりあえず、YawCtrlで代用
MasashiNomura 23:79e20be4bc5b 130 if(hb.chkSWUserOpe(HbUserOpe::YAW_CTRL))
MasashiNomura 23:79e20be4bc5b 131 {
MasashiNomura 23:79e20be4bc5b 132 // TODO IMU Cal
MasashiNomura 23:79e20be4bc5b 133 setState(TAKE_OFF);
MasashiNomura 23:79e20be4bc5b 134 }
MasashiNomura 23:79e20be4bc5b 135 //else if(hb.chkSWUserOpe(HbUserOpe::ENG_STOP))
MasashiNomura 23:79e20be4bc5b 136 //{
MasashiNomura 23:79e20be4bc5b 137 // setState(SLEEP);
MasashiNomura 23:79e20be4bc5b 138 //}
MasashiNomura 23:79e20be4bc5b 139 //if(hb.chkSWUserOpeAny()){
MasashiNomura 23:79e20be4bc5b 140 // sp.printf("SW :[%X]\r\n",hb.getUserSw());
MasashiNomura 23:79e20be4bc5b 141 //}
MasashiNomura 22:24c9c2dedca9 142 break;
MasashiNomura 22:24c9c2dedca9 143 case TAKE_OFF:
MasashiNomura 23:79e20be4bc5b 144 // エンジン回転数のチェック
MasashiNomura 23:79e20be4bc5b 145 // 着陸ボタンが優先度が高いので先に判定、たが、まだ作ってないのでコメントアウト
MasashiNomura 23:79e20be4bc5b 146 // if(hb.chkSWUserOpe(HbUserOpe::STOP)){
MasashiNomura 23:79e20be4bc5b 147 // setState(GROUND);
MasashiNomura 23:79e20be4bc5b 148 // }
MasashiNomura 23:79e20be4bc5b 149 //else
MasashiNomura 23:79e20be4bc5b 150 bDoCtrlMot = true;
MasashiNomura 23:79e20be4bc5b 151 bDoCtrlAtt = true;
MasashiNomura 23:79e20be4bc5b 152 //if(hb.chkSWUserOpe(HbUserOpe::STOP)){
MasashiNomura 23:79e20be4bc5b 153 // setState(HOVER);
MasashiNomura 23:79e20be4bc5b 154 //}
MasashiNomura 23:79e20be4bc5b 155
MasashiNomura 22:24c9c2dedca9 156 break;
MasashiNomura 22:24c9c2dedca9 157 case GROUND:
MasashiNomura 23:79e20be4bc5b 158 // そのままスルー
MasashiNomura 23:79e20be4bc5b 159 setState(IDLE);
MasashiNomura 22:24c9c2dedca9 160 break;
MasashiNomura 22:24c9c2dedca9 161 case HOVER:
MasashiNomura 23:79e20be4bc5b 162 //
MasashiNomura 22:24c9c2dedca9 163 break;
MasashiNomura 22:24c9c2dedca9 164 case DRIVE:
MasashiNomura 22:24c9c2dedca9 165 break;
MasashiNomura 22:24c9c2dedca9 166 case EMGGND:
MasashiNomura 22:24c9c2dedca9 167 break;
MasashiNomura 23:79e20be4bc5b 168 case CHK_ENT: //チェックエンター
MasashiNomura 23:79e20be4bc5b 169 sp.printf("Check enter\r\n");
MasashiNomura 23:79e20be4bc5b 170 break;
MasashiNomura 23:79e20be4bc5b 171 case CHK_MOT: //モーターチェック
MasashiNomura 23:79e20be4bc5b 172 sp.printf("Check Motor\r\n");
MasashiNomura 23:79e20be4bc5b 173 bDoCtrlMot = true;
MasashiNomura 23:79e20be4bc5b 174 break;
MasashiNomura 23:79e20be4bc5b 175 case CHK_AXL: //アクセルサーボチェック
MasashiNomura 23:79e20be4bc5b 176 sp.printf("Check Accel Servo\r\n");
MasashiNomura 23:79e20be4bc5b 177 break;
MasashiNomura 23:79e20be4bc5b 178 case CHK_ATT: //姿勢制御チェック
MasashiNomura 23:79e20be4bc5b 179 sp.printf("Check Attitude\r\n");
MasashiNomura 23:79e20be4bc5b 180 bDoCtrlAtt = true;
MasashiNomura 23:79e20be4bc5b 181 break;
MasashiNomura 23:79e20be4bc5b 182 case CHK_EXIT: //チェックステート脱出
MasashiNomura 23:79e20be4bc5b 183 sp.printf("Check Exit\r\n");
MasashiNomura 23:79e20be4bc5b 184 break;
MasashiNomura 22:24c9c2dedca9 185 }
takeru0x1103 21:78302ecdb661 186
takeru0x1103 21:78302ecdb661 187 //▼③各種設定
takeru0x1103 21:78302ecdb661 188 //hb.controlAttitude(); //姿勢制御
takeru0x1103 21:78302ecdb661 189 //hb.controlMotor();//モーター指令出し
MasashiNomura 23:79e20be4bc5b 190 if(bDoCtrlAtt)hb.controlAttitude(); //姿勢制御
MasashiNomura 23:79e20be4bc5b 191 if(bDoCtrlMot)hb.controlMotor();//モーター指令出し
takeru0x1103 18:5aa48aec9cae 192
MasashiNomura 23:79e20be4bc5b 193 if(gf_Print.bf.stat){
MasashiNomura 23:79e20be4bc5b 194 sp.printf("stat : [%X]\r\n",gf_State);
MasashiNomura 23:79e20be4bc5b 195 }
takeru0x1103 18:5aa48aec9cae 196 //表示フラグを落とす（けどモニタフラグが立ってる箇所は残る）
takeru0x1103 18:5aa48aec9cae 197 if(gf_Print.flg!=0){
takeru0x1103 18:5aa48aec9cae 198 gf_Print.flg=gf_Mon.flg;
takeru0x1103 18:5aa48aec9cae 199 sp.printf("\r\n");
takeru0x1103 18:5aa48aec9cae 200 }
takeru0x1103 1:15ab74f0d0f1 201
takeru0x1103 1:15ab74f0d0f1 202 //次の周期まで待つ
takeru0x1103 1:15ab74f0d0f1 203 vTaskDelayUntil(&xLastWakeTime, 20 / portTICK_RATE_MS );
takeru0x1103 1:15ab74f0d0f1 204 }
takeru0x1103 1:15ab74f0d0f1 205 }
takeru0x1103 18:5aa48aec9cae 206 //========================================================
takeru0x1103 21:78302ecdb661 207 //タスク２：デバッグ用タスク
takeru0x1103 18:5aa48aec9cae 208 //========================================================
takeru0x1103 18:5aa48aec9cae 209 void taskDebug(void *pvParameters){
takeru0x1103 21:78302ecdb661 210
takeru0x1103 21:78302ecdb661 211 taskStop(2);//自ら止めてレジュームされるまで待つ
MasashiNomura 24:c5945aaae777 212 UCHAR Num;
MasashiNomura 24:c5945aaae777 213 INT16 iVal;
MasashiNomura 24:c5945aaae777 214 bool rvFlg;
MasashiNomura 24:c5945aaae777 215
takeru0x1103 21:78302ecdb661 216 //この関数をリターンしてしまうと他のタスクも止まるのでwhileで回すこと！
takeru0x1103 18:5aa48aec9cae 217 while(1){
MasashiNomura 24:c5945aaae777 218 for(Num = 0; Num < 8; ++Num){
MasashiNomura 24:c5945aaae777 219 for(iVal = 50; iVal < 300; ++iVal){
MasashiNomura 24:c5945aaae777 220 fpgaMotor(Num, iVal);
MasashiNomura 24:c5945aaae777 221 wait(0.01);
MasashiNomura 24:c5945aaae777 222 }
MasashiNomura 24:c5945aaae777 223 for(iVal = 300; iVal >= 50; --iVal){
MasashiNomura 24:c5945aaae777 224 fpgaMotor(Num, iVal);
MasashiNomura 24:c5945aaae777 225 wait(0.01);
MasashiNomura 24:c5945aaae777 226 }
MasashiNomura 24:c5945aaae777 227 }
MasashiNomura 23:79e20be4bc5b 228 //自タスク停止させて次の周期まで待つ
MasashiNomura 22:24c9c2dedca9 229 gf_Dbg = false;
MasashiNomura 23:79e20be4bc5b 230 sp.printf("Do DebugTask!");
takeru0x1103 18:5aa48aec9cae 231 taskStop(2);
takeru0x1103 18:5aa48aec9cae 232 }
takeru0x1103 18:5aa48aec9cae 233 }
takeru0x1103 17:f9610f3cfa1b 234
takeru0x1103 1:15ab74f0d0f1 235 //-------------------------------------------------------------
takeru0x1103 17:f9610f3cfa1b 236 //初期化：タスク登録
takeru0x1103 1:15ab74f0d0f1 237 //-------------------------------------------------------------
takeru0x1103 1:15ab74f0d0f1 238 void taskInit(){
takeru0x1103 21:78302ecdb661 239 portBASE_TYPE TaskRtn;//タスククリエイトの結果を受け取る型
takeru0x1103 18:5aa48aec9cae 240
takeru0x1103 21:78302ecdb661 241 //タスク０：コマンド解析タスク
MasashiNomura 23:79e20be4bc5b 242 TaskRtn= xTaskCreate(taskCmdParser, (signed portCHAR *)"TaskCmd", 256, NULL, 1, &tskHandle[0]);
takeru0x1103 21:78302ecdb661 243 if(TaskRtn==pdTRUE){sp.printf("Task0 Set : Command parser\r\n");}
takeru0x1103 21:78302ecdb661 244
takeru0x1103 21:78302ecdb661 245 //タスク１：制御タスク
MasashiNomura 24:c5945aaae777 246 TaskRtn= xTaskCreate(taskHbControl, (signed portCHAR *)"TaskHbCont", 256, NULL, 2, &tskHandle[1]);
takeru0x1103 21:78302ecdb661 247 if(TaskRtn==pdTRUE){sp.printf("Task1 Set : Hoverbike Control\r\n");}
takeru0x1103 21:78302ecdb661 248
takeru0x1103 21:78302ecdb661 249 //タスク２：デバッグタスク
MasashiNomura 23:79e20be4bc5b 250 TaskRtn= xTaskCreate(taskDebug, (signed portCHAR *)"TaskDebug", 192, NULL, 0, &tskHandle[2]);
takeru0x1103 21:78302ecdb661 251 if(TaskRtn==pdTRUE){sp.printf("Task2 Set : Debug\r\n");}
takeru0x1103 8:1ca49cb18290 252
takeru0x1103 21:78302ecdb661 253 //RTOSカーネル起動（タスクが走り出す）
takeru0x1103 1:15ab74f0d0f1 254 vTaskStartScheduler();
takeru0x1103 1:15ab74f0d0f1 255 }
takeru0x1103 1:15ab74f0d0f1 256
takeru0x1103 1:15ab74f0d0f1 257