Diff: HbMotor.cpp

Revision:

29:eb3d72dd94aa

Parent:

28:fdb3b144e342

Child:

32:7f4145cc3551

--- a/HbMotor.cpp	Sat Dec 15 14:20:04 2018 +0000
+++ b/HbMotor.cpp	Mon Dec 17 13:25:00 2018 +0000
@@ -69,19 +69,22 @@
 HbMotCtrl::HbMotCtrl(eMotPos Pos, INT16 Type){
     pos = Pos;
     typ = Type;
-    for(int i = 0; i < 4; ++i){
-        ready.reset(i);
-    }
+    //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;
+    //curPwmVal = 0; 
+    //curRpmVal = 0;
 }
 //=========================================
 //デストラクタ
@@ -96,18 +99,24 @@
 void HbMotCtrl::setPwmLimt(INT16 Min, INT16 Max){
     pwmMax = Max;
     pwmMin = Min;
-    ready.set(0);
+    //ready.set(0);
 }
 void HbMotCtrl::setRpmLimt(INT16 Min, INT16 Max){
     rpmMax = Max;
     rpmMin = Min;
-    ready.set(1);
+    //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);
+    //ready.set(2);
 }
 //void HbMotCtrl::makeTable(){
     // 0は0rpm,1%は1500rpm(minimum)....100%はMaxrpm
@@ -132,50 +141,118 @@
     if(percent == 0){
         tmpPwm = 0;
     } else {
-        tmpRpm = rpmMin + (int)((rpmStep * (percent-1)) + 0.5);
+        //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;
 }
 
-void HbMotCtrl::setValue(float value){
-    int iVal = 0;
-    if(value + 0.5 > 49){
-        iVal = 49;
+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 + 0.5;
+        iVal = value;
     }
     val = iVal;
     iVal+=ofs;
     UCHAR uPos = pos * 2 + typ;
     
-    fpgaMotor(uPos, calcPwm(iVal));
+    fpgaMotor(uPos, calcPwm((UINT16)iVal));
 }
-void HbMotCtrl::setOfs(float value){
+void HbMotCtrl::setRpmOfs(INT16 value){
     int iVal = 0;
-    if(value + 0.5 > 49){
-        iVal = 49;
+    if(value + val > rpmMax){
+        iVal = rpmMax - val;
     }
     else if (value < 0){
         iVal = 0;
     }
     else{
-        iVal = value + 0.5;
+        iVal = value;
     }
     ofs = iVal;
     iVal+=val;
     UCHAR uPos = pos * 2 + typ;
-    fpgaMotor(uPos, calcPwm(iVal));
+    fpgaMotor(uPos, calcPwm((UINT16)iVal));
 }
-float HbMotCtrl::getValue(){
+INT16 HbMotCtrl::getRpmValue(){
     return val;
 }
-float HbMotCtrl::getOfs(){
+INT16 HbMotCtrl::getRpmOfs(){
     return ofs;
 }
 
@@ -213,6 +290,22 @@
     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;
@@ -229,24 +322,24 @@
 //    motCtrl[0]->makeTable();
 //    motCtrl[1]->makeTable();
 //}
-void HbSubProp::setValue(float val){
+void HbSubProp::setValue(INT16 val){
     if(motCtrl[0] == NULL || motCtrl[1] == NULL) return;
-    motCtrl[0]->setValue(val);
-    motCtrl[1]->setValue(val);
+    motCtrl[0]->setRpmValue(val);
+    motCtrl[1]->setRpmValue(val);
 }
-void HbSubProp::setOfs(float val){
+void HbSubProp::setOfs(INT16 val){
     if(motCtrl[0] == NULL || motCtrl[1] == NULL) return;
-    motCtrl[0]->setOfs(val);
-    motCtrl[1]->setOfs(val);
+    motCtrl[0]->setRpmOfs(val);
+    motCtrl[1]->setRpmOfs(val);
 }
-float HbSubProp::getValue(){
+INT16 HbSubProp::getValue(){
     if(motCtrl[0] == NULL || motCtrl[1] == NULL) return 0;
     // 同じはずなのでINのみ
-    return motCtrl[0]->getValue();
+    return motCtrl[0]->getRpmValue();
 }
-float HbSubProp::getOfs(){
+INT16 HbSubProp::getOfs(){
     if(motCtrl[0] == NULL || motCtrl[1] == NULL) return 0;
     // 同じはずなのでINのみ
-    return motCtrl[0]->getOfs();
+    return motCtrl[0]->getRpmOfs();
 }