teamALI
/
HB2018
ジャイロ追加前
HbManager.cpp
- Committer:
- MasashiNomura
- Date:
- 2019-03-11
- Revision:
- 64:40943c843da7
- Parent:
- 62:4b873328d3a8
- Parent:
- 63:aee44afe6363
- Child:
- 66:0b3b290137b6
File content as of revision 64:40943c843da7:
#include "HbManager.h" #include "globalFlags.h" #include "uart.h" #include "fpga.h" UINT16 HbManager::proofOfSurvival() { return fpgaTest(1); } //------------------------------------------------------ //姿勢状態取得 //------------------------------------------------------ void HbManager::getAttitude(){ float tmpf; INT16 tmpi; //姿勢状態取得 nowAngle = imu->GetYaw(); nowRate = imu->GetGyroZ(); if(gf_Cal.bf.yaw==true && gf_Cal.bf.gy==true){ nowTrgtAng = 0; } //ヨー角キャリブレーション if(gf_Cal.bf.yaw==true){ imu->CalYaw(); gf_Cal.bf.yaw=false; sp.printf("Yaw caribration %f\t",nowAngle); nowAngle=0; } //ジャイロオフセットキャリブレーション if(gf_Cal.bf.gy==true){ imu->CalGyro(); gf_Cal.bf.gy=false; sp.printf("Gyro caribration %d\t",nowRate); nowRate=0; } if(gf_Print.d1.bf.yaw) { tmpf = imu->GetYawRef(); sp.printf("Ang\t%f ref %f\t",nowAngle,tmpf); } if(gf_Print.d1.bf.gy) { tmpi = imu->GetGyroRef(); sp.printf("Gyr\t%d ref %d\t",nowRate,tmpi); } for(int i = 0; i < 4; ++i){ if(gf_MtAttOfs[i].req){ motorOfsVal[i] = gf_MtAttOfs[i].val; gf_MtAttOfs[i].req = false; } } if(gf_MtBrk.req){ mot_brk = gf_MtBrk.val; gf_MtBrk.req = false; sp.printf("Mot Brk:[%d]",mot_brk); } if(gf_AngBrk.req){ ang_rate_brk = gf_AngBrk.val / static_cast<float>(UPDATE_RATE); gf_AngBrk.req = false; sp.printf("Ang Rate Brk:[%f]",ang_rate_brk); } if(gf_MtDefaultOffset.req){ mot_takeoff_offset = gf_MtDefaultOffset.val; gf_MtDefaultOffset.req = false; sp.printf("take off Offset:[%d]",mot_takeoff_offset); } } void HbManager::calAtt(){ if(att == NULL) return; if(imu == NULL) return; imu->CalYaw(); sp.printf("Yaw auto caribration %f\t",nowAngle); nowAngle=0; imu->CalGyro(); sp.printf("Gyro auto caribration %d\t\r\n",nowRate); nowRate=0; nowTrgtAng = 0; } //------------------------------------------------------ //姿勢制御 //------------------------------------------------------ void HbManager::controlAttitude(){ float pidRtn; if(g_PidPara.mode == PID_0_OFF || g_PidPara.mode == PID_0_ON){ pidRtn = att->pid(nowTrgtAng,nowAngle,nowRate); } else { pidRtn = att->pid2(nowTrgtAng,nowAngle,nowRate); } if(att != NULL){ if(gf_Print.d1.bf.pp){sp.printf("PID:PP=%f,",att->getPp());} if(gf_Print.d1.bf.p) {sp.printf("PID:P=%f,",att->getP()); } if(gf_Print.d1.bf.i) {sp.printf("PID:I=%f,",att->getI()); } if(gf_Print.d1.bf.d) {sp.printf("PID:D=%f,",att->getD()); } if(gf_Print.d1.bf.v) {sp.printf("PID:V=%f,",att->getV()); } if(gf_Print.d1.bf.fb){ sp.printf("PID FB: %f, %f, %f, %f,", pidRtn, nowTrgtAng, nowAngle, nowRate); } } float fl, fr, bl, br; if(gf_State == TAKE_OFF || gf_State == DRIVE || gf_State == HOVER){ if(g_PidPara.mode == PID_0_ON){ if(pidRtn < 0){ //fl = pidRtn - motorOfsVal[0]; fl = 0.0; fr = -pidRtn + motorOfsVal[1]; bl = -pidRtn + motorOfsVal[2]; //br = pidRtn - motorOfsVal[3]; br = 0.0; } else { fl = pidRtn + motorOfsVal[0]; //fr = -pidRtn - motorOfsVal[1]; //bl = -pidRtn - motorOfsVal[2]; fr = 0.0; bl = 0.0; br = pidRtn + motorOfsVal[3]; } } else if(g_PidPara.mode == PID_1_ON){ if(pidRtn < 0){ fl = -pidRtn * 2.25 + motorOfsVal[0]; fr = pidRtn * 2.25 - motorOfsVal[1]; bl = pidRtn * 2.25 - motorOfsVal[2]; br = -pidRtn * 2.25 + motorOfsVal[3]; } else { fl = -pidRtn * 2.25 - motorOfsVal[0]; fr = pidRtn * 2.25 + motorOfsVal[1]; bl = pidRtn * 2.25 + motorOfsVal[2]; br = -pidRtn * 2.25 - motorOfsVal[3]; } }else{ fl = 0; fr = 0; bl = 0; br = 0; } } else { fl = 0; fr = 0; bl = 0; br = 0; } gf_MtReq[0].val = (INT16)fl; gf_MtReq[1].val = (INT16)fr; gf_MtReq[2].val = (INT16)bl; gf_MtReq[3].val = (INT16)br; for(int i = 0; i < 4; ++i){ gf_MtReq[i].req = true; } } void HbManager::controlAttitude(float cmdAngle){ float pidRtn; pidRtn = att->pid2(cmdAngle,nowAngle,nowRate); if(gf_Print.d1.bf.pp){sp.printf("PID:P=%f,",att->getPp());} if(gf_Print.d1.bf.p) {sp.printf("PID:P=%f,",att->getP()); } if(gf_Print.d1.bf.i) {sp.printf("PID:I=%f,",att->getI()); } if(gf_Print.d1.bf.d) {sp.printf("PID:D=%f,",att->getD()); } if(gf_Print.d1.bf.v) {sp.printf("PID:V=%f,",att->getV()); } if(gf_Print.d1.bf.fb){ sp.printf("PID FB,%f,",pidRtn); sp.printf("%f,",nowAngle); sp.printf("%f,",nowRate); } //右回りはセンサ値はプラスで逆に戻そうとしてPIDの結果はマイナスで左回りのフィードバック(左前と右後ろを強く) // MAX RPM=9000 PWM MAX Val=4095 -> 2.25 float fl = pidRtn * 2.25 * -1; float fr = pidRtn * 2.25; float bl = pidRtn * 2.25; float br = pidRtn * 2.25 * -1; gf_MtReq[0].val = (INT16)fl; gf_MtReq[1].val = (INT16)fr; gf_MtReq[2].val = (INT16)bl; gf_MtReq[3].val = (INT16)br; for(int i = 0; i < 4; ++i){ gf_MtReq[i].req = true; } // float pidRtn; // pidRtn= att->pid(cmdAngle,nowAngle,nowRate); // // // if(gf_Print.d1.bf.pp){sp.printf("PID:P=%f,",att->getPp());} // if(gf_Print.d1.bf.p) {sp.printf("PID:P=%f,",att->getP()); } // if(gf_Print.d1.bf.i) {sp.printf("PID:I=%f,",att->getI()); } // if(gf_Print.d1.bf.d) {sp.printf("PID:D=%f,",att->getD()); } // if(gf_Print.d1.bf.fb){sp.printf("PID feedback=%f,",pidRtn);} // //右回りはセンサ値はプラスで逆に戻そうとしてPIDの結果はマイナスで左回りのフィードバック(左前と右後ろを強く) // float fl = pidRtn * -1; // float fr = pidRtn; // float bl = pidRtn; // float br = pidRtn * -1; // motorVal[0] = (INT16)fl;//前左 // motorVal[1] = (INT16)fr;//前右 // motorVal[2] = (INT16)bl;//後左 // motorVal[3] = (INT16)br;//後右 } void HbManager::setAttPara(typPidPara para){ if(att == NULL) return; att->setPp(para.PP); att->setP(para.P); att->setI(para.I); att->setD(para.D); att->setIMax(para.IMax); att->setIMin(para.IMin); att->setV(para.V); } //------------------------------------------------------ //モーター制御 //------------------------------------------------------ void HbManager::controlMotor(){ INT16 tmp,dif; // DEBUG用FPGAに直接指示用 INT16 pos; INT16 typ; if(gf_State == SLEEP){ for(int i = 0; i < 8; ++i){ pos = i / 2; typ = i % 2; tmp = subProp[pos]->getValueFpgaMot((eMotType)typ); if(gf_MtReqDct[i].req){ dif = tmp - (INT16)gf_MtReqDct[i].val; if(dif == 0){ subProp[pos]->setValueFpgaMot((eMotType)typ, (INT16)gf_MtReqDct[i].val); gf_MtReqDct[i].req = false; } else { if(1000 < dif){ tmp-=100; } else if(100 < dif){ tmp-= 10; } else if(100 >= dif && 10 < dif){ tmp-= 5; } else if ( 10 >= dif && 1 <= dif){ tmp-=1; } else if ( -1000 > dif){ tmp+=100; } else if ( -100 >= dif){ tmp+=10; } else if(-100 <= dif && -10 > dif){ tmp+= 5; } else if ( -10 <= dif && -1 >= dif){ tmp+=1; } subProp[pos]->setValueFpgaMot((eMotType)typ, tmp); } } else { subProp[pos]->setValueFpgaMot((eMotType)typ, tmp); } } } else { // オフセット分の回転数セット for(int i = 0; i < 4; ++i){ tmp = subProp[i]->getValue(OFS); if(gf_MtReqOfs[i].req){ dif = tmp - (INT16)gf_MtReqOfs[i].val; if(dif == 0){ subProp[i]->setValue(OFS, (INT16)gf_MtReqOfs[i].val); gf_MtReqOfs[i].req = false; } else { if(1000 < dif){ tmp-=100; } else if(100 < dif){ tmp-= 10; } else if(100 >= dif && 10 < dif){ tmp-= 5; } else if ( 10 >= dif && 1 <= dif){ tmp-=1; } else if ( -1000 > dif){ tmp+=100; } else if ( -100 >= dif){ tmp+=10; } else if(-100 <= dif && -10 > dif){ tmp+= 5; } else if ( -10 <= dif && -1 >= dif){ tmp+=1; } subProp[i]->setValue(OFS, tmp); } } else { subProp[i]->setValue(OFS, tmp); } } // 姿勢制御分の回転数セット for(int i = 0; i< 4;++i){ if(gf_MtReq[i].req){ subProp[i]->setValue(ATT,gf_MtReq[i].val); gf_MtReq[i].req = false; } else { tmp = subProp[i]->getValue(ATT); subProp[i]->setValue(ATT,tmp); } } // ユーザー操作分の回転数セット for(int i = 0; i < 4; ++i){ if(gf_MtReqU[i].req){ subProp[i]->setValue(USER,gf_MtReqU[i].val); gf_MtReqU[i].req = false; } else { tmp = subProp[i]->getValue(USER); subProp[i]->setValue(USER,tmp); } } for(int i = 0; i < 4; ++i){ subProp[i]->setValue(); } } if(gf_Print.d1.bf.m1){ sp.printf("m1=%d,",(INT16)subProp[0]->getValue(ATT)); } if(gf_Print.d1.bf.m2){ sp.printf("m2=%d,",(INT16)subProp[1]->getValue(ATT)); } if(gf_Print.d1.bf.m3){ sp.printf("m3=%d,",(INT16)subProp[2]->getValue(ATT)); } if(gf_Print.d1.bf.m4){ sp.printf("m4=%d,",(INT16)subProp[3]->getValue(ATT)); } } INT16 HbManager::getCurMotVal(eMotPos pos){ if(subProp[(int)pos] == NULL) return 0; return subProp[(int)pos]->getValue(USER); } void HbManager::setMotVal(eMotPos pos, INT16 val){ if(subProp[(int)pos] == NULL) return; subProp[(int)pos]->setValue(USER, val); } void HbManager::setMotValOfs(eMotPos pos, INT16 val){ if(subProp[(int)pos] == NULL) return; subProp[(int)pos]->setValue(OFS, val); } void HbManager::setMotValAtt(eMotPos pos, INT16 val){ if(subProp[(int)pos] == NULL) return; subProp[(int)pos]->setValue(ATT, val); } void HbManager::setMotTakeOffOffset(){ for(int i = 0; i < 4; ++i){ gf_MtReqOfs[i].req = true; gf_MtReqOfs[i].val = mot_takeoff_offset; } } void HbManager::setFpgaMot(eMotPos pos,eMotType type, INT16 val){ if(subProp[(int)pos] == NULL) return; subProp[(int)pos]->setValueFpgaMot(type, val); } // void HbManager::setMotFPGA(UCHAR num, INT16 val){ // //UCHAR tmpN = num / 2; // //UCHAR tmp = num % 2; // // mot[tmpN]->setValueFPGA(tmp, val); // // sp.printf("FPGA Direct NUM:[%d] [%d]\r\n",tmpN, tmp, val); // } //------------------------------------------------------ //エンジン制御 //------------------------------------------------------ void HbManager::getEngine(){ UINT16 rpm[2]; //エンジン回転数読み出し rpm[0] = eng[0]->getRpm(); rpm[1] = eng[1]->getRpm(); if(gf_Print.d1.bf.e1){ sp.printf("%d",rpm[0]); } if(gf_Print.d1.bf.e2){ sp.printf("%d,",rpm[1]); } if(gf_Print.d2.bf.ep1){sp.printf("set %d hover %d ", getAccelVal(FRONT), getHvAxl(FRONT));} if(gf_Print.d2.bf.ep2){sp.printf("set %d hover %d ", getAccelVal(REAR), getHvAxl(REAR));} if(gf_AxReqH[0].bf.req){ setHvAxl(FRONT,gf_AxReqH[0].bf.val); gf_AxReqH[0].bf.req = false; } if(gf_AxReqH[1].bf.req){ setHvAxl(REAR,gf_AxReqH[1].bf.val); gf_AxReqH[1].bf.req = false; } for(int i = 0; i < 2; ++i){ if(gf_AxStepReq[i].bf.req){ en_srv_step[i] = gf_AxStepReq[i].bf.val; gf_AxStepReq[i].bf.req = false; sp.printf("set Axl Step [%d] val[%d]",i+1, en_srv_step[i]); } if(gf_AxAdjStepReq[i].bf.req){ en_srv_adj_step[i] = gf_AxAdjStepReq[i].bf.val; gf_AxAdjStepReq[i].bf.req = false; sp.printf("set Axl Adjust Step [%d] val[%d]",i+1, en_srv_adj_step[i]); } } } void HbManager::controlEngine(){ int i; INT16 tmp, dif; INT16 step = 100; //アクセル設定 for(i=0; i<2; i++){ if(gf_AxReq[i].bf.req==true){ step = en_srv_step[i]; tmp = eng[i]->getAccel(); dif = tmp - (INT16)gf_AxReq[i].bf.val; if(dif == 0){ accelVal[i] = gf_AxReq[i].bf.val; eng[i]->setAccel(accelVal[i]); gf_AxReq[i].bf.req=false; //sp.printf("Servo Moving FINISH!!\r\n"); } else { if(dif > 0){ if(dif > step){ tmp -= step; } else { tmp -= dif; } } else { if( abs(dif) > step){ tmp += step; } else { tmp -= dif; } } eng[i]->setAccel(tmp); } //sp.printf("val=%d\r\n" , gf_AxReq[i].bf.val); } else {// 直前でセットされた値をセット eng[i]->setAccel(accelVal[i]); } } } void HbManager::controlEngine(enmHbState stat){ int i; INT16 tmp, dif; INT16 step = 100; //アクセル設定 if(stat == CHK_EG_F || stat == CHK_EG_R){ for(i=0; i<2; i++){ if(gf_AxReq[i].bf.req==true){ accelVal[i] = gf_AxReq[i].bf.val; eng[i]->setAccel(accelVal[i]); gf_AxReq[i].bf.req=false; //sp.printf("val=%d\r\n" , accelVal[i]); } else {// 直前でセットされた値をセット eng[i]->setAccel(accelVal[i]); } } }else { for(i=0; i<2; i++){ if(gf_AxReq[i].bf.req==true){ step = en_srv_step[i]; tmp = eng[i]->getAccel(); dif = tmp - (INT16)gf_AxReq[i].bf.val; if(dif == 0){ accelVal[i] = gf_AxReq[i].bf.val; eng[i]->setAccel(accelVal[i]); gf_AxReq[i].bf.req=false; //sp.printf("Servo Moving FINISH!!\r\n"); } else { if(dif > 0){ if(dif > step){ tmp -= step; } else { tmp -= dif; } } else { if( abs(dif) > step){ tmp += step; } else { tmp -= dif; } } eng[i]->setAccel(tmp); } //sp.printf("val=%d\r\n" , gf_AxReq[i].bf.val); } else {// 直前でセットされた値をセット eng[i]->setAccel(accelVal[i]); } } } } void HbManager::clearHvAxl(){ for(int i = 0; i < 2; ++i){ eng[i]->clearHoverAccel(); } } bool HbManager::chkSetHvAxl(eEgPos pos){ if(pos == FRONT){ return eng[0]->chkSetHoverAccel(); } else/* if(pos == REAR)*/{ return eng[1]->chkSetHoverAccel(); } } void HbManager::setHvAxl(eEgPos pos, INT16 val){ if(pos == FRONT){ eng[0]->setHoverAccel(val); } else/* if(pos == REAR)*/{ eng[1]->setHoverAccel(val); } } INT16 HbManager::getHvAxl(eEgPos pos){ if(pos == FRONT){ return eng[0]->getHoverAccelVal(); } else/* if(pos == REAR)*/{ return eng[1]->getHoverAccelVal(); } } void HbManager::setAccelVal(eEgPos pos, INT16 val){ if(pos == FRONT){ gf_AxReq[0].bf.req = true; gf_AxReq[0].bf.val = val > MAX_VAL_12BIT ? MAX_VAL_12BIT: (UINT16)val; } else { gf_AxReq[1].bf.req = true; gf_AxReq[1].bf.val = val > MAX_VAL_12BIT ? MAX_VAL_12BIT: (UINT16)val; } } INT16 HbManager::getAccelVal(eEgPos pos){ if(pos == FRONT){ return eng[0]->getAccel(); } else/* if(pos == REAR)*/{ return eng[1]->getAccel(); } } //------------------------------------------------------ //ステート遷移関連 //------------------------------------------------------ bool HbManager::chkOverIDLE(){ return eng[0]->chkOverIDLECycle() && eng[1]->chkOverIDLECycle(); } bool HbManager::chkInRangeIDLE(){ return eng[0]->chkInRangeIDLE() && eng[1]->chkInRangeIDLE(); } // ステート遷移関連end //------------------------------------------------------ //------------------------------------------------------ //ユーザー操作関連 //------------------------------------------------------ void HbManager::getUserCommand(){ usrSW = ope->GetUserOpe(); //sp.printf("SW Val=%X\r\n", usrSW.w); } float HbManager::getUserMotAxlRaw(){ return ope->GetAinAccelRaw(); } INT16 HbManager::getUserMotAxl(){ return ope->GetAinAccel(); } float HbManager::getUserEngTrottleRaw(){ return ope->GetAinThrottleRaw(); } INT16 HbManager::getUserEngTrottle(){ return ope->GetAinThrottle(); } bool HbManager::chkSWUserOpe(HbUserOpe::SW_TYPE stype){ return ope->ChkCtrlSW(stype); } bool HbManager::chkSWUserOpeRE(HbUserOpe::SW_TYPE stype){ return ope->ChkCtrlSwRiseEdge(stype); } bool HbManager::chkSWUserOpeAny(){ return ope->ChkCtrlSwAny(); } bool HbManager::chkSWUserOpeBoth(HbUserOpe::SW_TYPE stype1,HbUserOpe::SW_TYPE stype2){ return ope->ChkCtrlSwBoth(stype1,stype2); } typUserSw HbManager::getUserSw(){ return usrSW; } 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; 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[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)){ gf_MtReqU[1].req = true; gf_MtReqU[1].val = 0; gf_MtReqU[2].req = true; gf_MtReqU[2].val = 0; } // 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); // } // 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::FLT_ON)){ if(stat == IDLE){ gf_AxReq[0].bf.req = true; gf_AxReq[1].bf.req = true; gf_AxReq[0].bf.val = getHvAxl(FRONT) / 2; gf_AxReq[1].bf.val = getHvAxl(REAR) / 2; setStateF(UPPER_IDLE); } else if(stat == UPPER_IDLE){ gf_AxReq[0].bf.req = true; gf_AxReq[1].bf.req = true; gf_AxReq[0].bf.val = getHvAxl(FRONT); gf_AxReq[1].bf.val = getHvAxl(REAR); setStateF(TAKE_OFF); } sp.printf("FLT_ON Push \r\n"); } if(chkSWUserOpeRE(HbUserOpe::FLT_OFF)){ if(stat == TAKE_OFF || stat == HOVER || stat == DRIVE || stat == UPPER_IDLE || stat == IDLE){ for(int i = 0; i < 2; ++i){ if(stat == TAKE_OFF || stat == HOVER || stat == DRIVE){ setHvAxl((eEgPos)i, gf_AxReq[i].bf.val); } gf_AxReq[i].bf.req = true; gf_AxReq[i].bf.val = 0; } if(stat == DRIVE){ setStateF(EMGGND); } else { setStateF(GROUND); } } 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; INT16 step = en_srv_adj_step[0]; tmpAxl = getAccelVal(FRONT); tmpAxl+=step; if(tmpAxl > MAX_VAL_12BIT){ tmpAxl = MAX_VAL_12BIT; } gf_AxReq[0].bf.req = true; 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; INT16 step = en_srv_adj_step[0]; tmpAxl = getAccelVal(FRONT); tmpAxl-=step; if(tmpAxl < 0){ tmpAxl = 0; } gf_AxReq[0].bf.req = true; 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]; gf_BlinkLED = true; tmpAxl = getAccelVal(REAR); tmpAxl+=step; if(tmpAxl > MAX_VAL_12BIT){ tmpAxl = MAX_VAL_12BIT; } gf_AxReq[1].bf.req = true; 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]; gf_BlinkLED = true; tmpAxl = getAccelVal(REAR); tmpAxl-=step; if(tmpAxl < 0){ tmpAxl = 0; } gf_AxReq[1].bf.req = true; gf_AxReq[1].bf.val = tmpAxl; } } if(chkSWUserOpeRE(HbUserOpe::CTRL_SW)){ bool flg = false; switch (g_PidPara.mode) { case PID_0_OFF: g_PidPara.mode = PID_0_ON; calAtt(); flg = true; break; case PID_1_OFF: g_PidPara.mode = PID_1_ON; flg = true; break; case PID_0_ON: /* code */ g_PidPara.mode = PID_0_OFF; break; case PID_1_ON: g_PidPara.mode = PID_1_OFF; break; default: g_PidPara.mode = PID_0_OFF; break; } 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 //------------------------------------------------------ //====================================================== //コンストラクタ //====================================================== HbManager::HbManager(){ //メンバクラスインスタンス eng[0] = new HbEngine(0); eng[1] = new HbEngine(1); eng[0]->setHoverAccel(1500); eng[1]->setHoverAccel(1500); att = new HbAttitude(2.0 , 2.0 , 0 , 0);//パラメータ outP P I D accelVal[0] = 0; accelVal[1] = 0; for(int i = 0; i < 4; ++i){ motorOfsVal[i] = 0; } // mot[0] = new HbMotor(0); // mot[1] = new HbMotor(1); // mot[2] = new HbMotor(2); // mot[3] = new HbMotor(3); // subProp[0] = new HbSubProp(F_L); // subProp[0]->setCoef(IN, 0.00004, 0.0401, 29.262); // subProp[0]->setCoef(OUT,0.00004, 0.1336, -69.184); // subProp[1] = new HbSubProp(F_R); // subProp[1]->setCoef(IN, 0.00004, 0.0151, 70.947); // subProp[1]->setCoef(OUT,0.00004, 0.1336, -69.184); // subProp[2] = new HbSubProp(R_L); // subProp[2]->setCoef(IN, 0.00004, 0.0244, 53.954); // subProp[2]->setCoef(OUT,0.00003, 0.1579, -114.22); // subProp[3] = new HbSubProp(R_R); // subProp[3]->setCoef(IN, 0.00004, 0.0111, 82.044); // subProp[3]->setCoef(OUT,0.00003, 0.1617, -123.27); subProp[0] = new HbSubProp(F_L); subProp[0]->setCoef(IN, 0.00004, 0.0786, -5.248); subProp[0]->setCoef(OUT,0.00005, 0.0112, 104.45); subProp[1] = new HbSubProp(F_R); subProp[1]->setCoef(IN, 0.00005, 0.0095, 126.11); subProp[1]->setCoef(OUT,0.00004, 0.0798, -2.1167); subProp[2] = new HbSubProp(R_L); subProp[2]->setCoef(IN, 0.00004, 0.1039, -50.946); subProp[2]->setCoef(OUT,0.00004, 0.0886, -21.033); subProp[3] = new HbSubProp(R_R); subProp[3]->setCoef(IN, 0.00005, 0.0108, 105.15); subProp[3]->setCoef(OUT,0.00004, 0.0456, 50.611); imu = new Imu(p28,p27); nowTrgtAng = 0.0f; ope = new HbUserOpe(); for(int i = 0; i < 2; ++i){ 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); mot_takeoff_offset = DEF_MOT_OFFSET; // //初期化 // motorVal[0]=0; // motorVal[1]=0; // motorVal[2]=0; // motorVal[3]=0; }