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@23:79e20be4bc5b, 2018-12-08 (annotated)

Committer:
MasashiNomura
Date:
Sat Dec 08 12:08:25 2018 +0000
Revision:
23:79e20be4bc5b
Parent:
22:24c9c2dedca9
Child:
24:c5945aaae777
2018/12/08 Add State Trans 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"
takeru0x1103 1:15ab74f0d0f1 11
takeru0x1103 18:5aa48aec9cae 12 //タスクハンドル（停止とか再開に必要）
takeru0x1103 18:5aa48aec9cae 13 static xTaskHandle tskHandle[3]={NULL,};
takeru0x1103 1:15ab74f0d0f1 14
takeru0x1103 21:78302ecdb661 15 //------------------
takeru0x1103 18:5aa48aec9cae 16 //タスク停止
takeru0x1103 18:5aa48aec9cae 17 //------------------
takeru0x1103 18:5aa48aec9cae 18 static void taskStop(int iId){
takeru0x1103 18:5aa48aec9cae 19 sp.printf("Task[%d] Stop\r\n" , iId);
takeru0x1103 18:5aa48aec9cae 20 vTaskSuspend(tskHandle[iId]);//タスクを止める
takeru0x1103 18:5aa48aec9cae 21 }
takeru0x1103 18:5aa48aec9cae 22
takeru0x1103 21:78302ecdb661 23 //------------------
takeru0x1103 18:5aa48aec9cae 24 //タスク再開
takeru0x1103 18:5aa48aec9cae 25 //------------------
takeru0x1103 18:5aa48aec9cae 26 static void taskStart(int iId){
takeru0x1103 18:5aa48aec9cae 27 sp.printf("Task[%d] Start!!\r\n" , iId);
takeru0x1103 18:5aa48aec9cae 28 vTaskResume(tskHandle[iId]);//タスク再開
takeru0x1103 18:5aa48aec9cae 29 }
takeru0x1103 18:5aa48aec9cae 30
takeru0x1103 18:5aa48aec9cae 31 //========================================================
takeru0x1103 21:78302ecdb661 32 //コマンド解析タスク
takeru0x1103 21:78302ecdb661 33 //========================================================
takeru0x1103 21:78302ecdb661 34 void taskCmdParser(void *pvParameters){
takeru0x1103 21:78302ecdb661 35 // int8_t *pcTaskName = (int8_t *) pvParameters;;
takeru0x1103 21:78302ecdb661 36 portTickType xLastWakeTime = xTaskGetTickCount();
takeru0x1103 21:78302ecdb661 37
takeru0x1103 21:78302ecdb661 38 // vTaskDelay(300);//制御タスクを先に走らせたいので待たす
takeru0x1103 21:78302ecdb661 39
takeru0x1103 21:78302ecdb661 40 while(1){
takeru0x1103 21:78302ecdb661 41 led2=!led2;
takeru0x1103 21:78302ecdb661 42 //コマンド解釈
takeru0x1103 21:78302ecdb661 43 commandParse();
takeru0x1103 21:78302ecdb661 44
takeru0x1103 21:78302ecdb661 45 //
takeru0x1103 21:78302ecdb661 46 if(gf_Armed){
MasashiNomura 22:24c9c2dedca9 47 setState(WAKEUP);
MasashiNomura 22:24c9c2dedca9 48 gf_Armed = false;
MasashiNomura 22:24c9c2dedca9 49 // taskStop(0);//制御タスクを止める
MasashiNomura 22:24c9c2dedca9 50 // taskStart(2);//デバッグタスク再開
MasashiNomura 22:24c9c2dedca9 51 }
MasashiNomura 22:24c9c2dedca9 52 // else{
MasashiNomura 22:24c9c2dedca9 53 // taskStop(2);//デバッグタスクを止める
MasashiNomura 22:24c9c2dedca9 54 // taskStart(0);//制御タスク再開
MasashiNomura 22:24c9c2dedca9 55 // }//switch
takeru0x1103 21:78302ecdb661 56
takeru0x1103 21:78302ecdb661 57 //次の周期まで待つ
takeru0x1103 21:78302ecdb661 58 vTaskDelayUntil(&xLastWakeTime, 50 / portTICK_RATE_MS );
takeru0x1103 21:78302ecdb661 59 }
takeru0x1103 21:78302ecdb661 60 }
takeru0x1103 21:78302ecdb661 61 //========================================================
takeru0x1103 17:f9610f3cfa1b 62 //ホバーバイク制御タスク
takeru0x1103 18:5aa48aec9cae 63 //========================================================
takeru0x1103 17:f9610f3cfa1b 64 void taskHbControl(void *pvParameters){
takeru0x1103 21:78302ecdb661 65 // int8_t *pcTaskName = (int8_t *) pvParameters;;
takeru0x1103 21:78302ecdb661 66 portTickType xLastWakeTime = xTaskGetTickCount();
takeru0x1103 21:78302ecdb661 67
takeru0x1103 17:f9610f3cfa1b 68 HbManager hb;
takeru0x1103 8:1ca49cb18290 69
MasashiNomura 23:79e20be4bc5b 70 bool bDoCtrlAtt;
MasashiNomura 23:79e20be4bc5b 71 bool bDoCtrlMot;
takeru0x1103 18:5aa48aec9cae 72 //
takeru0x1103 1:15ab74f0d0f1 73 while(1){
takeru0x1103 16:05b9e44889f1 74 led1=!led1;
MasashiNomura 23:79e20be4bc5b 75 bDoCtrlAtt = false;
MasashiNomura 23:79e20be4bc5b 76 bDoCtrlMot = false;
MasashiNomura 23:79e20be4bc5b 77
takeru0x1103 21:78302ecdb661 78 //▼①状態読み出し系
MasashiNomura 22:24c9c2dedca9 79 hb.getAttitude(); //現在角度読み出し
MasashiNomura 22:24c9c2dedca9 80 hb.controlEngine(); //エンジン回転数読み出し
MasashiNomura 22:24c9c2dedca9 81 hb.getUserCommand(); //操作ボタン状態読み出し
takeru0x1103 18:5aa48aec9cae 82
takeru0x1103 21:78302ecdb661 83 //▼②ステート遷移
MasashiNomura 22:24c9c2dedca9 84 switch(gf_State){
MasashiNomura 22:24c9c2dedca9 85 case SLEEP:
MasashiNomura 22:24c9c2dedca9 86 if(gf_Dbg){
MasashiNomura 22:24c9c2dedca9 87 taskStart(2);
MasashiNomura 22:24c9c2dedca9 88 }
MasashiNomura 23:79e20be4bc5b 89 else if(gf_Armed){
MasashiNomura 23:79e20be4bc5b 90 setState(WAKEUP);
MasashiNomura 23:79e20be4bc5b 91 gf_Armed = false;
MasashiNomura 23:79e20be4bc5b 92 }
MasashiNomura 22:24c9c2dedca9 93 break;
MasashiNomura 22:24c9c2dedca9 94 case WAKEUP:
MasashiNomura 23:79e20be4bc5b 95 { //各種モーター
MasashiNomura 23:79e20be4bc5b 96 //機材のチェック
MasashiNomura 23:79e20be4bc5b 97 //今はそのまま次へ
MasashiNomura 23:79e20be4bc5b 98 setState(STANDBY);
MasashiNomura 23:79e20be4bc5b 99 }
MasashiNomura 22:24c9c2dedca9 100 break;
MasashiNomura 22:24c9c2dedca9 101 case STANDBY:
MasashiNomura 23:79e20be4bc5b 102 // モーター回転数のチェック
MasashiNomura 23:79e20be4bc5b 103 if(hb.chkInRangeIDLE()){
MasashiNomura 23:79e20be4bc5b 104 setState(IDLE);
MasashiNomura 23:79e20be4bc5b 105 }
MasashiNomura 23:79e20be4bc5b 106 else{
MasashiNomura 23:79e20be4bc5b 107 // デバッグのためここもスルー
MasashiNomura 23:79e20be4bc5b 108 setState(IDLE);
MasashiNomura 23:79e20be4bc5b 109 }
MasashiNomura 22:24c9c2dedca9 110 break;
MasashiNomura 22:24c9c2dedca9 111 case IDLE:
MasashiNomura 23:79e20be4bc5b 112 //SWのチェック
MasashiNomura 23:79e20be4bc5b 113 //離陸ボタン押下
MasashiNomura 23:79e20be4bc5b 114 //とりあえず、YawCtrlで代用
MasashiNomura 23:79e20be4bc5b 115 if(hb.chkSWUserOpe(HbUserOpe::YAW_CTRL))
MasashiNomura 23:79e20be4bc5b 116 {
MasashiNomura 23:79e20be4bc5b 117 // TODO IMU Cal
MasashiNomura 23:79e20be4bc5b 118 setState(TAKE_OFF);
MasashiNomura 23:79e20be4bc5b 119 }
MasashiNomura 23:79e20be4bc5b 120 //else if(hb.chkSWUserOpe(HbUserOpe::ENG_STOP))
MasashiNomura 23:79e20be4bc5b 121 //{
MasashiNomura 23:79e20be4bc5b 122 // setState(SLEEP);
MasashiNomura 23:79e20be4bc5b 123 //}
MasashiNomura 23:79e20be4bc5b 124 //if(hb.chkSWUserOpeAny()){
MasashiNomura 23:79e20be4bc5b 125 // sp.printf("SW :[%X]\r\n",hb.getUserSw());
MasashiNomura 23:79e20be4bc5b 126 //}
MasashiNomura 22:24c9c2dedca9 127 break;
MasashiNomura 22:24c9c2dedca9 128 case TAKE_OFF:
MasashiNomura 23:79e20be4bc5b 129 // エンジン回転数のチェック
MasashiNomura 23:79e20be4bc5b 130 // 着陸ボタンが優先度が高いので先に判定、たが、まだ作ってないのでコメントアウト
MasashiNomura 23:79e20be4bc5b 131 // if(hb.chkSWUserOpe(HbUserOpe::STOP)){
MasashiNomura 23:79e20be4bc5b 132 // setState(GROUND);
MasashiNomura 23:79e20be4bc5b 133 // }
MasashiNomura 23:79e20be4bc5b 134 //else
MasashiNomura 23:79e20be4bc5b 135 bDoCtrlMot = true;
MasashiNomura 23:79e20be4bc5b 136 bDoCtrlAtt = true;
MasashiNomura 23:79e20be4bc5b 137 //if(hb.chkSWUserOpe(HbUserOpe::STOP)){
MasashiNomura 23:79e20be4bc5b 138 // setState(HOVER);
MasashiNomura 23:79e20be4bc5b 139 //}
MasashiNomura 23:79e20be4bc5b 140
MasashiNomura 22:24c9c2dedca9 141 break;
MasashiNomura 22:24c9c2dedca9 142 case GROUND:
MasashiNomura 23:79e20be4bc5b 143 // そのままスルー
MasashiNomura 23:79e20be4bc5b 144 setState(IDLE);
MasashiNomura 22:24c9c2dedca9 145 break;
MasashiNomura 22:24c9c2dedca9 146 case HOVER:
MasashiNomura 23:79e20be4bc5b 147 //
MasashiNomura 22:24c9c2dedca9 148 break;
MasashiNomura 22:24c9c2dedca9 149 case DRIVE:
MasashiNomura 22:24c9c2dedca9 150 break;
MasashiNomura 22:24c9c2dedca9 151 case EMGGND:
MasashiNomura 22:24c9c2dedca9 152 break;
MasashiNomura 23:79e20be4bc5b 153 case CHK_ENT: //チェックエンター
MasashiNomura 23:79e20be4bc5b 154 sp.printf("Check enter\r\n");
MasashiNomura 23:79e20be4bc5b 155 break;
MasashiNomura 23:79e20be4bc5b 156 case CHK_MOT: //モーターチェック
MasashiNomura 23:79e20be4bc5b 157 sp.printf("Check Motor\r\n");
MasashiNomura 23:79e20be4bc5b 158 bDoCtrlMot = true;
MasashiNomura 23:79e20be4bc5b 159 break;
MasashiNomura 23:79e20be4bc5b 160 case CHK_AXL: //アクセルサーボチェック
MasashiNomura 23:79e20be4bc5b 161 sp.printf("Check Accel Servo\r\n");
MasashiNomura 23:79e20be4bc5b 162 break;
MasashiNomura 23:79e20be4bc5b 163 case CHK_ATT: //姿勢制御チェック
MasashiNomura 23:79e20be4bc5b 164 sp.printf("Check Attitude\r\n");
MasashiNomura 23:79e20be4bc5b 165 bDoCtrlAtt = true;
MasashiNomura 23:79e20be4bc5b 166 break;
MasashiNomura 23:79e20be4bc5b 167 case CHK_EXIT: //チェックステート脱出
MasashiNomura 23:79e20be4bc5b 168 sp.printf("Check Exit\r\n");
MasashiNomura 23:79e20be4bc5b 169 break;
MasashiNomura 22:24c9c2dedca9 170 }
takeru0x1103 21:78302ecdb661 171
takeru0x1103 21:78302ecdb661 172 //▼③各種設定
takeru0x1103 21:78302ecdb661 173 //hb.controlAttitude(); //姿勢制御
takeru0x1103 21:78302ecdb661 174 //hb.controlMotor();//モーター指令出し
MasashiNomura 23:79e20be4bc5b 175 if(bDoCtrlAtt)hb.controlAttitude(); //姿勢制御
MasashiNomura 23:79e20be4bc5b 176 if(bDoCtrlMot)hb.controlMotor();//モーター指令出し
takeru0x1103 18:5aa48aec9cae 177
MasashiNomura 23:79e20be4bc5b 178 if(gf_Print.bf.stat){
MasashiNomura 23:79e20be4bc5b 179 sp.printf("stat : [%X]\r\n",gf_State);
MasashiNomura 23:79e20be4bc5b 180 }
takeru0x1103 18:5aa48aec9cae 181 //表示フラグを落とす（けどモニタフラグが立ってる箇所は残る）
takeru0x1103 18:5aa48aec9cae 182 if(gf_Print.flg!=0){
takeru0x1103 18:5aa48aec9cae 183 gf_Print.flg=gf_Mon.flg;
takeru0x1103 18:5aa48aec9cae 184 sp.printf("\r\n");
takeru0x1103 18:5aa48aec9cae 185 }
takeru0x1103 1:15ab74f0d0f1 186
takeru0x1103 1:15ab74f0d0f1 187 //次の周期まで待つ
takeru0x1103 1:15ab74f0d0f1 188 vTaskDelayUntil(&xLastWakeTime, 20 / portTICK_RATE_MS );
takeru0x1103 1:15ab74f0d0f1 189 }
takeru0x1103 1:15ab74f0d0f1 190 }
takeru0x1103 18:5aa48aec9cae 191 //========================================================
takeru0x1103 21:78302ecdb661 192 //タスク２：デバッグ用タスク
takeru0x1103 18:5aa48aec9cae 193 //========================================================
takeru0x1103 18:5aa48aec9cae 194 void taskDebug(void *pvParameters){
takeru0x1103 21:78302ecdb661 195
takeru0x1103 21:78302ecdb661 196 taskStop(2);//自ら止めてレジュームされるまで待つ
takeru0x1103 18:5aa48aec9cae 197
takeru0x1103 21:78302ecdb661 198 //この関数をリターンしてしまうと他のタスクも止まるのでwhileで回すこと！
takeru0x1103 18:5aa48aec9cae 199 while(1){
takeru0x1103 21:78302ecdb661 200
MasashiNomura 22:24c9c2dedca9 201
MasashiNomura 22:24c9c2dedca9 202
MasashiNomura 23:79e20be4bc5b 203 //自タスク停止させて次の周期まで待つ
MasashiNomura 22:24c9c2dedca9 204 gf_Dbg = false;
MasashiNomura 23:79e20be4bc5b 205 sp.printf("Do DebugTask!");
takeru0x1103 18:5aa48aec9cae 206 taskStop(2);
takeru0x1103 18:5aa48aec9cae 207 }
takeru0x1103 18:5aa48aec9cae 208 }
takeru0x1103 17:f9610f3cfa1b 209
takeru0x1103 1:15ab74f0d0f1 210 //-------------------------------------------------------------
takeru0x1103 17:f9610f3cfa1b 211 //初期化：タスク登録
takeru0x1103 1:15ab74f0d0f1 212 //-------------------------------------------------------------
takeru0x1103 1:15ab74f0d0f1 213 void taskInit(){
takeru0x1103 21:78302ecdb661 214 portBASE_TYPE TaskRtn;//タスククリエイトの結果を受け取る型
takeru0x1103 18:5aa48aec9cae 215
takeru0x1103 21:78302ecdb661 216 //タスク０：コマンド解析タスク
MasashiNomura 23:79e20be4bc5b 217 TaskRtn= xTaskCreate(taskCmdParser, (signed portCHAR *)"TaskCmd", 256, NULL, 1, &tskHandle[0]);
takeru0x1103 21:78302ecdb661 218 if(TaskRtn==pdTRUE){sp.printf("Task0 Set : Command parser\r\n");}
takeru0x1103 21:78302ecdb661 219
takeru0x1103 21:78302ecdb661 220 //タスク１：制御タスク
takeru0x1103 21:78302ecdb661 221 TaskRtn= xTaskCreate(taskHbControl, (signed portCHAR *)"TaskHbCont", 192, NULL, 2, &tskHandle[1]);
takeru0x1103 21:78302ecdb661 222 if(TaskRtn==pdTRUE){sp.printf("Task1 Set : Hoverbike Control\r\n");}
takeru0x1103 21:78302ecdb661 223
takeru0x1103 21:78302ecdb661 224 //タスク２：デバッグタスク
MasashiNomura 23:79e20be4bc5b 225 TaskRtn= xTaskCreate(taskDebug, (signed portCHAR *)"TaskDebug", 192, NULL, 0, &tskHandle[2]);
takeru0x1103 21:78302ecdb661 226 if(TaskRtn==pdTRUE){sp.printf("Task2 Set : Debug\r\n");}
takeru0x1103 8:1ca49cb18290 227
takeru0x1103 21:78302ecdb661 228 //RTOSカーネル起動（タスクが走り出す）
takeru0x1103 1:15ab74f0d0f1 229 vTaskStartScheduler();
takeru0x1103 1:15ab74f0d0f1 230 }
takeru0x1103 1:15ab74f0d0f1 231
takeru0x1103 1:15ab74f0d0f1 232