File content as of revision 60:8ed7b7f0c371:

#include "HbMotor.h"
#include "fpga.h"

//=========================================
//コンストラクタ
//=========================================
HbMotor::HbMotor(UCHAR iID ){
    id      =iID;
    ofs     =0;//
    limitH  =2000;
    limitL  =-2000;
    curVal = 0;
}

//-----------------------------------------
//モーター設定
//-----------------------------------------
void HbMotor::setValue(INT16 iVal){
    UINT16  val;
    
    //入力リミット
    if( iVal > limitH ){
        val = limitH;
    }else if( iVal < limitL ){
        val = limitL;
    }else{
        val = iVal;
    }
    
    //オフセット重畳
    curVal = val = val + ofs;
    //PWM幅設定
    fpgaSubProp(id,val);
}

void HbMotor::setOfs(INT16 iVal){
    ofs = iVal;
}

INT16 HbMotor::getOfs(){
    return ofs;
}

void HbMotor::setLimit(INT16 low, INT16 hi){
    limitL = low;
    limitH = hi;
}

INT16 HbMotor::getCurrentValue(){
    return curVal;
}

void HbMotor::setValueDirect(INT16 iVal){
    //PWM幅設定
    //sp.printf("MOT [%d]",iVal);//Test Code
    fpgaSubProp(id,iVal);
}

void  HbMotor::setValueFPGA(UCHAR iID, INT16 iVal){
    UCHAR tmp = id * 2 + iID;
    fpgaMotor(tmp,iVal);
}


//=========================================
//コンストラクタ
//=========================================
HbMotCtrl::HbMotCtrl(eMotPos Pos, INT16 Type){
    pos = Pos;
    typ = Type;
    //for(int i = 0; i < 4; ++i){
    //    ready.reset(i);
    //}
    val = 0;
    ofs = 0;
    attval = 0;
    pwmMin = 0;    
    pwmMax = 0;     
    rpmMin = 0;
    rpmMax = 0;
    rpmGain = 1;
    rpmOfs = 0;
    coefA = 0;     
    coefB = 0;     
    coefC = 0;     

    fpgaval = 0;
}
//=========================================
//デストラクタ
//=========================================
HbMotCtrl::~HbMotCtrl(){
}

void HbMotCtrl::setPwmLimt(INT16 Min, INT16 Max){
    pwmMax = Max;
    pwmMin = Min;
}
void HbMotCtrl::setRpmLimt(INT16 Min, INT16 Max){
    rpmMax = Max;
    rpmMin = Min;
}
void HbMotCtrl::setRpmOffset(INT16 ofs){
    rpmOfs = ofs;
}
void HbMotCtrl::setRpmGain(float gain){
    rpmGain = gain;
}
void HbMotCtrl::setMotCoef(float a, float b, float c){
    coefA = a;
    coefB = b;
    coefC = c;
}

INT16 HbMotCtrl::calcPwm(INT16 rpm)
{
    int tmpRpm;
    INT16 tmpPwm;
    if(rpm <= 0){
        tmpPwm = 0;
    } else {
        tmpRpm = rpmGain * rpm + rpmOfs;
        if(tmpRpm > rpmMax) {
            tmpRpm = rpmMax;
        }
        else if( tmpRpm < 0){
            tmpRpm = 0;
        }
        tmpPwm = (INT16)(coefA * tmpRpm * tmpRpm + coefB * tmpRpm + coefC);
        if(tmpPwm < 0) tmpPwm = 0;
    }
    fpgaval = tmpPwm;
    return fpgaval;
}

void HbMotCtrl::setRpmValue(eMotInType type, INT16 value){
    INT16 iVal = 0;
    INT16 tmpVal;
    if(type == OFS){
        iVal = attval + val;
    } else if(type == ATT){
        iVal = val + ofs;
    } else /*if(type == USER)*/{
        iVal = attval + ofs;
    }
    if(value + iVal > rpmMax){
        tmpVal = rpmMax - iVal;
    }
    else if (value < 0 && type != ATT){
        tmpVal = 0;
    }
    else{
        tmpVal = value;
    }
    if(OFS == type){
        ofs = tmpVal;
    } else if(ATT == type){
        attval = tmpVal;
    } else /*if(eMotInType::USER == type)*/
    {
        val = tmpVal;
    }

    // iVal = ofs + attval + val;
    // UCHAR uPos = pos * 2 + typ;

    // if(gf_Print.d2.bf.mrpm){
    //     sp.printf("POS[%d] Val[%d]  ", uPos, iVal);
    // }

    // fpgaMotor(uPos, calcPwm(iVal));
}

void HbMotCtrl::setRpmValue(){
    INT16 iVal = 0;
    iVal = static_cast<int>(sqrt((double)(ofs * ofs + attval * attval + val * val)));
    UCHAR uPos = pos * 2 + typ;

    if(gf_Print.d2.bf.mrpm){
        sp.printf("P%d V:%d  ", uPos, iVal);
    }

    fpgaMotor(uPos, calcPwm(iVal));
}
INT16 HbMotCtrl::getRpmValue(eMotInType type){
    if(OFS == type){
       return ofs;
    } else if(ATT == type){
       return attval;
    } else /*if(eMotInType::USER == type)*/{
        return val;
    }
}
void HbMotCtrl::setValFpga(INT16 value)
{
    UCHAR uPos = pos * 2 + typ;
    fpgaval = value;
    fpgaMotor(uPos, value);
}
INT16 HbMotCtrl::getValFpga()
{
    return fpgaval;
}
//=========================================
//コンストラクタ
//=========================================
HbSubProp::HbSubProp(eMotPos Pos){
    pos = Pos;
    motCtrl[0] = new HbMotCtrl(pos,0);
    motCtrl[0]->setPwmLimt(200, PWN_MAX);
    motCtrl[0]->setRpmLimt(RPM_MIN, RPM_IN_MAX);
    motCtrl[1] = new HbMotCtrl(pos,1);
    motCtrl[1]->setPwmLimt(200, PWN_MAX);
    motCtrl[1]->setRpmLimt(RPM_MIN, RPM_OUT_MAX);
}
//=========================================
//デストラクタ
//=========================================
HbSubProp::~HbSubProp(){
    delete motCtrl[0];
    delete motCtrl[1];
}

void HbSubProp::setRpmLimIn(INT16 min, INT16 max){
    if(motCtrl[0] == NULL) return;
    motCtrl[0]->setRpmLimt(min,max);
}
void HbSubProp::setRpmLimOut(INT16 min, INT16 max){
    if(motCtrl[1] == NULL) return;
    motCtrl[1]->setRpmLimt(min,max);
}
void HbSubProp::setPwmLimit(INT16 min, INT16 max){
    if(motCtrl[0] == NULL) return;
    motCtrl[0]->setPwmLimt(min, PWN_MAX);
    if(motCtrl[1] == NULL) return;
    motCtrl[1]->setPwmLimt(min, PWN_MAX);
}
void HbSubProp::setRpmGain(eMotType type, float gain){
    if(type == IN){
        motCtrl[0]->setRpmGain(gain);
    }
    else{
        motCtrl[1]->setRpmGain(gain);
    }
}
void HbSubProp::setRpmOffset(eMotType type, INT16 ofs){
    if(type == IN){
        motCtrl[0]->setRpmOffset(ofs);
    }
    else{
        motCtrl[1]->setRpmOffset(ofs);
    }
}
void HbSubProp::setCoef(eMotType type, float a, float b, float c){
    if(type == IN){
        if(motCtrl[0] == NULL)return;
        motCtrl[0]->setMotCoef(a,b,c);
    }
    else {
        if(motCtrl[1] == NULL)return;
        motCtrl[1]->setMotCoef(a,b,c);
    }
}

void HbSubProp::setValue(eMotInType type, INT16 val){
    if(motCtrl[0] == NULL || motCtrl[1] == NULL) return;
    motCtrl[0]->setRpmValue(type, val);
    motCtrl[1]->setRpmValue(type, val);
}

void HbSubProp::setValue(){
    if(motCtrl[0] == NULL || motCtrl[1] == NULL) return;
    motCtrl[0]->setRpmValue();
    motCtrl[1]->setRpmValue();
}

INT16 HbSubProp::getValue(eMotInType type){
    if(motCtrl[0] == NULL || motCtrl[1] == NULL) return 0;
    // 同じはずなのでINのみ
    return motCtrl[0]->getRpmValue(type);
}

void HbSubProp::setValueFpgaMot(eMotType type, INT16 val)
{
    if(motCtrl[0] == NULL || motCtrl[1] == NULL) return;
    if(type == IN){
        motCtrl[0]->setValFpga(val);
    }
    else
    {
        motCtrl[1]->setValFpga(val);
    }
}

INT16 HbSubProp::getValueFpgaMot(eMotType type)
{
    if(motCtrl[0] == NULL || motCtrl[1] == NULL) return 0;
    if(type == IN){
        return motCtrl[0]->getValFpga();
    }
    else
    {
        return motCtrl[1]->getValFpga();
    }
}