Diff: HbMotor.cpp

Revision:

32:7f4145cc3551

Parent:

29:eb3d72dd94aa

Child:

40:debe99e228d3

--- a/HbMotor.cpp	Wed Dec 19 12:22:22 2018 +0000
+++ b/HbMotor.cpp	Thu Dec 20 11:38:09 2018 +0000
@@ -73,7 +73,8 @@
     //    ready.reset(i);
     //}
     val = 0;
-    ofs = 0;       
+    ofs = 0;
+    attval = 0;
     pwmMin = 0;    
     pwmMax = 0;     
     rpmMin = 0;
@@ -92,19 +93,13 @@
 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;
@@ -116,56 +111,19 @@
     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)
+INT16 HbMotCtrl::calcPwm(INT16 rpm)
 {
-    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){
+    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;
         }
@@ -176,85 +134,87 @@
     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;
+// 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(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;
-//}
+//     fpgaMotor(uPos, calcPwm((UINT16)iVal));
+// }
 
-void HbMotCtrl::setRpmValue(INT16 value){
+void HbMotCtrl::setRpmValue(eMotInType type, INT16 value){
     INT16 iVal = 0;
-    if(value + ofs > rpmMax){
-        iVal = rpmMax - ofs;
+    INT16 tmpVal;
+    if(type == OFS){
+        iVal = attval + val;
+    } else if(type == ATT){
+        iVal = val + ofs;
+    } else /*if(type == USER)*/{
+        iVal = attval + ofs;
     }
-    else if (value < 0){
-        iVal = 0;
+    if(value + iVal > rpmMax){
+        tmpVal = rpmMax - iVal;
+    }
+    else if (value < 0 && type != ATT){
+        tmpVal = 0;
     }
     else{
-        iVal = value;
+        tmpVal = value;
     }
-    val = iVal;
-    iVal+=ofs;
+    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;
     
-    fpgaMotor(uPos, calcPwm((UINT16)iVal));
+    fpgaMotor(uPos, calcPwm(iVal));
 }
-void HbMotCtrl::setRpmOfs(INT16 value){
-    int iVal = 0;
-    if(value + val > rpmMax){
-        iVal = rpmMax - val;
-    }
-    else if (value < 0){
-        iVal = 0;
+
+// 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(eMotInType type){
+    if(OFS == type){
+       return ofs;
+    } else if(ATT == type){
+       return attval;
+    } else /*if(eMotInType::USER == type)*/{
+        return val;
     }
-    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;
-}
+// INT16 HbMotCtrl::getRpmOfs(){
+//     return ofs;
+// }
 
 //=========================================
 //コンストラクタ
@@ -322,24 +282,24 @@
 //    motCtrl[0]->makeTable();
 //    motCtrl[1]->makeTable();
 //}
-void HbSubProp::setValue(INT16 val){
+void HbSubProp::setValue(eMotInType type, INT16 val){
     if(motCtrl[0] == NULL || motCtrl[1] == NULL) return;
-    motCtrl[0]->setRpmValue(val);
-    motCtrl[1]->setRpmValue(val);
+    motCtrl[0]->setRpmValue(type, val);
+    motCtrl[1]->setRpmValue(type, 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(){
+// void HbSubProp::setOfs(INT16 val){
+//     if(motCtrl[0] == NULL || motCtrl[1] == NULL) return;
+//     motCtrl[0]->setRpmOfs(val);
+//     motCtrl[1]->setRpmOfs(val);
+// }
+INT16 HbSubProp::getValue(eMotInType type){
     if(motCtrl[0] == NULL || motCtrl[1] == NULL) return 0;
     // 同じはずなのでINのみ
-    return motCtrl[0]->getRpmValue();
+    return motCtrl[0]->getRpmValue(type);
 }
-INT16 HbSubProp::getOfs(){
-    if(motCtrl[0] == NULL || motCtrl[1] == NULL) return 0;
-    // 同じはずなのでINのみ
-    return motCtrl[0]->getRpmOfs();
-}
+// INT16 HbSubProp::getOfs(){
+//     if(motCtrl[0] == NULL || motCtrl[1] == NULL) return 0;
+//     // 同じはずなのでINのみ
+//     return motCtrl[0]->getRpmOfs();
+// }