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@36:2cc739c7e4cb, 2019-01-16 (annotated)

Committer:
MasashiNomura
Date:
Wed Jan 16 10:51:07 2019 +0000
Revision:
36:2cc739c7e4cb
Parent:
35:3779201b4c73
Child:
37:d51dacb4c30f
2019/01/16 modify for 45inc Alfa HB / no debug, no checking

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;
MasashiNomura 36:2cc739c7e4cb 75 INT16 cntSW1 = 0;
MasashiNomura 36:2cc739c7e4cb 76 INT16 cntSW2 = 0;
MasashiNomura 33:eb260dbfc22a 77 INT16 AxlRpm;
MasashiNomura 36:2cc739c7e4cb 78 INT16 TrtlVal;
takeru0x1103 1:15ab74f0d0f1 79 while(1){
takeru0x1103 16:05b9e44889f1 80 led1=!led1;
MasashiNomura 23:79e20be4bc5b 81 bDoCtrlAtt = false;
MasashiNomura 23:79e20be4bc5b 82 bDoCtrlMot = false;
MasashiNomura 31:56c554c560c1 83 bDoCtrlEng = false;
MasashiNomura 24:c5945aaae777 84 // PID係数アップデートチェック
MasashiNomura 24:c5945aaae777 85 if(gf_PidParaUpdate){
MasashiNomura 24:c5945aaae777 86 hb.setAttPara(g_PidPara);
MasashiNomura 24:c5945aaae777 87 sp.printf("Pp : [%f]\r\n",g_PidPara.PP);
MasashiNomura 24:c5945aaae777 88 sp.printf("P : [%f]\r\n",g_PidPara.P);
MasashiNomura 24:c5945aaae777 89 sp.printf("I : [%f]\r\n",g_PidPara.I);
MasashiNomura 24:c5945aaae777 90 sp.printf("D : [%f]\r\n",g_PidPara.D);
MasashiNomura 24:c5945aaae777 91 sp.printf("IMAX: [%f]\r\n",g_PidPara.IMax);
MasashiNomura 24:c5945aaae777 92 sp.printf("IMIN: [%f]\r\n",g_PidPara.IMin);
MasashiNomura 31:56c554c560c1 93 sp.printf("V : [%f]\r\n",g_PidPara.V);
MasashiNomura 24:c5945aaae777 94 gf_PidParaUpdate = false;
MasashiNomura 24:c5945aaae777 95 }
MasashiNomura 36:2cc739c7e4cb 96 // for(int i = 0; i < 4; ++i){
MasashiNomura 36:2cc739c7e4cb 97 // if(gf_MotParaUpdate[i]){
MasashiNomura 36:2cc739c7e4cb 98 // hb.setMotPara((UCHAR)i, g_MotPara[i]);
MasashiNomura 36:2cc739c7e4cb 99 // sp.printf("num : [%d]\r\n",i);
MasashiNomura 36:2cc739c7e4cb 100 // //sp.printf("ofs : [%d]\r\n",g_MotPara[i].offset);
MasashiNomura 36:2cc739c7e4cb 101 // sp.printf("low : [%d]\r\n",g_MotPara[i].limit_low);
MasashiNomura 36:2cc739c7e4cb 102 // sp.printf("hi : [%d]\r\n",g_MotPara[i].limit_hi);
MasashiNomura 36:2cc739c7e4cb 103 // gf_MotParaUpdate[i] = false;
MasashiNomura 36:2cc739c7e4cb 104 // }
MasashiNomura 36:2cc739c7e4cb 105 // }
MasashiNomura 25:f3a6e7eec9c3 106 if(gf_StopMot){
MasashiNomura 29:eb3d72dd94aa 107 //setStateF(MOT_STOP);
MasashiNomura 29:eb3d72dd94aa 108 //hb.getCurMotVal();
MasashiNomura 29:eb3d72dd94aa 109 for(int i = 0; i < 4; ++i){
MasashiNomura 32:7f4145cc3551 110 gf_MtReq[i].req = true;
MasashiNomura 32:7f4145cc3551 111 gf_MtReq[i].val = 0;
MasashiNomura 32:7f4145cc3551 112 gf_MtReqOfs[i].req = true;
MasashiNomura 32:7f4145cc3551 113 gf_MtReqOfs[i].val = 0;
MasashiNomura 29:eb3d72dd94aa 114 }
MasashiNomura 25:f3a6e7eec9c3 115 gf_StopMot = false;
MasashiNomura 25:f3a6e7eec9c3 116 }
MasashiNomura 23:79e20be4bc5b 117
MasashiNomura 29:eb3d72dd94aa 118
takeru0x1103 21:78302ecdb661 119 //▼①状態読み出し系
MasashiNomura 22:24c9c2dedca9 120 hb.getAttitude(); //現在角度読み出し
MasashiNomura 22:24c9c2dedca9 121 hb.controlEngine(); //エンジン回転数読み出し
MasashiNomura 22:24c9c2dedca9 122 hb.getUserCommand(); //操作ボタン状態読み出し
MasashiNomura 29:eb3d72dd94aa 123
takeru0x1103 21:78302ecdb661 124 //▼②ステート遷移
MasashiNomura 22:24c9c2dedca9 125 switch(gf_State){
MasashiNomura 22:24c9c2dedca9 126 case SLEEP:
MasashiNomura 36:2cc739c7e4cb 127 if(gf_StateEnt){
MasashiNomura 36:2cc739c7e4cb 128 sp.printf("SLEEP state\r\n");
MasashiNomura 36:2cc739c7e4cb 129 gf_StateEnt = false;
MasashiNomura 36:2cc739c7e4cb 130 }
MasashiNomura 36:2cc739c7e4cb 131 // DEBUGタスクスタートフラグの監視
MasashiNomura 22:24c9c2dedca9 132 if(gf_Dbg){
MasashiNomura 24:c5945aaae777 133 gf_Dbg = false;
MasashiNomura 22:24c9c2dedca9 134 taskStart(2);
MasashiNomura 22:24c9c2dedca9 135 }
MasashiNomura 36:2cc739c7e4cb 136 // 浮上（離陸）ボタン、着陸ボタンの同時長押し監視
MasashiNomura 36:2cc739c7e4cb 137 if(hb.chkSWUserOpeBoth(HbUserOpe::FLT_ON,HbUserOpe::FLT_OFF)){
MasashiNomura 36:2cc739c7e4cb 138 ++cntSW1;
MasashiNomura 36:2cc739c7e4cb 139 if(cntSW1 > 30){
MasashiNomura 36:2cc739c7e4cb 140 cntSW1 = 0;
MasashiNomura 36:2cc739c7e4cb 141 setStateF(CHK_EG_ENT);
MasashiNomura 36:2cc739c7e4cb 142 }
MasashiNomura 36:2cc739c7e4cb 143 }
MasashiNomura 36:2cc739c7e4cb 144 else {
MasashiNomura 36:2cc739c7e4cb 145 cntSW1 = 0;
MasashiNomura 36:2cc739c7e4cb 146 }
MasashiNomura 36:2cc739c7e4cb 147 if(hb.chkSWUserOpeBoth(HbUserOpe::BRK_L, HbUserOpe::BRK_R)){
MasashiNomura 36:2cc739c7e4cb 148 ++cntSW2;
MasashiNomura 36:2cc739c7e4cb 149 if(cntSW2 > 30){
MasashiNomura 36:2cc739c7e4cb 150 cntSW2 = 0;
MasashiNomura 36:2cc739c7e4cb 151 setStateF(WAKEUP);
MasashiNomura 36:2cc739c7e4cb 152 }
MasashiNomura 36:2cc739c7e4cb 153 }
MasashiNomura 36:2cc739c7e4cb 154 else {
MasashiNomura 36:2cc739c7e4cb 155 cntSW2 = 0;
MasashiNomura 36:2cc739c7e4cb 156 }
MasashiNomura 22:24c9c2dedca9 157 break;
MasashiNomura 22:24c9c2dedca9 158 case WAKEUP:
MasashiNomura 36:2cc739c7e4cb 159 if(gf_StateEnt){
MasashiNomura 36:2cc739c7e4cb 160 //スイッチが何も押されていないことの確認
MasashiNomura 36:2cc739c7e4cb 161 if(!hb.chkSWUserOpeAny()){
MasashiNomura 36:2cc739c7e4cb 162 sp.printf("WAKEUP state\r\n");
MasashiNomura 36:2cc739c7e4cb 163 gf_StateEnt = false;
MasashiNomura 36:2cc739c7e4cb 164 }
MasashiNomura 36:2cc739c7e4cb 165 }
MasashiNomura 36:2cc739c7e4cb 166 else
MasashiNomura 36:2cc739c7e4cb 167 {
MasashiNomura 36:2cc739c7e4cb 168 //各種モーター
MasashiNomura 36:2cc739c7e4cb 169 //機材のチェック
MasashiNomura 36:2cc739c7e4cb 170 hb.calAtt();//現在値でヨー角校正
MasashiNomura 36:2cc739c7e4cb 171 //モーター用のアナログ入力が下げられているかの確認
MasashiNomura 36:2cc739c7e4cb 172 AxlRpm = hb.getUserMotAxl();
MasashiNomura 36:2cc739c7e4cb 173 if(AxlRpm > 0){
MasashiNomura 36:2cc739c7e4cb 174 // 将来的に警告音等
MasashiNomura 36:2cc739c7e4cb 175 sp.printf("Warning!! Motor Accel Opened!!\r\n");
MasashiNomura 36:2cc739c7e4cb 176 setStateF(SLEEP);
MasashiNomura 36:2cc739c7e4cb 177 } else {
MasashiNomura 36:2cc739c7e4cb 178 setState(STANDBY);
MasashiNomura 36:2cc739c7e4cb 179 }
MasashiNomura 35:3779201b4c73 180 }
MasashiNomura 22:24c9c2dedca9 181 break;
MasashiNomura 22:24c9c2dedca9 182 case STANDBY:
MasashiNomura 36:2cc739c7e4cb 183 bDoCtrlMot = true;
MasashiNomura 36:2cc739c7e4cb 184 if(gf_StateEnt){
MasashiNomura 36:2cc739c7e4cb 185 // モーター回転数のチェック→モーターの回転数をオフセット値まで上げる
MasashiNomura 36:2cc739c7e4cb 186 // TODO
MasashiNomura 36:2cc739c7e4cb 187 sp.printf("STANDBY state\r\n");
MasashiNomura 36:2cc739c7e4cb 188 gf_StateEnt = false;
MasashiNomura 36:2cc739c7e4cb 189 }
MasashiNomura 36:2cc739c7e4cb 190 else {
MasashiNomura 36:2cc739c7e4cb 191
MasashiNomura 25:f3a6e7eec9c3 192 if(hb.chkInRangeIDLE()){
MasashiNomura 36:2cc739c7e4cb 193 hb.calAtt();//現在値でヨー角校正
MasashiNomura 25:f3a6e7eec9c3 194 setState(IDLE);
MasashiNomura 25:f3a6e7eec9c3 195 }
MasashiNomura 25:f3a6e7eec9c3 196 else{
MasashiNomura 25:f3a6e7eec9c3 197 // デバッグのためここもスルー
MasashiNomura 36:2cc739c7e4cb 198 hb.calAtt();//現在値でヨー角校正
MasashiNomura 25:f3a6e7eec9c3 199 setState(IDLE);
MasashiNomura 25:f3a6e7eec9c3 200 }
MasashiNomura 23:79e20be4bc5b 201 }
MasashiNomura 22:24c9c2dedca9 202 break;
MasashiNomura 22:24c9c2dedca9 203 case IDLE:
MasashiNomura 36:2cc739c7e4cb 204 if(gf_StateEnt){
MasashiNomura 36:2cc739c7e4cb 205 sp.printf("IDLE state\r\n");
MasashiNomura 36:2cc739c7e4cb 206 gf_StateEnt = false;
MasashiNomura 36:2cc739c7e4cb 207 }
MasashiNomura 23:79e20be4bc5b 208 //SWのチェック
MasashiNomura 31:56c554c560c1 209 hb.chkSW(IDLE);
MasashiNomura 33:eb260dbfc22a 210 AxlRpm = hb.getUserMotAxl();
MasashiNomura 33:eb260dbfc22a 211 hb.setMotVal(R_L,AxlRpm);
MasashiNomura 33:eb260dbfc22a 212 hb.setMotVal(R_R,AxlRpm);
MasashiNomura 31:56c554c560c1 213 bDoCtrlEng = true;
MasashiNomura 25:f3a6e7eec9c3 214 bDoCtrlMot = true;
MasashiNomura 32:7f4145cc3551 215 //bDoCtrlAtt = true;
MasashiNomura 22:24c9c2dedca9 216 break;
MasashiNomura 22:24c9c2dedca9 217 case TAKE_OFF:
MasashiNomura 36:2cc739c7e4cb 218 if(gf_StateEnt){
MasashiNomura 36:2cc739c7e4cb 219 sp.printf("TAKE_OFF state\r\n");
MasashiNomura 36:2cc739c7e4cb 220 gf_StateEnt = false;
MasashiNomura 36:2cc739c7e4cb 221 }
MasashiNomura 23:79e20be4bc5b 222 // エンジン回転数のチェック
MasashiNomura 23:79e20be4bc5b 223 // 着陸ボタンが優先度が高いので先に判定、たが、まだ作ってないのでコメントアウト
MasashiNomura 23:79e20be4bc5b 224 // if(hb.chkSWUserOpe(HbUserOpe::STOP)){
MasashiNomura 23:79e20be4bc5b 225 // setState(GROUND);
MasashiNomura 23:79e20be4bc5b 226 // }
MasashiNomura 23:79e20be4bc5b 227 //else
MasashiNomura 34:234b87f3e6ce 228 hb.chkSW(TAKE_OFF);
MasashiNomura 34:234b87f3e6ce 229 AxlRpm = hb.getUserMotAxl();
MasashiNomura 34:234b87f3e6ce 230 hb.setMotVal(R_L,AxlRpm);
MasashiNomura 34:234b87f3e6ce 231 hb.setMotVal(R_R,AxlRpm);
MasashiNomura 31:56c554c560c1 232 bDoCtrlEng = true;
MasashiNomura 23:79e20be4bc5b 233 bDoCtrlMot = true;
MasashiNomura 23:79e20be4bc5b 234 bDoCtrlAtt = true;
MasashiNomura 22:24c9c2dedca9 235 break;
MasashiNomura 22:24c9c2dedca9 236 case GROUND:
MasashiNomura 36:2cc739c7e4cb 237 if(gf_StateEnt){
MasashiNomura 36:2cc739c7e4cb 238 sp.printf("GROUND state\r\n");
MasashiNomura 36:2cc739c7e4cb 239 gf_StateEnt = false;
MasashiNomura 36:2cc739c7e4cb 240 }
MasashiNomura 23:79e20be4bc5b 241 // そのままスルー
MasashiNomura 36:2cc739c7e4cb 242 // TODO エンジン回転数が一定以下になることを確認する
MasashiNomura 23:79e20be4bc5b 243 setState(IDLE);
MasashiNomura 22:24c9c2dedca9 244 break;
MasashiNomura 22:24c9c2dedca9 245 case HOVER:
MasashiNomura 36:2cc739c7e4cb 246 gf_StateEnt = false;
MasashiNomura 23:79e20be4bc5b 247 //
MasashiNomura 22:24c9c2dedca9 248 break;
MasashiNomura 22:24c9c2dedca9 249 case DRIVE:
MasashiNomura 36:2cc739c7e4cb 250 gf_StateEnt = false;
MasashiNomura 22:24c9c2dedca9 251 break;
MasashiNomura 22:24c9c2dedca9 252 case EMGGND:
MasashiNomura 36:2cc739c7e4cb 253 gf_StateEnt = false;
MasashiNomura 36:2cc739c7e4cb 254 break;
MasashiNomura 36:2cc739c7e4cb 255 case CHK_EG_ENT:
MasashiNomura 36:2cc739c7e4cb 256 if(gf_StateEnt){
MasashiNomura 36:2cc739c7e4cb 257 sp.printf("Check Engine enter\r\n");
MasashiNomura 36:2cc739c7e4cb 258 gf_StateEnt = false;
MasashiNomura 36:2cc739c7e4cb 259 }
MasashiNomura 36:2cc739c7e4cb 260 if(!hb.chkSWUserOpeAny()){
MasashiNomura 36:2cc739c7e4cb 261 AxlRpm = hb.getUserMotAxl();
MasashiNomura 36:2cc739c7e4cb 262 TrtlVal = hb.getUserEngTrottle();
MasashiNomura 36:2cc739c7e4cb 263 if(AxlRpm > 0 || TrtlVal > 40){
MasashiNomura 36:2cc739c7e4cb 264 break;
MasashiNomura 36:2cc739c7e4cb 265 } else {
MasashiNomura 36:2cc739c7e4cb 266 setState(CHK_EG_F);
MasashiNomura 36:2cc739c7e4cb 267 }
MasashiNomura 36:2cc739c7e4cb 268 }
MasashiNomura 36:2cc739c7e4cb 269 break;
MasashiNomura 36:2cc739c7e4cb 270 case CHK_EG_F:
MasashiNomura 36:2cc739c7e4cb 271 if(gf_StateEnt){
MasashiNomura 36:2cc739c7e4cb 272 sp.printf("Check Engine Front\r\n");
MasashiNomura 36:2cc739c7e4cb 273 gf_StateEnt = false;
MasashiNomura 36:2cc739c7e4cb 274 }
MasashiNomura 36:2cc739c7e4cb 275 TrtlVal = hb.getUserEngTrottle();
MasashiNomura 36:2cc739c7e4cb 276 hb.setAccelVal(FRONT, TrtlVal);
MasashiNomura 36:2cc739c7e4cb 277 if(hb.chkSWUserOpe(HbUserOpe::F_ENG_UP)){
MasashiNomura 36:2cc739c7e4cb 278 hb.setHvAxl(FRONT, TrtlVal);
MasashiNomura 36:2cc739c7e4cb 279 }
MasashiNomura 36:2cc739c7e4cb 280 if(hb.chkSWUserOpe(HbUserOpe::R_ENG_UP)){
MasashiNomura 36:2cc739c7e4cb 281 if(hb.chkSetHvAxl(FRONT)){
MasashiNomura 36:2cc739c7e4cb 282 setState(CHK_EG_MID);
MasashiNomura 36:2cc739c7e4cb 283 }
MasashiNomura 36:2cc739c7e4cb 284 }
MasashiNomura 36:2cc739c7e4cb 285 break;
MasashiNomura 36:2cc739c7e4cb 286 case CHK_EG_MID:
MasashiNomura 36:2cc739c7e4cb 287 if(gf_StateEnt){
MasashiNomura 36:2cc739c7e4cb 288 sp.printf("Check Engine MIDDLE\r\n");
MasashiNomura 36:2cc739c7e4cb 289 gf_StateEnt = false;
MasashiNomura 36:2cc739c7e4cb 290 }
MasashiNomura 36:2cc739c7e4cb 291 if(!hb.chkSWUserOpeAny()){
MasashiNomura 36:2cc739c7e4cb 292 AxlRpm = hb.getUserMotAxl();
MasashiNomura 36:2cc739c7e4cb 293 TrtlVal = hb.getUserEngTrottle();
MasashiNomura 36:2cc739c7e4cb 294 if(AxlRpm > 0 || TrtlVal > 40){
MasashiNomura 36:2cc739c7e4cb 295 break;
MasashiNomura 36:2cc739c7e4cb 296 } else {
MasashiNomura 36:2cc739c7e4cb 297 setState(CHK_EG_R);
MasashiNomura 36:2cc739c7e4cb 298 }
MasashiNomura 36:2cc739c7e4cb 299 }
MasashiNomura 36:2cc739c7e4cb 300 break;
MasashiNomura 36:2cc739c7e4cb 301 case CHK_EG_R:
MasashiNomura 36:2cc739c7e4cb 302 if(gf_StateEnt){
MasashiNomura 36:2cc739c7e4cb 303 sp.printf("Check Engine Rear\r\n");
MasashiNomura 36:2cc739c7e4cb 304 gf_StateEnt = false;
MasashiNomura 36:2cc739c7e4cb 305 }
MasashiNomura 36:2cc739c7e4cb 306 TrtlVal = hb.getUserEngTrottle();
MasashiNomura 36:2cc739c7e4cb 307 hb.setAccelVal(REAR, TrtlVal);
MasashiNomura 36:2cc739c7e4cb 308 if(hb.chkSWUserOpe(HbUserOpe::F_ENG_UP)){
MasashiNomura 36:2cc739c7e4cb 309 hb.setHvAxl(REAR, TrtlVal);
MasashiNomura 36:2cc739c7e4cb 310 }
MasashiNomura 36:2cc739c7e4cb 311 if(hb.chkSWUserOpe(HbUserOpe::R_ENG_UP)){
MasashiNomura 36:2cc739c7e4cb 312 if(hb.chkSetHvAxl(REAR)){
MasashiNomura 36:2cc739c7e4cb 313 setState(CHK_EG_EXIT);
MasashiNomura 36:2cc739c7e4cb 314 }
MasashiNomura 36:2cc739c7e4cb 315 }
MasashiNomura 36:2cc739c7e4cb 316 break;
MasashiNomura 36:2cc739c7e4cb 317 case CHK_EG_EXIT:
MasashiNomura 36:2cc739c7e4cb 318 if(gf_StateEnt){
MasashiNomura 36:2cc739c7e4cb 319 sp.printf("Check Engine Exit\r\n");
MasashiNomura 36:2cc739c7e4cb 320 gf_StateEnt = false;
MasashiNomura 36:2cc739c7e4cb 321 }
MasashiNomura 36:2cc739c7e4cb 322 if(!hb.chkSWUserOpeAny()){
MasashiNomura 36:2cc739c7e4cb 323 AxlRpm = hb.getUserMotAxl();
MasashiNomura 36:2cc739c7e4cb 324 TrtlVal = hb.getUserEngTrottle();
MasashiNomura 36:2cc739c7e4cb 325 if(AxlRpm > 0 || TrtlVal > 40){
MasashiNomura 36:2cc739c7e4cb 326 break;
MasashiNomura 36:2cc739c7e4cb 327 } else {
MasashiNomura 36:2cc739c7e4cb 328 setState(SLEEP);
MasashiNomura 36:2cc739c7e4cb 329 }
MasashiNomura 36:2cc739c7e4cb 330 }
MasashiNomura 22:24c9c2dedca9 331 break;
MasashiNomura 23:79e20be4bc5b 332 case CHK_ENT: //チェックエンター
MasashiNomura 36:2cc739c7e4cb 333 if(gf_StateEnt){
MasashiNomura 36:2cc739c7e4cb 334 sp.printf("Check enter\r\n");
MasashiNomura 36:2cc739c7e4cb 335 gf_StateEnt = false;
MasashiNomura 36:2cc739c7e4cb 336 }
MasashiNomura 23:79e20be4bc5b 337 break;
MasashiNomura 23:79e20be4bc5b 338 case CHK_MOT: //モーターチェック
MasashiNomura 36:2cc739c7e4cb 339 if(gf_StateEnt){
MasashiNomura 36:2cc739c7e4cb 340 sp.printf("Check Motor\r\n");
MasashiNomura 36:2cc739c7e4cb 341 gf_StateEnt = false;
MasashiNomura 36:2cc739c7e4cb 342 }
MasashiNomura 23:79e20be4bc5b 343 bDoCtrlMot = true;
MasashiNomura 23:79e20be4bc5b 344 break;
MasashiNomura 23:79e20be4bc5b 345 case CHK_AXL: //アクセルサーボチェック
MasashiNomura 36:2cc739c7e4cb 346 if(gf_StateEnt){
MasashiNomura 36:2cc739c7e4cb 347 sp.printf("Check Accel Servo\r\n");
MasashiNomura 36:2cc739c7e4cb 348 gf_StateEnt = false;
MasashiNomura 36:2cc739c7e4cb 349 }
MasashiNomura 23:79e20be4bc5b 350 break;
MasashiNomura 23:79e20be4bc5b 351 case CHK_ATT: //姿勢制御チェック
MasashiNomura 36:2cc739c7e4cb 352 if(gf_StateEnt){
MasashiNomura 36:2cc739c7e4cb 353 sp.printf("Check Attitude\r\n");
MasashiNomura 36:2cc739c7e4cb 354 gf_StateEnt = false;
MasashiNomura 36:2cc739c7e4cb 355 }
MasashiNomura 23:79e20be4bc5b 356 bDoCtrlAtt = true;
MasashiNomura 23:79e20be4bc5b 357 break;
MasashiNomura 23:79e20be4bc5b 358 case CHK_EXIT: //チェックステート脱出
MasashiNomura 36:2cc739c7e4cb 359 if(gf_StateEnt){
MasashiNomura 36:2cc739c7e4cb 360 sp.printf("Check Exit\r\n");
MasashiNomura 36:2cc739c7e4cb 361 gf_StateEnt = false;
MasashiNomura 36:2cc739c7e4cb 362 }
MasashiNomura 23:79e20be4bc5b 363 break;
MasashiNomura 25:f3a6e7eec9c3 364 case MOT_STOP:
MasashiNomura 36:2cc739c7e4cb 365 if(gf_StateEnt){
MasashiNomura 36:2cc739c7e4cb 366 sp.printf("Motor Stop\r\n");
MasashiNomura 36:2cc739c7e4cb 367 gf_StateEnt = false;
MasashiNomura 36:2cc739c7e4cb 368 }
MasashiNomura 32:7f4145cc3551 369 // if(hb.stopMotor() == true){
MasashiNomura 32:7f4145cc3551 370 // hb.initMotVal();
MasashiNomura 32:7f4145cc3551 371 // setState(SLEEP);
MasashiNomura 32:7f4145cc3551 372 // sp.printf("MOTOR STOPPED!\r\n");
MasashiNomura 32:7f4145cc3551 373 // }
MasashiNomura 32:7f4145cc3551 374 // else{
MasashiNomura 32:7f4145cc3551 375 // //bDoCtrlMot = true;
MasashiNomura 32:7f4145cc3551 376 // sp.printf(".");
MasashiNomura 32:7f4145cc3551 377 // }
MasashiNomura 25:f3a6e7eec9c3 378 break;
MasashiNomura 22:24c9c2dedca9 379 }
takeru0x1103 21:78302ecdb661 380
takeru0x1103 21:78302ecdb661 381 //▼③各種設定
takeru0x1103 21:78302ecdb661 382 //hb.controlAttitude(); //姿勢制御
takeru0x1103 21:78302ecdb661 383 //hb.controlMotor();//モーター指令出し
MasashiNomura 23:79e20be4bc5b 384 if(bDoCtrlAtt)hb.controlAttitude(); //姿勢制御
MasashiNomura 23:79e20be4bc5b 385 if(bDoCtrlMot)hb.controlMotor();//モーター指令出し
MasashiNomura 31:56c554c560c1 386 //if(bDoCtrlEng)hb.controlEngine();//エンジン指令出し
MasashiNomura 31:56c554c560c1 387
MasashiNomura 23:79e20be4bc5b 388 if(gf_Print.bf.stat){
MasashiNomura 23:79e20be4bc5b 389 sp.printf("stat : [%X]\r\n",gf_State);
MasashiNomura 23:79e20be4bc5b 390 }
takeru0x1103 18:5aa48aec9cae 391 //表示フラグを落とす（けどモニタフラグが立ってる箇所は残る）
takeru0x1103 18:5aa48aec9cae 392 if(gf_Print.flg!=0){
takeru0x1103 18:5aa48aec9cae 393 gf_Print.flg=gf_Mon.flg;
takeru0x1103 18:5aa48aec9cae 394 sp.printf("\r\n");
takeru0x1103 18:5aa48aec9cae 395 }
MasashiNomura 26:732bc37fbefd 396 if(gf_DbgPrint.flg != 0){
MasashiNomura 26:732bc37fbefd 397 gf_DbgPrint.flg = 0;
MasashiNomura 26:732bc37fbefd 398 sp.printf("\r\n");
MasashiNomura 26:732bc37fbefd 399 }
takeru0x1103 1:15ab74f0d0f1 400
takeru0x1103 1:15ab74f0d0f1 401 //次の周期まで待つ
takeru0x1103 1:15ab74f0d0f1 402 vTaskDelayUntil(&xLastWakeTime, 20 / portTICK_RATE_MS );
takeru0x1103 1:15ab74f0d0f1 403 }
takeru0x1103 1:15ab74f0d0f1 404 }
takeru0x1103 18:5aa48aec9cae 405 //========================================================
takeru0x1103 21:78302ecdb661 406 //タスク２：デバッグ用タスク
takeru0x1103 18:5aa48aec9cae 407 //========================================================
takeru0x1103 18:5aa48aec9cae 408 void taskDebug(void *pvParameters){
takeru0x1103 21:78302ecdb661 409
takeru0x1103 21:78302ecdb661 410 taskStop(2);//自ら止めてレジュームされるまで待つ
MasashiNomura 24:c5945aaae777 411 UCHAR Num;
MasashiNomura 24:c5945aaae777 412 INT16 iVal;
MasashiNomura 25:f3a6e7eec9c3 413 //bool rvFlg;
MasashiNomura 24:c5945aaae777 414
takeru0x1103 21:78302ecdb661 415 //この関数をリターンしてしまうと他のタスクも止まるのでwhileで回すこと！
takeru0x1103 18:5aa48aec9cae 416 while(1){
MasashiNomura 24:c5945aaae777 417 for(Num = 0; Num < 8; ++Num){
MasashiNomura 24:c5945aaae777 418 for(iVal = 50; iVal < 300; ++iVal){
MasashiNomura 24:c5945aaae777 419 fpgaMotor(Num, iVal);
MasashiNomura 25:f3a6e7eec9c3 420 wait(0.002);
MasashiNomura 24:c5945aaae777 421 }
MasashiNomura 24:c5945aaae777 422 for(iVal = 300; iVal >= 50; --iVal){
MasashiNomura 24:c5945aaae777 423 fpgaMotor(Num, iVal);
MasashiNomura 25:f3a6e7eec9c3 424 wait(0.002);
MasashiNomura 24:c5945aaae777 425 }
MasashiNomura 24:c5945aaae777 426 }
MasashiNomura 23:79e20be4bc5b 427 //自タスク停止させて次の周期まで待つ
MasashiNomura 22:24c9c2dedca9 428 gf_Dbg = false;
MasashiNomura 23:79e20be4bc5b 429 sp.printf("Do DebugTask!");
takeru0x1103 18:5aa48aec9cae 430 taskStop(2);
takeru0x1103 18:5aa48aec9cae 431 }
takeru0x1103 18:5aa48aec9cae 432 }
takeru0x1103 17:f9610f3cfa1b 433
takeru0x1103 1:15ab74f0d0f1 434 //-------------------------------------------------------------
takeru0x1103 17:f9610f3cfa1b 435 //初期化：タスク登録
takeru0x1103 1:15ab74f0d0f1 436 //-------------------------------------------------------------
takeru0x1103 1:15ab74f0d0f1 437 void taskInit(){
takeru0x1103 21:78302ecdb661 438 portBASE_TYPE TaskRtn;//タスククリエイトの結果を受け取る型
takeru0x1103 18:5aa48aec9cae 439
takeru0x1103 21:78302ecdb661 440 //タスク０：コマンド解析タスク
MasashiNomura 26:732bc37fbefd 441 TaskRtn= xTaskCreate(taskCmdParser, (signed portCHAR *)"TaskCmd", 512, NULL, 1, &tskHandle[0]);
takeru0x1103 21:78302ecdb661 442 if(TaskRtn==pdTRUE){sp.printf("Task0 Set : Command parser\r\n");}
takeru0x1103 21:78302ecdb661 443
takeru0x1103 21:78302ecdb661 444 //タスク１：制御タスク
MasashiNomura 28:fdb3b144e342 445 TaskRtn= xTaskCreate(taskHbControl, (signed portCHAR *)"TaskHbCont", 1024, NULL, 2, &tskHandle[1]);
takeru0x1103 21:78302ecdb661 446 if(TaskRtn==pdTRUE){sp.printf("Task1 Set : Hoverbike Control\r\n");}
takeru0x1103 21:78302ecdb661 447
takeru0x1103 21:78302ecdb661 448 //タスク２：デバッグタスク
MasashiNomura 23:79e20be4bc5b 449 TaskRtn= xTaskCreate(taskDebug, (signed portCHAR *)"TaskDebug", 192, NULL, 0, &tskHandle[2]);
takeru0x1103 21:78302ecdb661 450 if(TaskRtn==pdTRUE){sp.printf("Task2 Set : Debug\r\n");}
takeru0x1103 8:1ca49cb18290 451
takeru0x1103 21:78302ecdb661 452 //RTOSカーネル起動（タスクが走り出す）
takeru0x1103 1:15ab74f0d0f1 453 vTaskStartScheduler();
takeru0x1103 1:15ab74f0d0f1 454 }
takeru0x1103 1:15ab74f0d0f1 455
takeru0x1103 1:15ab74f0d0f1 456