File content as of revision 44:14fe4bd10fdb:

#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){
        if(gf_MtReqOfs[i].req){
            tmp = subProp[i]->getValue(OFS);
            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);
            }
        }
    }

    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;
        }
    }

    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(){
    UINT16  rpm[2];
    int     i;
    INT16 tmp, dif;
    // //エンジン回転数読み出し
    // 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]); }
    
    //INT16 step = 100;
    INT16 step = 30;
    //アクセル設定
    for(i=0; i<2; i++){
        step = en_srv_step[i];
        if(gf_AxReq[i].bf.req==true){
            tmp = eng[i]->getAccel();
            dif = tmp - (INT16)gf_AxReq[i].bf.val;
            if(dif == 0){
                eng[i]->setAccel((INT16)gf_AxReq[i].bf.val);
                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);
                // 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;
                // }
                // eng[i]->setAccel(tmp);
            }
            //sp.printf("val=%d\r\n" , gf_AxReq[i].bf.val);
            // accelVal[i] = gf_AxReq[i].bf.val;
            // eng[i]->setAccel(accelVal[i]);
            // gf_AxReq[i].bf.req=false;
        }
    }
}
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(stat == TAKE_OFF || stat == HOVER || stat == DRIVE){
            gf_BlinkLED = true;
            INT16 step = 3;
            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(stat == TAKE_OFF || stat == HOVER || stat == DRIVE){
            gf_BlinkLED = true;
            INT16 step = 3;
            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(stat == TAKE_OFF || stat == HOVER || stat == DRIVE){
            INT16 step = 3;
            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(stat == TAKE_OFF || stat == HOVER || stat == DRIVE){
            INT16 step = 3;
            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
//    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;
}