teamALI
/
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.
Revision 66:0b3b290137b6, committed 2019-03-11
- Comitter:
- MasashiNomura
- Date:
- Mon Mar 11 06:24:39 2019 +0000
- Parent:
- 65:64c56433f661
- Child:
- 68:1d5471344acf
- Child:
- 70:a7b1e3eec51b
- Commit message:
- 2019/03/11 Add Brake and forward.; delete some unnecessary comment-out.
Changed in this revision
| HbManager.cpp | Show annotated file Show diff for this revision Revisions of this file |
| userTask.cpp | Show annotated file Show diff for this revision Revisions of this file |
--- a/HbManager.cpp Mon Mar 11 05:24:02 2019 +0000
+++ b/HbManager.cpp Mon Mar 11 06:24:39 2019 +0000
@@ -637,155 +637,178 @@
void HbManager::chkSW(enmHbState stat){
INT16 tmpRpm,tmpAxl;
- // Front Left
- if(g_PidPara.mode == PID_0_ON){
- if(chkSWUserOpeRE(HbUserOpe::BRK_L) || chkSWUserOpe(HbUserOpe::BRK_L)){
- nowTrgtAng -= ang_rate_brk;
- }
- if(chkSWUserOpeRE(HbUserOpe::BRK_R) || chkSWUserOpe(HbUserOpe::BRK_R)){
- nowTrgtAng += ang_rate_brk;
- }
- for(int i = 0; i < 4; ++i){
- gf_MtReqU[i].req = true;
- gf_MtReqU[i].val = 0;
- }
- }
- else{
- if(chkSWUserOpeRE(HbUserOpe::BRK_L)){
- //nowTrgtAng -= 1;
+ tmpAxl = getUserMotAxl();
+ if(stat == IDLE || stat == UPPER_IDLE){
+ if(chkSWUserOpeBoth(HbUserOpe::BRK_L, HbUserOpe::BRK_R)){
gf_MtReqU[0].req = true;
gf_MtReqU[0].val = mot_brk;
- //setMotVal(F_L, mot_brk);
- gf_MtReqU[3].req = true;
- gf_MtReqU[3].val = mot_brk;
- //setMotVal(R_R, mot_brk);
- } else if(chkSWUserOpe(HbUserOpe::BRK_L)){
- //nowTrgtAng -= 0.1;
- gf_MtReqU[0].req = true;
- gf_MtReqU[0].val = mot_brk;
- //setMotVal(F_L, mot_brk);
- gf_MtReqU[3].req = true;
- gf_MtReqU[3].val = mot_brk;
- //setMotVal(R_R, mot_brk);
- }else if(!chkSWUserOpe(HbUserOpe::BRK_L)){
- gf_MtReqU[0].req = true;
- gf_MtReqU[0].val = 0;
- //setMotVal(F_L, mot_brk);
+ gf_MtReqU[1].req = true;
+ gf_MtReqU[1].val = mot_brk;
+ gf_MtReqU[2].req = true;
+ gf_MtReqU[2].val = 0;
gf_MtReqU[3].req = true;
gf_MtReqU[3].val = 0;
}
- // else if(!chkSWUserOpe(HbUserOpe::BRK_L)){
- // if(gf_MtReqU[0].req){
- // tmpRpm = getCurMotVal(F_L);
- // if(tmpRpm == 0){
- // gf_MtReqU[0].req = false;
- // }else if(tmpRpm > 1000){
- // tmpRpm-=100;
- // } else if(tmpRpm >= 100){
- // tmpRpm-=50;
- // } else if(tmpRpm > 0){
- // tmpRpm-=10;
- // if(tmpRpm < 0) tmpRpm = 0;
- // } else if(tmpRpm < -1000){
- // tmpRpm+=100;
- // } else if(tmpRpm <= -100){
- // tmpRpm+=50;
- // }else if(tmpRpm < 0){
- // tmpRpm+=10;
- // if(tmpRpm > 0) tmpRpm = 0;
- // }
- // setMotVal(F_L, tmpRpm);
- // }
- // if(gf_MtReqU[3].req){
- // tmpRpm = getCurMotVal(R_R);
- // if(tmpRpm == 0){
- // gf_MtReqU[3].req = false;
- // }else if(tmpRpm > 1000){
- // tmpRpm-=100;
- // } else if(tmpRpm >= 100){
- // tmpRpm-=50;
- // } else if(tmpRpm > 0){
- // tmpRpm-=10;
- // if(tmpRpm < 0) tmpRpm = 0;
- // } else if(tmpRpm < -1000){
- // tmpRpm+=100;
- // } else if(tmpRpm <= -100){
- // tmpRpm+=50;
- // }else if(tmpRpm < 0){
- // tmpRpm+=10;
- // if(tmpRpm > 0) tmpRpm = 0;
- // }
- // setMotVal(R_R, tmpRpm);
- // }
- // }
-
- if(chkSWUserOpeRE(HbUserOpe::BRK_R)){
- //nowTrgtAng += 1;
- gf_MtReqU[1].req = true;
- gf_MtReqU[1].val = mot_brk;
- //setMotVal(F_R, mot_brk);
- gf_MtReqU[2].req = true;
- gf_MtReqU[2].val = mot_brk;
- //setMotVal(R_L, mot_brk);
- } else if(chkSWUserOpe(HbUserOpe::BRK_R)){
- //nowTrgtAng += 0.1;
- gf_MtReqU[1].req = true;
- gf_MtReqU[1].val = mot_brk;
- //setMotVal(F_R, mot_brk);
- gf_MtReqU[2].req = true;
- gf_MtReqU[2].val = mot_brk;
- //setMotVal(R_L, mot_brk);
- } else if(!chkSWUserOpe(HbUserOpe::BRK_R)){
+ else if (chkSWUserOpe(HbUserOpe::BRK_L)){
+ gf_MtReqU[0].req = true;
+ gf_MtReqU[0].val = mot_brk;
gf_MtReqU[1].req = true;
gf_MtReqU[1].val = 0;
gf_MtReqU[2].req = true;
gf_MtReqU[2].val = 0;
+ gf_MtReqU[3].req = true;
+ gf_MtReqU[3].val = mot_brk;
}
- // else if(!chkSWUserOpe(HbUserOpe::BRK_R)){
- // if(gf_MtReqU[1].req){
- // tmpRpm = getCurMotVal(F_R);
- // if(tmpRpm == 0){
- // gf_MtReqU[1].req = false;
- // }else if(tmpRpm > 1000){
- // tmpRpm-=100;
- // } else if(tmpRpm >= 100){
- // tmpRpm-=50;
- // } else if(tmpRpm > 0){
- // tmpRpm-=10;
- // if(tmpRpm < 0) tmpRpm = 0;
- // } else if(tmpRpm < -1000){
- // tmpRpm+=100;
- // } else if(tmpRpm <= -100){
- // tmpRpm+=50;
- // }else if(tmpRpm < 0){
- // tmpRpm+=10;
- // if(tmpRpm > 0) tmpRpm = 0;
- // }
- // setMotVal(F_R, tmpRpm);
+ else if (chkSWUserOpe(HbUserOpe::BRK_R)){
+ gf_MtReqU[0].req = true;
+ gf_MtReqU[0].val = 0;
+ gf_MtReqU[1].req = true;
+ gf_MtReqU[1].val = mot_brk;
+ gf_MtReqU[2].req = true;
+ gf_MtReqU[2].val = mot_brk;
+ gf_MtReqU[3].req = true;
+ gf_MtReqU[3].val = 0;
+ }
+ else
+ {
+ gf_MtReqU[0].req = true;
+ gf_MtReqU[0].val = 0;
+ gf_MtReqU[1].req = true;
+ gf_MtReqU[1].val = 0;
+ // アナログ入力値で前進噴射
+ gf_MtReqU[2].req = true;
+ gf_MtReqU[2].val = tmpAxl;
+ gf_MtReqU[3].req = true;
+ gf_MtReqU[3].val = tmpAxl;
+ }
+ }
+ else if(stat == TAKE_OFF/* || stat == HOVER || stat == DRIVE*/)
+ {
+ if(g_PidPara.mode == PID_0_ON /*|| g_PidPara.mode == PID_1_ON*/)
+ {
+ if(chkSWUserOpeBoth(HbUserOpe::BRK_L, HbUserOpe::BRK_R)){
+ gf_MtReqU[0].req = true;
+ gf_MtReqU[0].val = mot_brk;
+ gf_MtReqU[1].req = true;
+ gf_MtReqU[1].val = mot_brk;
+ gf_MtReqU[2].req = true;
+ gf_MtReqU[2].val = 0;
+ gf_MtReqU[3].req = true;
+ gf_MtReqU[3].val = 0;
+ }
+ else if (chkSWUserOpe(HbUserOpe::BRK_L)){
+ nowTrgtAng -= ang_rate_brk;
+ }
+ else if (chkSWUserOpe(HbUserOpe::BRK_R)){
+ nowTrgtAng += ang_rate_brk;
+ }
+ else
+ {
+ gf_MtReqU[0].req = true;
+ gf_MtReqU[0].val = 0;
+ gf_MtReqU[1].req = true;
+ gf_MtReqU[1].val = 0;
+ // アナログ入力値で前進噴射
+ gf_MtReqU[2].req = true;
+ gf_MtReqU[2].val = tmpAxl;
+ gf_MtReqU[3].req = true;
+ gf_MtReqU[3].val = tmpAxl;
+ }
+ }
+ else
+ {
+ /* code IDLE UPPER_IDLEの内容と同じ*/
+ if(chkSWUserOpeBoth(HbUserOpe::BRK_L, HbUserOpe::BRK_R)){
+ gf_MtReqU[0].req = true;
+ gf_MtReqU[0].val = mot_brk;
+ gf_MtReqU[1].req = true;
+ gf_MtReqU[1].val = mot_brk;
+ gf_MtReqU[2].req = true;
+ gf_MtReqU[2].val = 0;
+ gf_MtReqU[3].req = true;
+ gf_MtReqU[3].val = 0;
+ }
+ else if (chkSWUserOpe(HbUserOpe::BRK_L)){
+ gf_MtReqU[0].req = true;
+ gf_MtReqU[0].val = mot_brk;
+ gf_MtReqU[1].req = true;
+ gf_MtReqU[1].val = 0;
+ gf_MtReqU[2].req = true;
+ gf_MtReqU[2].val = 0;
+ gf_MtReqU[3].req = true;
+ gf_MtReqU[3].val = mot_brk;
+ }
+ else if (chkSWUserOpe(HbUserOpe::BRK_R)){
+ gf_MtReqU[0].req = true;
+ gf_MtReqU[0].val = 0;
+ gf_MtReqU[1].req = true;
+ gf_MtReqU[1].val = mot_brk;
+ gf_MtReqU[2].req = true;
+ gf_MtReqU[2].val = mot_brk;
+ gf_MtReqU[3].req = true;
+ gf_MtReqU[3].val = 0;
+ }
+ else
+ {
+ gf_MtReqU[0].req = true;
+ gf_MtReqU[0].val = 0;
+ gf_MtReqU[1].req = true;
+ gf_MtReqU[1].val = 0;
+ // アナログ入力値で前進噴射
+ gf_MtReqU[2].req = true;
+ gf_MtReqU[2].val = tmpAxl;
+ gf_MtReqU[3].req = true;
+ gf_MtReqU[3].val = tmpAxl;
+ }
+ }
+ }
+ // if(g_PidPara.mode == PID_0_ON){
+ // if(chkSWUserOpeRE(HbUserOpe::BRK_L) || chkSWUserOpe(HbUserOpe::BRK_L)){
+ // nowTrgtAng -= ang_rate_brk;
// }
- // if(gf_MtReqU[2].req){
- // tmpRpm = getCurMotVal(R_L);
- // if(tmpRpm == 0){
- // gf_MtReqU[1].req = false;
- // }else if(tmpRpm > 1000){
- // tmpRpm-=100;
- // } else if(tmpRpm >= 100){
- // tmpRpm-=50;
- // } else if(tmpRpm > 0){
- // tmpRpm-=10;
- // if(tmpRpm < 0) tmpRpm = 0;
- // } else if(tmpRpm < -1000){
- // tmpRpm+=100;
- // } else if(tmpRpm <= -100){
- // tmpRpm+=50;
- // }else if(tmpRpm < 0){
- // tmpRpm+=10;
- // if(tmpRpm > 0) tmpRpm = 0;
- // }
- // setMotVal(R_L, tmpRpm);
+ // if(chkSWUserOpeRE(HbUserOpe::BRK_R) || chkSWUserOpe(HbUserOpe::BRK_R)){
+ // nowTrgtAng += ang_rate_brk;
+ // }
+ // for(int i = 0; i < 4; ++i){
+ // gf_MtReqU[i].req = true;
+ // gf_MtReqU[i].val = 0;
// }
// }
- }
+ // else{
+ // if(chkSWUserOpeRE(HbUserOpe::BRK_L)){
+ // gf_MtReqU[0].req = true;
+ // gf_MtReqU[0].val = mot_brk;
+ // gf_MtReqU[3].req = true;
+ // gf_MtReqU[3].val = mot_brk;
+ // } else if(chkSWUserOpe(HbUserOpe::BRK_L)){
+ // gf_MtReqU[0].req = true;
+ // gf_MtReqU[0].val = mot_brk;
+ // gf_MtReqU[3].req = true;
+ // gf_MtReqU[3].val = mot_brk;
+ // }else if(!chkSWUserOpe(HbUserOpe::BRK_L)){
+ // gf_MtReqU[0].req = true;
+ // gf_MtReqU[0].val = 0;
+ // gf_MtReqU[3].req = true;
+ // gf_MtReqU[3].val = 0;
+ // }
+
+ // if(chkSWUserOpeRE(HbUserOpe::BRK_R)){
+ // gf_MtReqU[1].req = true;
+ // gf_MtReqU[1].val = mot_brk;
+ // gf_MtReqU[2].req = true;
+ // gf_MtReqU[2].val = mot_brk;
+ // } else if(chkSWUserOpe(HbUserOpe::BRK_R)){
+ // gf_MtReqU[1].req = true;
+ // gf_MtReqU[1].val = mot_brk;
+ // gf_MtReqU[2].req = true;
+ // gf_MtReqU[2].val = mot_brk;
+ // } else if(!chkSWUserOpe(HbUserOpe::BRK_R)){
+ // gf_MtReqU[1].req = true;
+ // gf_MtReqU[1].val = 0;
+ // gf_MtReqU[2].req = true;
+ // gf_MtReqU[2].val = 0;
+ // }
+ // }
if(chkSWUserOpeRE(HbUserOpe::FLT_ON)){
if(stat == IDLE){
@@ -821,7 +844,6 @@
}
sp.printf("FLT_OFF Push \r\n");
}
- //if(chkSWUserOpe(HbUserOpe::F_ENG_UP)){
if(chkSWUserOpeRE(HbUserOpe::F_ENG_UP)){
if(stat == TAKE_OFF || stat == HOVER || stat == DRIVE){
gf_BlinkLED = true;
@@ -835,7 +857,6 @@
gf_AxReq[0].bf.val = tmpAxl;
}
}
- //if(chkSWUserOpe(HbUserOpe::F_ENG_DOWN)){
if(chkSWUserOpeRE(HbUserOpe::F_ENG_DOWN)){
if(stat == TAKE_OFF || stat == HOVER || stat == DRIVE){
gf_BlinkLED = true;
@@ -849,7 +870,6 @@
gf_AxReq[0].bf.val = tmpAxl;
}
}
- //if(chkSWUserOpe(HbUserOpe::R_ENG_UP)){
if(chkSWUserOpeRE(HbUserOpe::R_ENG_UP)){
if(stat == TAKE_OFF || stat == HOVER || stat == DRIVE){
INT16 step = en_srv_adj_step[1];
@@ -863,7 +883,6 @@
gf_AxReq[1].bf.val = tmpAxl;
}
}
- //if(chkSWUserOpe(HbUserOpe::R_ENG_DOWN)){
if(chkSWUserOpeRE(HbUserOpe::R_ENG_DOWN)){
if(stat == TAKE_OFF || stat == HOVER || stat == DRIVE){
INT16 step = en_srv_adj_step[1];
@@ -891,7 +910,6 @@
flg = true;
break;
case PID_0_ON:
- /* code */
g_PidPara.mode = PID_0_OFF;
break;
case PID_1_ON:
@@ -903,22 +921,6 @@
}
sp.printf("CTRL_SW Push : %d \r\n", flg);
}
- // if(chkSWUserOpe(HbUserOpe::ALL_STOP)){
- // // モーター
- // for(int i = 0; i < 4; ++i){
- // gf_MtReq[i].req = true;
- // gf_MtReq[i].val = 0;
- // gf_MtReqOfs[i].req = true;
- // gf_MtReqOfs[i].val = 0;
- // }
- // // エンジン用サーボ
- // for(int i = 0; i < 2; ++i){
- // gf_AxReq[i].bf.req = true;
- // gf_AxReq[i].bf.val = 0;
- // }
- // gf_FromActiveStat = isActiveState();
- // setStateF(MOT_STOP);
- // }
}
// ユーザー操作関連end
//------------------------------------------------------
@@ -984,7 +986,6 @@
en_srv_step[i] = 30;
en_srv_adj_step[i] = 50;
}
-
mot_brk = DEF_BRK_RPM;
ang_rate_brk = DEF_BRK_ANG / static_cast<float>(UPDATE_RATE);
--- a/userTask.cpp Mon Mar 11 05:24:02 2019 +0000
+++ b/userTask.cpp Mon Mar 11 06:24:39 2019 +0000
@@ -76,9 +76,9 @@
INT16 AxlRpm;
INT16 TrtlVal;
float AxlRow;
+
+ hb.setAttPara(g_PidPara);
- INT16 MotorOffsetValue = 1500;
-
while(1){
// DISPLAY LED
led1=!led1;
@@ -217,24 +217,15 @@
}
//SWのチェック
hb.chkSW(IDLE);
- // AxlRpm = hb.getUserMotAxl();
- // hb.setMotVal(R_L,AxlRpm);
- // hb.setMotVal(R_R,AxlRpm);
- // hb.setMotValOfs(R_L,AxlRpm);
- // hb.setMotValOfs(R_R,AxlRpm);
break;
case UPPER_IDLE:
if(gf_StateEnt){
sp.printf("UPPER IDLE state\r\n");
hb.calAtt();//現在値でヨー角校正
- //
gf_StateEnt = false;
}
//SWのチェック
hb.chkSW(UPPER_IDLE);
- // AxlRpm = hb.getUserMotAxl();
- // hb.setMotVal(R_L,AxlRpm);
- // hb.setMotVal(R_R,AxlRpm);
break;
case TAKE_OFF:
if(gf_StateEnt){
@@ -250,11 +241,6 @@
// }
//else
hb.chkSW(TAKE_OFF);
- // AxlRpm = hb.getUserMotAxl();
- // hb.setMotVal(R_L,AxlRpm);
- // hb.setMotVal(R_R,AxlRpm);
- // hb.setMotValOfs(R_L,AxlRpm);
- // hb.setMotValOfs(R_R,AxlRpm);
break;
case GROUND:
if(gf_StateEnt){