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@30:13ada1a24c59, 2018-12-18 (annotated)

Committer:
MasashiNomura
Date:
Tue Dec 18 11:57:37 2018 +0000
Revision:
30:13ada1a24c59
Parent:
29:eb3d72dd94aa
Child:
31:56c554c560c1
12/18 modify SW bit etc

Who changed what in which revision?

UserRevisionLine numberNew contents of line
takeru0x1103 21:78302ecdb661 1 //RTOS関連------------------
takeru0x1103 1:15ab74f0d0f1 2 #include "FreeRTOS.h"
takeru0x1103 1:15ab74f0d0f1 3 #include "task.h"
takeru0x1103 21:78302ecdb661 4 //#include "queue.h"
takeru0x1103 21:78302ecdb661 5 //------------------RTOS関連
takeru0x1103 21:78302ecdb661 6
takeru0x1103 17:f9610f3cfa1b 7 #include "globalFlags.h"
takeru0x1103 17:f9610f3cfa1b 8 #include "HbManager.h"
takeru0x1103 18:5aa48aec9cae 9 #include "hbCommand.h"
takeru0x1103 18:5aa48aec9cae 10 #include "uart.h"
MasashiNomura 24:c5945aaae777 11 #include "fpga.h"
takeru0x1103 1:15ab74f0d0f1 12
takeru0x1103 18:5aa48aec9cae 13 //タスクハンドル（停止とか再開に必要）
takeru0x1103 18:5aa48aec9cae 14 static xTaskHandle tskHandle[3]={NULL,};
takeru0x1103 1:15ab74f0d0f1 15
takeru0x1103 21:78302ecdb661 16 //------------------
takeru0x1103 18:5aa48aec9cae 17 //タスク停止
takeru0x1103 18:5aa48aec9cae 18 //------------------
takeru0x1103 18:5aa48aec9cae 19 static void taskStop(int iId){
takeru0x1103 18:5aa48aec9cae 20 sp.printf("Task[%d] Stop\r\n" , iId);
takeru0x1103 18:5aa48aec9cae 21 vTaskSuspend(tskHandle[iId]);//タスクを止める
takeru0x1103 18:5aa48aec9cae 22 }
takeru0x1103 18:5aa48aec9cae 23
takeru0x1103 21:78302ecdb661 24 //------------------
takeru0x1103 18:5aa48aec9cae 25 //タスク再開
takeru0x1103 18:5aa48aec9cae 26 //------------------
takeru0x1103 18:5aa48aec9cae 27 static void taskStart(int iId){
takeru0x1103 18:5aa48aec9cae 28 sp.printf("Task[%d] Start!!\r\n" , iId);
takeru0x1103 18:5aa48aec9cae 29 vTaskResume(tskHandle[iId]);//タスク再開
takeru0x1103 18:5aa48aec9cae 30 }
takeru0x1103 18:5aa48aec9cae 31
takeru0x1103 18:5aa48aec9cae 32 //========================================================
takeru0x1103 21:78302ecdb661 33 //コマンド解析タスク
takeru0x1103 21:78302ecdb661 34 //========================================================
takeru0x1103 21:78302ecdb661 35 void taskCmdParser(void *pvParameters){
takeru0x1103 21:78302ecdb661 36 // int8_t *pcTaskName = (int8_t *) pvParameters;;
takeru0x1103 21:78302ecdb661 37 portTickType xLastWakeTime = xTaskGetTickCount();
takeru0x1103 21:78302ecdb661 38
takeru0x1103 21:78302ecdb661 39 // vTaskDelay(300);//制御タスクを先に走らせたいので待たす
takeru0x1103 21:78302ecdb661 40
takeru0x1103 21:78302ecdb661 41 while(1){
takeru0x1103 21:78302ecdb661 42 led2=!led2;
takeru0x1103 21:78302ecdb661 43 //コマンド解釈
takeru0x1103 21:78302ecdb661 44 commandParse();
takeru0x1103 21:78302ecdb661 45
takeru0x1103 21:78302ecdb661 46 //
MasashiNomura 24:c5945aaae777 47 // if(gf_Armed){
MasashiNomura 24:c5945aaae777 48 // setState(WAKEUP);
MasashiNomura 24:c5945aaae777 49 // gf_Armed = false;
MasashiNomura 24:c5945aaae777 50 // // taskStop(0);//制御タスクを止める
MasashiNomura 24:c5945aaae777 51 // // taskStart(2);//デバッグタスク再開
MasashiNomura 24:c5945aaae777 52 // }
MasashiNomura 22:24c9c2dedca9 53 // else{
MasashiNomura 22:24c9c2dedca9 54 // taskStop(2);//デバッグタスクを止める
MasashiNomura 22:24c9c2dedca9 55 // taskStart(0);//制御タスク再開
MasashiNomura 22:24c9c2dedca9 56 // }//switch
takeru0x1103 21:78302ecdb661 57
takeru0x1103 21:78302ecdb661 58 //次の周期まで待つ
takeru0x1103 21:78302ecdb661 59 vTaskDelayUntil(&xLastWakeTime, 50 / portTICK_RATE_MS );
takeru0x1103 21:78302ecdb661 60 }
takeru0x1103 21:78302ecdb661 61 }
takeru0x1103 21:78302ecdb661 62 //========================================================
takeru0x1103 17:f9610f3cfa1b 63 //ホバーバイク制御タスク
takeru0x1103 18:5aa48aec9cae 64 //========================================================
takeru0x1103 17:f9610f3cfa1b 65 void taskHbControl(void *pvParameters){
takeru0x1103 21:78302ecdb661 66 // int8_t *pcTaskName = (int8_t *) pvParameters;;
takeru0x1103 21:78302ecdb661 67 portTickType xLastWakeTime = xTaskGetTickCount();
takeru0x1103 21:78302ecdb661 68
takeru0x1103 17:f9610f3cfa1b 69 HbManager hb;
takeru0x1103 8:1ca49cb18290 70
MasashiNomura 23:79e20be4bc5b 71 bool bDoCtrlAtt;
MasashiNomura 23:79e20be4bc5b 72 bool bDoCtrlMot;
MasashiNomura 30:13ada1a24c59 73 INT16 tmpRpm, difRpm;
takeru0x1103 18:5aa48aec9cae 74 //
takeru0x1103 1:15ab74f0d0f1 75 while(1){
takeru0x1103 16:05b9e44889f1 76 led1=!led1;
MasashiNomura 23:79e20be4bc5b 77 bDoCtrlAtt = false;
MasashiNomura 23:79e20be4bc5b 78 bDoCtrlMot = false;
MasashiNomura 24:c5945aaae777 79
MasashiNomura 24:c5945aaae777 80 // PID係数アップデートチェック
MasashiNomura 24:c5945aaae777 81 if(gf_PidParaUpdate){
MasashiNomura 24:c5945aaae777 82 hb.setAttPara(g_PidPara);
MasashiNomura 24:c5945aaae777 83 sp.printf("Pp : [%f]\r\n",g_PidPara.PP);
MasashiNomura 24:c5945aaae777 84 sp.printf("P : [%f]\r\n",g_PidPara.P);
MasashiNomura 24:c5945aaae777 85 sp.printf("I : [%f]\r\n",g_PidPara.I);
MasashiNomura 24:c5945aaae777 86 sp.printf("D : [%f]\r\n",g_PidPara.D);
MasashiNomura 24:c5945aaae777 87 sp.printf("IMAX: [%f]\r\n",g_PidPara.IMax);
MasashiNomura 24:c5945aaae777 88 sp.printf("IMIN: [%f]\r\n",g_PidPara.IMin);
MasashiNomura 24:c5945aaae777 89 gf_PidParaUpdate = false;
MasashiNomura 24:c5945aaae777 90 }
MasashiNomura 25:f3a6e7eec9c3 91 for(int i = 0; i < 4; ++i){
MasashiNomura 25:f3a6e7eec9c3 92 if(gf_MotParaUpdate[i]){
MasashiNomura 25:f3a6e7eec9c3 93 hb.setMotPara((UCHAR)i, g_MotPara[i]);
MasashiNomura 25:f3a6e7eec9c3 94 sp.printf("num : [%d]\r\n",i);
MasashiNomura 25:f3a6e7eec9c3 95 //sp.printf("ofs : [%d]\r\n",g_MotPara[i].offset);
MasashiNomura 25:f3a6e7eec9c3 96 sp.printf("low : [%d]\r\n",g_MotPara[i].limit_low);
MasashiNomura 25:f3a6e7eec9c3 97 sp.printf("hi : [%d]\r\n",g_MotPara[i].limit_hi);
MasashiNomura 25:f3a6e7eec9c3 98 gf_MotParaUpdate[i] = false;
MasashiNomura 25:f3a6e7eec9c3 99 }
MasashiNomura 25:f3a6e7eec9c3 100 }
MasashiNomura 25:f3a6e7eec9c3 101 if(gf_StopMot){
MasashiNomura 29:eb3d72dd94aa 102 //setStateF(MOT_STOP);
MasashiNomura 29:eb3d72dd94aa 103 //hb.getCurMotVal();
MasashiNomura 29:eb3d72dd94aa 104 for(int i = 0; i < 4; ++i){
MasashiNomura 29:eb3d72dd94aa 105 gf_MtReq[i].bf.req = true;
MasashiNomura 29:eb3d72dd94aa 106 gf_MtReq[i].bf.val = 0;
MasashiNomura 29:eb3d72dd94aa 107 gf_MtReqOfs[i].bf.req = true;
MasashiNomura 29:eb3d72dd94aa 108 gf_MtReqOfs[i].bf.val = 0;
MasashiNomura 29:eb3d72dd94aa 109 }
MasashiNomura 25:f3a6e7eec9c3 110 gf_StopMot = false;
MasashiNomura 25:f3a6e7eec9c3 111 }
MasashiNomura 23:79e20be4bc5b 112
MasashiNomura 29:eb3d72dd94aa 113
takeru0x1103 21:78302ecdb661 114 //▼①状態読み出し系
MasashiNomura 22:24c9c2dedca9 115 hb.getAttitude(); //現在角度読み出し
MasashiNomura 22:24c9c2dedca9 116 hb.controlEngine(); //エンジン回転数読み出し
MasashiNomura 22:24c9c2dedca9 117 hb.getUserCommand(); //操作ボタン状態読み出し
takeru0x1103 18:5aa48aec9cae 118
MasashiNomura 29:eb3d72dd94aa 119
takeru0x1103 21:78302ecdb661 120 //▼②ステート遷移
MasashiNomura 22:24c9c2dedca9 121 switch(gf_State){
MasashiNomura 22:24c9c2dedca9 122 case SLEEP:
MasashiNomura 22:24c9c2dedca9 123 if(gf_Dbg){
MasashiNomura 24:c5945aaae777 124 gf_Dbg = false;
MasashiNomura 22:24c9c2dedca9 125 taskStart(2);
MasashiNomura 22:24c9c2dedca9 126 }
MasashiNomura 27:ff63c23bc689 127 for(int i = 0; i < 8; ++i){
MasashiNomura 27:ff63c23bc689 128 if(gf_MtReqDct[i].bf.req){
MasashiNomura 27:ff63c23bc689 129 hb.setMotFPGA(i, gf_MtReqDct[i].bf.val);
MasashiNomura 27:ff63c23bc689 130 gf_MtReqDct[i].bf.req = false;
MasashiNomura 27:ff63c23bc689 131 }
MasashiNomura 23:79e20be4bc5b 132 }
MasashiNomura 27:ff63c23bc689 133 //else if(gf_Armed){
MasashiNomura 27:ff63c23bc689 134 // gf_Armed = false;
MasashiNomura 27:ff63c23bc689 135 // // hb.initChkMotor();
MasashiNomura 27:ff63c23bc689 136 // setState(WAKEUP);
MasashiNomura 27:ff63c23bc689 137 //}
MasashiNomura 22:24c9c2dedca9 138 break;
MasashiNomura 22:24c9c2dedca9 139 case WAKEUP:
MasashiNomura 24:c5945aaae777 140 //各種モーター
MasashiNomura 24:c5945aaae777 141 //機材のチェック
MasashiNomura 24:c5945aaae777 142 // if(hb.chkMotor() == true){
MasashiNomura 25:f3a6e7eec9c3 143 hb.calAtt();
MasashiNomura 25:f3a6e7eec9c3 144 // STANDBYでモーター回転をオフセット値まで上げるためのフラグセット
MasashiNomura 25:f3a6e7eec9c3 145 //for(int i = 0; i < 4; ++i){
MasashiNomura 25:f3a6e7eec9c3 146 // gf_MtReq[i].bf.req = true;
MasashiNomura 25:f3a6e7eec9c3 147 // gf_MtReq[i].bf.val = 200;
MasashiNomura 25:f3a6e7eec9c3 148 //}
MasashiNomura 23:79e20be4bc5b 149 setState(STANDBY);
MasashiNomura 24:c5945aaae777 150 // }
MasashiNomura 22:24c9c2dedca9 151 break;
MasashiNomura 22:24c9c2dedca9 152 case STANDBY:
MasashiNomura 25:f3a6e7eec9c3 153 // モーター回転数のチェック→モーターの回転数をオフセット値まで上げる
MasashiNomura 25:f3a6e7eec9c3 154 //nxtStatFlg = true;
MasashiNomura 25:f3a6e7eec9c3 155 //for(int i = 0; i < 4; ++i){
MasashiNomura 25:f3a6e7eec9c3 156 // if(gf_MtReq[i].bf.req){
MasashiNomura 25:f3a6e7eec9c3 157 // bDoCtrlMot = true;
MasashiNomura 25:f3a6e7eec9c3 158 // }
MasashiNomura 25:f3a6e7eec9c3 159 //}
MasashiNomura 25:f3a6e7eec9c3 160 if(!bDoCtrlMot){
MasashiNomura 25:f3a6e7eec9c3 161 if(hb.chkInRangeIDLE()){
MasashiNomura 25:f3a6e7eec9c3 162 setState(IDLE);
MasashiNomura 25:f3a6e7eec9c3 163 }
MasashiNomura 25:f3a6e7eec9c3 164 else{
MasashiNomura 25:f3a6e7eec9c3 165 // デバッグのためここもスルー
MasashiNomura 25:f3a6e7eec9c3 166 setState(IDLE);
MasashiNomura 25:f3a6e7eec9c3 167 }
MasashiNomura 23:79e20be4bc5b 168 }
MasashiNomura 22:24c9c2dedca9 169 break;
MasashiNomura 22:24c9c2dedca9 170 case IDLE:
MasashiNomura 23:79e20be4bc5b 171 //SWのチェック
MasashiNomura 26:732bc37fbefd 172 // Front Left
MasashiNomura 30:13ada1a24c59 173 if(hb.chkSWUserOpeRE(HbUserOpe::BRK_L)){
MasashiNomura 30:13ada1a24c59 174 gf_MtReqOP[0].req=true;
MasashiNomura 30:13ada1a24c59 175 tmpRpm = hb.getCurMotVal(F_L);
MasashiNomura 30:13ada1a24c59 176 if(tmpRpm < 6000){
MasashiNomura 30:13ada1a24c59 177 hb.setMotVal(F_L, 6000);
MasashiNomura 30:13ada1a24c59 178 }
MasashiNomura 30:13ada1a24c59 179 } else if(hb.chkSWUserOpe(HbUserOpe::BRK_L)){
MasashiNomura 30:13ada1a24c59 180 if(gf_MtReqOP[0].req==true){
MasashiNomura 30:13ada1a24c59 181 hb.setMotVal(F_L, 6000);
MasashiNomura 30:13ada1a24c59 182 }
MasashiNomura 30:13ada1a24c59 183 } else if(!hb.chkSWUserOpe(HbUserOpe::BRK_L)){
MasashiNomura 30:13ada1a24c59 184 if(gf_MtReqOP[0].req){
MasashiNomura 30:13ada1a24c59 185 tmpRpm = hb.getCurMotVal(F_L);
MasashiNomura 30:13ada1a24c59 186 if(tmpRpm == 0){
MasashiNomura 30:13ada1a24c59 187 init1PushStruct(gf_MtReqOP[0]);
MasashiNomura 30:13ada1a24c59 188 }else if(tmpRpm > 1000){
MasashiNomura 30:13ada1a24c59 189 tmpRpm-=100;
MasashiNomura 30:13ada1a24c59 190 } else if(tmpRpm >= 100){
MasashiNomura 30:13ada1a24c59 191 tmpRpm-=50;
MasashiNomura 30:13ada1a24c59 192 } else if(tmpRpm > 0){
MasashiNomura 30:13ada1a24c59 193 tmpRpm-=10;
MasashiNomura 30:13ada1a24c59 194 if(tmpRpm < 0) tmpRpm = 0;
MasashiNomura 30:13ada1a24c59 195 } else if(tmpRpm < -1000){
MasashiNomura 30:13ada1a24c59 196 tmpRpm+=100;
MasashiNomura 30:13ada1a24c59 197 } else if(tmpRpm <= -100){
MasashiNomura 30:13ada1a24c59 198 tmpRpm+=50;
MasashiNomura 30:13ada1a24c59 199 }else if(tmpRpm < 0){
MasashiNomura 30:13ada1a24c59 200 tmpRpm+=10;
MasashiNomura 30:13ada1a24c59 201 if(tmpRpm > 0) tmpRpm = 0;
MasashiNomura 30:13ada1a24c59 202 }
MasashiNomura 30:13ada1a24c59 203 hb.setMotVal(F_L, tmpRpm);
MasashiNomura 30:13ada1a24c59 204 }
MasashiNomura 26:732bc37fbefd 205 }
MasashiNomura 30:13ada1a24c59 206
MasashiNomura 30:13ada1a24c59 207 if(hb.chkSWUserOpeRE(HbUserOpe::BRK_R)){
MasashiNomura 30:13ada1a24c59 208 gf_MtReqOP[1].req=true;
MasashiNomura 30:13ada1a24c59 209 tmpRpm = hb.getCurMotVal(F_R);
MasashiNomura 30:13ada1a24c59 210 if(tmpRpm < 6000){
MasashiNomura 30:13ada1a24c59 211 hb.setMotVal(F_R, 6000);
MasashiNomura 30:13ada1a24c59 212 }
MasashiNomura 30:13ada1a24c59 213 } else if(hb.chkSWUserOpe(HbUserOpe::BRK_R)){
MasashiNomura 30:13ada1a24c59 214 if(gf_MtReqOP[1].req==true){
MasashiNomura 30:13ada1a24c59 215 hb.setMotVal(F_R, 6000);
MasashiNomura 30:13ada1a24c59 216 }
MasashiNomura 30:13ada1a24c59 217 } else if(!hb.chkSWUserOpe(HbUserOpe::BRK_R)){
MasashiNomura 30:13ada1a24c59 218 if(gf_MtReqOP[1].req){
MasashiNomura 30:13ada1a24c59 219 tmpRpm = hb.getCurMotVal(F_R);
MasashiNomura 30:13ada1a24c59 220 if(tmpRpm == 0){
MasashiNomura 30:13ada1a24c59 221 init1PushStruct(gf_MtReqOP[1]);
MasashiNomura 30:13ada1a24c59 222 }else if(tmpRpm > 1000){
MasashiNomura 30:13ada1a24c59 223 tmpRpm-=100;
MasashiNomura 30:13ada1a24c59 224 } else if(tmpRpm >= 100){
MasashiNomura 30:13ada1a24c59 225 tmpRpm-=50;
MasashiNomura 30:13ada1a24c59 226 } else if(tmpRpm > 0){
MasashiNomura 30:13ada1a24c59 227 tmpRpm-=10;
MasashiNomura 30:13ada1a24c59 228 if(tmpRpm < 0) tmpRpm = 0;
MasashiNomura 30:13ada1a24c59 229 } else if(tmpRpm < -1000){
MasashiNomura 30:13ada1a24c59 230 tmpRpm+=100;
MasashiNomura 30:13ada1a24c59 231 } else if(tmpRpm <= -100){
MasashiNomura 30:13ada1a24c59 232 tmpRpm+=50;
MasashiNomura 30:13ada1a24c59 233 }else if(tmpRpm < 0){
MasashiNomura 30:13ada1a24c59 234 tmpRpm+=10;
MasashiNomura 30:13ada1a24c59 235 if(tmpRpm > 0) tmpRpm = 0;
MasashiNomura 30:13ada1a24c59 236 }
MasashiNomura 30:13ada1a24c59 237 hb.setMotVal(F_R, tmpRpm);
MasashiNomura 30:13ada1a24c59 238 }
MasashiNomura 23:79e20be4bc5b 239 }
MasashiNomura 30:13ada1a24c59 240
MasashiNomura 30:13ada1a24c59 241
MasashiNomura 30:13ada1a24c59 242 // //if(hb.chkSWUserOpeRE(HbUserOpe::F_R)){
MasashiNomura 30:13ada1a24c59 243 // if(hb.chkSWUserOpe(HbUserOpe::BRK_R)){
MasashiNomura 30:13ada1a24c59 244 // // Front Right
MasashiNomura 30:13ada1a24c59 245 // //hb.addMotOfs(HbUserOpe::F_R);
MasashiNomura 30:13ada1a24c59 246 // // gf_MtReqOP[1].req=true; gf_MtReqOP[1].add_end=false; gf_MtReqOP[1].counter = 0; gf_MtReqOP[1].num = 20;
MasashiNomura 30:13ada1a24c59 247 // // gf_MtReqOP[1].addVal = 0;
MasashiNomura 30:13ada1a24c59 248 // }
MasashiNomura 30:13ada1a24c59 249 // if(hb.chkSWUserOpe(HbUserOpe::R_1)){
MasashiNomura 30:13ada1a24c59 250 // // Rear Left
MasashiNomura 30:13ada1a24c59 251 // //hb.addMotOfs(HbUserOpe::R_L);
MasashiNomura 30:13ada1a24c59 252 // }
MasashiNomura 30:13ada1a24c59 253 // if(hb.chkSWUserOpe(HbUserOpe::R_2)){
MasashiNomura 30:13ada1a24c59 254 // // Rear Right
MasashiNomura 30:13ada1a24c59 255 // //hb.addMotOfs(HbUserOpe::R_R);
MasashiNomura 30:13ada1a24c59 256 // }
MasashiNomura 30:13ada1a24c59 257 if(hb.chkSWUserOpeRE(HbUserOpe::MOT_STOP)){
MasashiNomura 26:732bc37fbefd 258 // EMG STOP だが、テスト用のため、モーターのオフセットを0にして回転を止める
MasashiNomura 26:732bc37fbefd 259 //sp.printf("RisingEdge!\r\n");
MasashiNomura 26:732bc37fbefd 260 for(int i = 0; i < 4; ++i){
MasashiNomura 29:eb3d72dd94aa 261 gf_MtReq[i].bf.req = true;
MasashiNomura 29:eb3d72dd94aa 262 gf_MtReq[i].bf.val = 0;
MasashiNomura 27:ff63c23bc689 263 gf_MtReqOfs[i].bf.req = true;
MasashiNomura 27:ff63c23bc689 264 gf_MtReqOfs[i].bf.val = 0;
MasashiNomura 26:732bc37fbefd 265 }
MasashiNomura 26:732bc37fbefd 266 }
MasashiNomura 26:732bc37fbefd 267
MasashiNomura 23:79e20be4bc5b 268 //else if(hb.chkSWUserOpe(HbUserOpe::ENG_STOP))
MasashiNomura 23:79e20be4bc5b 269 //{
MasashiNomura 23:79e20be4bc5b 270 // setState(SLEEP);
MasashiNomura 23:79e20be4bc5b 271 //}
MasashiNomura 30:13ada1a24c59 272 // if(hb.chkSWUserOpeAny()){
MasashiNomura 30:13ada1a24c59 273 // sp.printf("SW :[%X]\r\n",hb.getUserSw());
MasashiNomura 30:13ada1a24c59 274 // }
MasashiNomura 25:f3a6e7eec9c3 275 bDoCtrlMot = true;
MasashiNomura 22:24c9c2dedca9 276 break;
MasashiNomura 22:24c9c2dedca9 277 case TAKE_OFF:
MasashiNomura 23:79e20be4bc5b 278 // エンジン回転数のチェック
MasashiNomura 23:79e20be4bc5b 279 // 着陸ボタンが優先度が高いので先に判定、たが、まだ作ってないのでコメントアウト
MasashiNomura 23:79e20be4bc5b 280 // if(hb.chkSWUserOpe(HbUserOpe::STOP)){
MasashiNomura 23:79e20be4bc5b 281 // setState(GROUND);
MasashiNomura 23:79e20be4bc5b 282 // }
MasashiNomura 23:79e20be4bc5b 283 //else
MasashiNomura 23:79e20be4bc5b 284 bDoCtrlMot = true;
MasashiNomura 23:79e20be4bc5b 285 bDoCtrlAtt = true;
MasashiNomura 23:79e20be4bc5b 286 //if(hb.chkSWUserOpe(HbUserOpe::STOP)){
MasashiNomura 23:79e20be4bc5b 287 // setState(HOVER);
MasashiNomura 23:79e20be4bc5b 288 //}
MasashiNomura 23:79e20be4bc5b 289
MasashiNomura 22:24c9c2dedca9 290 break;
MasashiNomura 22:24c9c2dedca9 291 case GROUND:
MasashiNomura 23:79e20be4bc5b 292 // そのままスルー
MasashiNomura 23:79e20be4bc5b 293 setState(IDLE);
MasashiNomura 22:24c9c2dedca9 294 break;
MasashiNomura 22:24c9c2dedca9 295 case HOVER:
MasashiNomura 23:79e20be4bc5b 296 //
MasashiNomura 22:24c9c2dedca9 297 break;
MasashiNomura 22:24c9c2dedca9 298 case DRIVE:
MasashiNomura 22:24c9c2dedca9 299 break;
MasashiNomura 22:24c9c2dedca9 300 case EMGGND:
MasashiNomura 22:24c9c2dedca9 301 break;
MasashiNomura 23:79e20be4bc5b 302 case CHK_ENT: //チェックエンター
MasashiNomura 23:79e20be4bc5b 303 sp.printf("Check enter\r\n");
MasashiNomura 23:79e20be4bc5b 304 break;
MasashiNomura 23:79e20be4bc5b 305 case CHK_MOT: //モーターチェック
MasashiNomura 23:79e20be4bc5b 306 sp.printf("Check Motor\r\n");
MasashiNomura 23:79e20be4bc5b 307 bDoCtrlMot = true;
MasashiNomura 23:79e20be4bc5b 308 break;
MasashiNomura 23:79e20be4bc5b 309 case CHK_AXL: //アクセルサーボチェック
MasashiNomura 23:79e20be4bc5b 310 sp.printf("Check Accel Servo\r\n");
MasashiNomura 23:79e20be4bc5b 311 break;
MasashiNomura 23:79e20be4bc5b 312 case CHK_ATT: //姿勢制御チェック
MasashiNomura 23:79e20be4bc5b 313 sp.printf("Check Attitude\r\n");
MasashiNomura 23:79e20be4bc5b 314 bDoCtrlAtt = true;
MasashiNomura 23:79e20be4bc5b 315 break;
MasashiNomura 23:79e20be4bc5b 316 case CHK_EXIT: //チェックステート脱出
MasashiNomura 23:79e20be4bc5b 317 sp.printf("Check Exit\r\n");
MasashiNomura 23:79e20be4bc5b 318 break;
MasashiNomura 25:f3a6e7eec9c3 319 case MOT_STOP:
MasashiNomura 25:f3a6e7eec9c3 320 if(hb.stopMotor() == true){
MasashiNomura 25:f3a6e7eec9c3 321 hb.initMotVal();
MasashiNomura 25:f3a6e7eec9c3 322 setState(SLEEP);
MasashiNomura 25:f3a6e7eec9c3 323 sp.printf("MOTOR STOPPED!\r\n");
MasashiNomura 25:f3a6e7eec9c3 324 }
MasashiNomura 25:f3a6e7eec9c3 325 else{
MasashiNomura 25:f3a6e7eec9c3 326 //bDoCtrlMot = true;
MasashiNomura 25:f3a6e7eec9c3 327 sp.printf(".");
MasashiNomura 25:f3a6e7eec9c3 328 }
MasashiNomura 25:f3a6e7eec9c3 329 break;
MasashiNomura 22:24c9c2dedca9 330 }
takeru0x1103 21:78302ecdb661 331
takeru0x1103 21:78302ecdb661 332 //▼③各種設定
takeru0x1103 21:78302ecdb661 333 //hb.controlAttitude(); //姿勢制御
takeru0x1103 21:78302ecdb661 334 //hb.controlMotor();//モーター指令出し
MasashiNomura 23:79e20be4bc5b 335 if(bDoCtrlAtt)hb.controlAttitude(); //姿勢制御
MasashiNomura 23:79e20be4bc5b 336 if(bDoCtrlMot)hb.controlMotor();//モーター指令出し
takeru0x1103 18:5aa48aec9cae 337
MasashiNomura 23:79e20be4bc5b 338 if(gf_Print.bf.stat){
MasashiNomura 23:79e20be4bc5b 339 sp.printf("stat : [%X]\r\n",gf_State);
MasashiNomura 23:79e20be4bc5b 340 }
takeru0x1103 18:5aa48aec9cae 341 //表示フラグを落とす（けどモニタフラグが立ってる箇所は残る）
takeru0x1103 18:5aa48aec9cae 342 if(gf_Print.flg!=0){
takeru0x1103 18:5aa48aec9cae 343 gf_Print.flg=gf_Mon.flg;
takeru0x1103 18:5aa48aec9cae 344 sp.printf("\r\n");
takeru0x1103 18:5aa48aec9cae 345 }
MasashiNomura 26:732bc37fbefd 346 if(gf_DbgPrint.flg != 0){
MasashiNomura 26:732bc37fbefd 347 gf_DbgPrint.flg = 0;
MasashiNomura 26:732bc37fbefd 348 sp.printf("\r\n");
MasashiNomura 26:732bc37fbefd 349 }
takeru0x1103 1:15ab74f0d0f1 350
takeru0x1103 1:15ab74f0d0f1 351 //次の周期まで待つ
takeru0x1103 1:15ab74f0d0f1 352 vTaskDelayUntil(&xLastWakeTime, 20 / portTICK_RATE_MS );
takeru0x1103 1:15ab74f0d0f1 353 }
takeru0x1103 1:15ab74f0d0f1 354 }
takeru0x1103 18:5aa48aec9cae 355 //========================================================
takeru0x1103 21:78302ecdb661 356 //タスク２：デバッグ用タスク
takeru0x1103 18:5aa48aec9cae 357 //========================================================
takeru0x1103 18:5aa48aec9cae 358 void taskDebug(void *pvParameters){
takeru0x1103 21:78302ecdb661 359
takeru0x1103 21:78302ecdb661 360 taskStop(2);//自ら止めてレジュームされるまで待つ
MasashiNomura 24:c5945aaae777 361 UCHAR Num;
MasashiNomura 24:c5945aaae777 362 INT16 iVal;
MasashiNomura 25:f3a6e7eec9c3 363 //bool rvFlg;
MasashiNomura 24:c5945aaae777 364
takeru0x1103 21:78302ecdb661 365 //この関数をリターンしてしまうと他のタスクも止まるのでwhileで回すこと！
takeru0x1103 18:5aa48aec9cae 366 while(1){
MasashiNomura 24:c5945aaae777 367 for(Num = 0; Num < 8; ++Num){
MasashiNomura 24:c5945aaae777 368 for(iVal = 50; iVal < 300; ++iVal){
MasashiNomura 24:c5945aaae777 369 fpgaMotor(Num, iVal);
MasashiNomura 25:f3a6e7eec9c3 370 wait(0.002);
MasashiNomura 24:c5945aaae777 371 }
MasashiNomura 24:c5945aaae777 372 for(iVal = 300; iVal >= 50; --iVal){
MasashiNomura 24:c5945aaae777 373 fpgaMotor(Num, iVal);
MasashiNomura 25:f3a6e7eec9c3 374 wait(0.002);
MasashiNomura 24:c5945aaae777 375 }
MasashiNomura 24:c5945aaae777 376 }
MasashiNomura 23:79e20be4bc5b 377 //自タスク停止させて次の周期まで待つ
MasashiNomura 22:24c9c2dedca9 378 gf_Dbg = false;
MasashiNomura 23:79e20be4bc5b 379 sp.printf("Do DebugTask!");
takeru0x1103 18:5aa48aec9cae 380 taskStop(2);
takeru0x1103 18:5aa48aec9cae 381 }
takeru0x1103 18:5aa48aec9cae 382 }
takeru0x1103 17:f9610f3cfa1b 383
takeru0x1103 1:15ab74f0d0f1 384 //-------------------------------------------------------------
takeru0x1103 17:f9610f3cfa1b 385 //初期化：タスク登録
takeru0x1103 1:15ab74f0d0f1 386 //-------------------------------------------------------------
takeru0x1103 1:15ab74f0d0f1 387 void taskInit(){
takeru0x1103 21:78302ecdb661 388 portBASE_TYPE TaskRtn;//タスククリエイトの結果を受け取る型
takeru0x1103 18:5aa48aec9cae 389
takeru0x1103 21:78302ecdb661 390 //タスク０：コマンド解析タスク
MasashiNomura 26:732bc37fbefd 391 TaskRtn= xTaskCreate(taskCmdParser, (signed portCHAR *)"TaskCmd", 512, NULL, 1, &tskHandle[0]);
takeru0x1103 21:78302ecdb661 392 if(TaskRtn==pdTRUE){sp.printf("Task0 Set : Command parser\r\n");}
takeru0x1103 21:78302ecdb661 393
takeru0x1103 21:78302ecdb661 394 //タスク１：制御タスク
MasashiNomura 28:fdb3b144e342 395 TaskRtn= xTaskCreate(taskHbControl, (signed portCHAR *)"TaskHbCont", 1024, NULL, 2, &tskHandle[1]);
takeru0x1103 21:78302ecdb661 396 if(TaskRtn==pdTRUE){sp.printf("Task1 Set : Hoverbike Control\r\n");}
takeru0x1103 21:78302ecdb661 397
takeru0x1103 21:78302ecdb661 398 //タスク２：デバッグタスク
MasashiNomura 23:79e20be4bc5b 399 TaskRtn= xTaskCreate(taskDebug, (signed portCHAR *)"TaskDebug", 192, NULL, 0, &tskHandle[2]);
takeru0x1103 21:78302ecdb661 400 if(TaskRtn==pdTRUE){sp.printf("Task2 Set : Debug\r\n");}
takeru0x1103 8:1ca49cb18290 401
takeru0x1103 21:78302ecdb661 402 //RTOSカーネル起動（タスクが走り出す）
takeru0x1103 1:15ab74f0d0f1 403 vTaskStartScheduler();
takeru0x1103 1:15ab74f0d0f1 404 }
takeru0x1103 1:15ab74f0d0f1 405
takeru0x1103 1:15ab74f0d0f1 406