File content as of revision 28:fdb3b144e342:

#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;
    //sp.printf("MOT [%d]",curVal);//Test Code
    //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);
    }
    ofs = 0;       
    pwmMin = 0;    
    pwmMax = 0;     
    rpmMin = 0;
    rpmMax = 0;
    coefA = 0;     
    coefB = 0;     
    coefC = 0;     
    curPwmVal = 0; 
    curRpmVal = 0;
}
//=========================================
//デストラクタ
//=========================================
HbMotCtrl::~HbMotCtrl(){
}

bool HbMotCtrl::isReady()
{
    return true;
}
void HbMotCtrl::setPwmLimt(INT16 Min, INT16 Max){
    pwmMax = Max;
    pwmMin = Min;
    ready.set(0);
}
void HbMotCtrl::setRpmLimt(INT16 Min, INT16 Max){
    rpmMax = Max;
    rpmMin = Min;
    ready.set(1);
}
void HbMotCtrl::setMotCoef(float a, float b, float c){
    coefA = a;
    coefB = b;
    coefC = c;
    ready.set(2);
}
//void HbMotCtrl::makeTable(){
    // 0は0rpm,1%は1500rpm(minimum)....100%はMaxrpm
//    tblVal[0] = 0;
//    tblVal[99] = pwmMax;
//    float rpmStep = (rpmMax - rpmMin) / 99;
//    int tmpRpm;
//    int tmpPwm;
//    for(int i = 1; i < 99; ++i){
//        tmpRpm = rpmMin + (int)((rpmStep * (i-1)) + 0.5);
//        tmpPwm = (int)(coefA * tmpRpm * tmpRpm + coefB * tmpRpm + coefC);
//        tblVal[i] = tmpPwm;
//    }
//    ready.set(3);
//}

INT16 HbMotCtrl::calcPwm(INT16 percent)
{
    float rpmStep = (rpmMax - rpmMin) / 99.0;
    int tmpRpm;
    INT16 tmpPwm;
    if(percent == 0){
        tmpPwm = 0;
    } else {
        tmpRpm = rpmMin + (int)((rpmStep * (percent-1)) + 0.5);
        tmpPwm = (INT16)(coefA * tmpRpm * tmpRpm + coefB * tmpRpm + coefC);
    }
    
    return tmpPwm;
}

void HbMotCtrl::setValue(float value){
    int iVal = 0;
    if(value + 0.5 > 49){
        iVal = 49;
    }
    else if (value < 0){
        iVal = 0;
    }
    else{
        iVal = value + 0.5;
    }
    val = iVal;
    iVal+=ofs;
    UCHAR uPos = pos * 2 + typ;
    
    fpgaMotor(uPos, calcPwm(iVal));
}
void HbMotCtrl::setOfs(float value){
    int iVal = 0;
    if(value + 0.5 > 49){
        iVal = 49;
    }
    else if (value < 0){
        iVal = 0;
    }
    else{
        iVal = value + 0.5;
    }
    ofs = iVal;
    iVal+=val;
    UCHAR uPos = pos * 2 + typ;
    fpgaMotor(uPos, calcPwm(iVal));
}
float HbMotCtrl::getValue(){
    return val;
}
float HbMotCtrl::getOfs(){
    return ofs;
}

//=========================================
//コンストラクタ
//=========================================
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::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::makeTbl(){
//    if(motCtrl[0] == NULL) return;
//    if(motCtrl[1] == NULL) return;
//    motCtrl[0]->makeTable();
//    motCtrl[1]->makeTable();
//}
void HbSubProp::setValue(float val){
    if(motCtrl[0] == NULL || motCtrl[1] == NULL) return;
    motCtrl[0]->setValue(val);
    motCtrl[1]->setValue(val);
}
void HbSubProp::setOfs(float val){
    if(motCtrl[0] == NULL || motCtrl[1] == NULL) return;
    motCtrl[0]->setOfs(val);
    motCtrl[1]->setOfs(val);
}
float HbSubProp::getValue(){
    if(motCtrl[0] == NULL || motCtrl[1] == NULL) return 0;
    // 同じはずなのでINのみ
    return motCtrl[0]->getValue();
}
float HbSubProp::getOfs(){
    if(motCtrl[0] == NULL || motCtrl[1] == NULL) return 0;
    // 同じはずなのでINのみ
    return motCtrl[0]->getOfs();
}