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@39:1b76f7df8804, 2019-01-19 (annotated)

Committer:
MasashiNomura
Date:
Sat Jan 19 12:35:23 2019 +0000
Revision:
39:1b76f7df8804
Parent:
38:24ee50452755
Child:
40:debe99e228d3
20190119 Modify for 45inc; Checking User SW and sequence

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