teamALI
/
HB2018
ジャイロ追加前
HbManager.cpp
- Committer:
- MasashiNomura
- Date:
- 2019-02-13
- Revision:
- 45:3b51dd26b579
- Parent:
- 44:14fe4bd10fdb
- Child:
- 46:5074781a28dd
File content as of revision 45:3b51dd26b579:
#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){ 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); } } 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; } //------------------------------------------------------ //姿勢制御 //------------------------------------------------------ void HbManager::controlAttitude(){ float pidRtn; pidRtn = att->pid2(0,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の結果はマイナスで左回りのフィードバック(左前と右後ろを強く) // float fl = pidRtn * -1; // float fr = pidRtn; // float bl = pidRtn; // float br = pidRtn * -1; // 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; for(int i = 0; i < 4; ++i){ gf_MtReq[i].req = true; } gf_MtReq[0].val = (INT16)fl; gf_MtReq[1].val = (INT16)fr; gf_MtReq[2].val = (INT16)bl; gf_MtReq[3].val = (INT16)br; } 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; for(int i = 0; i < 4; ++i){ gf_MtReq[i].req = true; } gf_MtReq[0].val = (INT16)fl; gf_MtReq[1].val = (INT16)fr; gf_MtReq[2].val = (INT16)bl; gf_MtReq[3].val = (INT16)br; // 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; 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(dif > 0){ // --tmp; //} //else{ // ++tmp; //} 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); } } 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)); } //DEBUG用FPGAに直接指示 INT16 pos; INT16 typ; if(gf_State == SLEEP){ for(int i = 0; i < 8; ++i){ pos = i / 2; typ = i % 2; if(gf_MtReqDct[i].req){ tmp = subProp[pos]->getValueFpgaMot((eMotType)typ); 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); } } } } } 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::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]; int i; //エンジン回転数読み出し 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; } } } 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::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(chkSWUserOpeRE(HbUserOpe::BRK_L)){ gf_MtReqU[0].req=true; tmpRpm = getCurMotVal(F_L); if(tmpRpm < BRK_RPM){ setMotVal(F_L, BRK_RPM); } } else if(chkSWUserOpe(HbUserOpe::BRK_L)){ if(gf_MtReqU[0].req==true){ setMotVal(F_L, BRK_RPM); } } 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(chkSWUserOpeRE(HbUserOpe::BRK_R)){ gf_MtReqU[1].req=true; tmpRpm = getCurMotVal(F_R); if(tmpRpm < BRK_RPM){ setMotVal(F_R, BRK_RPM); } } else if(chkSWUserOpe(HbUserOpe::BRK_R)){ if(gf_MtReqU[1].req==true){ setMotVal(F_R, BRK_RPM); } } 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(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); 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){ for(int i = 0; i < 2; ++i){ setHvAxl((eEgPos)i, gf_AxReq[i].bf.val); gf_AxReq[i].bf.req = true; gf_AxReq[i].bf.val = 0; } // gf_AxReq[0].bf.req = true; // gf_AxReq[1].bf.req = true; // gf_AxReq[0].bf.val = 0; // gf_AxReq[1].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 = 5; 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 = 5; 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 = 5; 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 = 5; 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(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 , 0 , 0 , 0);//パラメータ outP P I D accelVal[0] = 0; accelVal[1] = 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); ope = new HbUserOpe(); en_srv_step[0] = 100; en_srv_step[1] = 100; // //初期化 // motorVal[0]=0; // motorVal[1]=0; // motorVal[2]=0; // motorVal[3]=0; }