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@48:71aec693a7dc, 2019-02-24 (annotated)

Committer:
MasashiNomura
Date:
Sun Feb 24 10:33:34 2019 +0000
Revision:
48:71aec693a7dc
Parent:
47:d3fa874f336e
Child:
53:b09c062cc31c
20190224 add cmd pid mode etc.

Who changed what in which revision?

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