File content as of revision 29:eb3d72dd94aa:

#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);
    //}
    val = 0;
    ofs = 0;       
    pwmMin = 0;    
    pwmMax = 0;     
    rpmMin = 0;
    rpmMax = 0;
    rpmGain = 1;
    rpmOfs = 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::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;
    //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);
        tmpRpm = rpmGain * (rpmMin + (int)((rpmStep * (percent-1)) + 0.5)) +rpmOfs;
        if(tmpRpm > rpmMax) tmpRpm = rpmMax;
        if(tmpRpm < 0) tmpRpm = 0;
        tmpPwm = (INT16)(coefA * tmpRpm * tmpRpm + coefB * tmpRpm + coefC);
    }
    
    return tmpPwm;
}

INT16 HbMotCtrl::calcPwm(UINT16 rpm)
{
    //float rpmStep = (rpmMax - rpmMin) / 99.0;
    int tmpRpm;
    INT16 tmpPwm;
    if(rpm == 0){
        tmpPwm = 0;
    } else {
        tmpRpm = rpmGain * rpm + rpmOfs;
        if(tmpRpm > rpmMax) {
            tmpRpm = rpmMax;
        }
        //else if(tmpRpm < rpmMin && tmpRpm > 0) {
        //    tmpRpm = rpmMin;
        //}
        else if( tmpRpm < 0){
            tmpRpm = 0;
        }
        tmpPwm = (INT16)(coefA * tmpRpm * tmpRpm + coefB * tmpRpm + coefC);
        if(tmpPwm < 0) tmpPwm = 0;
    }
    
    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;
//}

void HbMotCtrl::setRpmValue(INT16 value){
    INT16 iVal = 0;
    if(value + ofs > rpmMax){
        iVal = rpmMax - ofs;
    }
    else if (value < 0){
        iVal = 0;
    }
    else{
        iVal = value;
    }
    val = iVal;
    iVal+=ofs;
    UCHAR uPos = pos * 2 + typ;
    
    fpgaMotor(uPos, calcPwm((UINT16)iVal));
}
void HbMotCtrl::setRpmOfs(INT16 value){
    int iVal = 0;
    if(value + val > rpmMax){
        iVal = rpmMax - val;
    }
    else if (value < 0){
        iVal = 0;
    }
    else{
        iVal = value;
    }
    ofs = iVal;
    iVal+=val;
    UCHAR uPos = pos * 2 + typ;
    fpgaMotor(uPos, calcPwm((UINT16)iVal));
}
INT16 HbMotCtrl::getRpmValue(){
    return val;
}
INT16 HbMotCtrl::getRpmOfs(){
    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::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::makeTbl(){
//    if(motCtrl[0] == NULL) return;
//    if(motCtrl[1] == NULL) return;
//    motCtrl[0]->makeTable();
//    motCtrl[1]->makeTable();
//}
void HbSubProp::setValue(INT16 val){
    if(motCtrl[0] == NULL || motCtrl[1] == NULL) return;
    motCtrl[0]->setRpmValue(val);
    motCtrl[1]->setRpmValue(val);
}
void HbSubProp::setOfs(INT16 val){
    if(motCtrl[0] == NULL || motCtrl[1] == NULL) return;
    motCtrl[0]->setRpmOfs(val);
    motCtrl[1]->setRpmOfs(val);
}
INT16 HbSubProp::getValue(){
    if(motCtrl[0] == NULL || motCtrl[1] == NULL) return 0;
    // 同じはずなのでINのみ
    return motCtrl[0]->getRpmValue();
}
INT16 HbSubProp::getOfs(){
    if(motCtrl[0] == NULL || motCtrl[1] == NULL) return 0;
    // 同じはずなのでINのみ
    return motCtrl[0]->getRpmOfs();
}