![](/media/cache/group/default_image.jpg.50x50_q85.jpg)
ジャイロ追加前
Diff: HbManager.cpp
- Revision:
- 48:71aec693a7dc
- Parent:
- 47:d3fa874f336e
- Child:
- 49:048966a8264a
--- a/HbManager.cpp Wed Feb 20 12:54:25 2019 +0000 +++ b/HbManager.cpp Sun Feb 24 10:33:34 2019 +0000 @@ -65,19 +65,22 @@ float pidRtn; - //pidRtn = att->pid2(0,nowAngle,nowRate); - pidRtn = att->pid2(nowTrgtAng,nowAngle,nowRate); + 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:P=%f,",att->getPp());} + 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,",pidRtn); - sp.printf("%f,",nowAngle); - sp.printf("%f,",nowRate); + sp.printf("PID FB:%f, %f, %f, ",pidRtn, nowAngle, nowRate); + //sp.printf("%f,",nowAngle); + //sp.printf("%f,",nowRate); } } @@ -93,18 +96,38 @@ // float bl = pidRtn * 2.25; // float br = pidRtn * 2.25 * -1; - // 推力は回転数の二乗に比例するので、得られた望む角速度の平方根に係数かけて回転数とする - float tmpRtn = abs(pidRtn); - tmpRtn = sqrtf(tmpRtn); - if(pidRtn < 0){ - tmpRtn *= -1; + // // 推力は回転数の二乗に比例するので、得られた望む角速度の平方根に係数かけて回転数とする + // float tmpRtn = abs(pidRtn); + // tmpRtn = sqrtf(tmpRtn); + // if(pidRtn < 0){ + // tmpRtn *= -1; + // } + // pidRtn = tmpRtn; + float fl, fr, bl, br; + //float fl = pidRtn * 2.25 * -1; + //float fr = pidRtn * 2.25; + //float bl = pidRtn * 2.25; + //float br = pidRtn * 2.25 * -1; + // float fl = pidRtn; + // float fr = -pidRtn; + // float bl = -pidRtn; + // float br = pidRtn; + if(g_PidPara.mode == PID_0_ON){ + fl = pidRtn; + fr = -pidRtn; + bl = -pidRtn; + br = pidRtn; + } else if(g_PidPara.mode == PID_1_ON){ + fl = pidRtn * 2.25 * -1; + fr = pidRtn * 2.25; + bl = pidRtn * 2.25; + br = pidRtn * 2.25 * -1; + }else{ + fl = 0; + fr = 0; + bl = 0; + br = 0; } - pidRtn = tmpRtn; - - 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; @@ -112,6 +135,9 @@ for(int i = 0; i < 4; ++i){ gf_MtReq[i].req = true; } + // if(gf_Print.d1.bf.fb){ + // sp.printf("REQ RPM FL:%d, FR:%d, RL:%d, RR:%d",gf_MtReq[0].val,gf_MtReq[1].val,gf_MtReq[2].val,gf_MtReq[3].val); + // } } void HbManager::controlAttitude(float cmdAngle){ @@ -242,6 +268,10 @@ } } + 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)); }