Diff: HbManager.cpp

Revision:

46:5074781a28dd

Parent:

45:3b51dd26b579

Child:

47:d3fa874f336e

--- a/HbManager.cpp	Wed Feb 13 11:55:39 2019 +0000
+++ b/HbManager.cpp	Tue Feb 19 11:52:49 2019 +0000
@@ -70,25 +70,39 @@
         sp.printf("PID FB,%f,",pidRtn);
         sp.printf("%f,",nowAngle);
         sp.printf("%f,",nowRate);
-        }
+    }
     
     //右回りはセンサ値はプラスで逆に戻そうとしてPIDの結果はマイナスで左回りのフィードバック(左前と右後ろを強く)
-    // float fl = pidRtn * -1;
-    // float fr = pidRtn;
-    // float bl = pidRtn;
-    // float br = pidRtn * -1;
-    // MAX RPM=9000 PWM MAX Val=4095 -> 2.25
+    // 現在、望む角速度から正確なプロペラ回転数に変換する方法を持たないため、得られた値から決まった係数で回転数に変換
+    // // float fl = pidRtn * -1;
+    // // float fr = pidRtn;
+    // // float bl = pidRtn;
+    // // float br = pidRtn * -1;
+    // // MAX RPM=9000 PWM MAX Val=4095 -> 2.25
+    // float fl = pidRtn * 2.25 * -1;
+    // float fr = pidRtn * 2.25;
+    // float bl = pidRtn * 2.25;
+    // float br = pidRtn * 2.25 * -1;
+
+    // 推力は回転数の二乗に比例するので、得られた望む角速度の平方根に係数かけて回転数とする
+    float tmpRtn = abs(pidRtn);
+    tmpRtn = sqrtf(tmpRtn);
+    if(pidRtn < 0){
+        tmpRtn *= -1;
+    }
+    pidRtn = tmpRtn;
+    
     float fl = pidRtn * 2.25 * -1;
     float fr = pidRtn * 2.25;
     float bl = pidRtn * 2.25;
     float br = pidRtn * 2.25 * -1;
-    for(int i = 0; i < 4; ++i){
-        gf_MtReq[i].req = true;
-    }
     gf_MtReq[0].val = (INT16)fl;
     gf_MtReq[1].val = (INT16)fr;
     gf_MtReq[2].val = (INT16)bl;
     gf_MtReq[3].val = (INT16)br;
+    for(int i = 0; i < 4; ++i){
+        gf_MtReq[i].req = true;
+    }
 }
 
 void HbManager::controlAttitude(float cmdAngle){
@@ -113,13 +127,13 @@
     float fr = pidRtn * 2.25;
     float bl = pidRtn * 2.25;
     float br = pidRtn * 2.25 * -1;
-    for(int i = 0; i < 4; ++i){
-        gf_MtReq[i].req = true;
-    }
     gf_MtReq[0].val = (INT16)fl;
     gf_MtReq[1].val = (INT16)fr;
     gf_MtReq[2].val = (INT16)bl;
     gf_MtReq[3].val = (INT16)br;
+    for(int i = 0; i < 4; ++i){
+        gf_MtReq[i].req = true;
+    }
 
     // float pidRtn;
 
@@ -274,11 +288,17 @@
     if(subProp[(int)pos] == NULL) return 0;
     return subProp[(int)pos]->getValue(USER);
 }
+
 void HbManager::setMotVal(eMotPos pos, INT16 val){
     if(subProp[(int)pos] == NULL) return;
     subProp[(int)pos]->setValue(USER, val);
 }
 
+void HbManager::setMotValOfs(eMotPos pos, INT16 val){
+    if(subProp[(int)pos] == NULL) return;
+    subProp[(int)pos]->setValue(OFS, val);
+}
+
 void HbManager::setFpgaMot(eMotPos pos,eMotType type, INT16 val){
     if(subProp[(int)pos] == NULL) return;
     subProp[(int)pos]->setValueFpgaMot(type, val);
@@ -368,6 +388,68 @@
         }
     }
 }
+
+void HbManager::controlEngine(enmHbState stat){
+    int     i;
+    INT16 tmp, dif;
+    INT16 step = 100;
+    //アクセル設定
+    if(stat == CHK_EG_F || stat == CHK_EG_R){
+        for(i=0; i<2; i++){
+            if(gf_AxReq[i].bf.req==true){
+                accelVal[i] = gf_AxReq[i].bf.val;
+                eng[i]->setAccel(accelVal[i]);
+                gf_AxReq[i].bf.req=false;
+                //sp.printf("val=%d\r\n" , accelVal[i]);
+            }
+            else
+            {// 直前でセットされた値をセット
+                eng[i]->setAccel(accelVal[i]);
+            }
+        }
+    }else {
+        for(i=0; i<2; i++){
+            if(gf_AxReq[i].bf.req==true){
+                step = en_srv_step[i];
+                tmp = eng[i]->getAccel();
+                dif = tmp - (INT16)gf_AxReq[i].bf.val;
+                if(dif == 0){
+                    accelVal[i] = gf_AxReq[i].bf.val;
+                    eng[i]->setAccel(accelVal[i]);
+                    gf_AxReq[i].bf.req=false;
+                    //sp.printf("Servo Moving FINISH!!\r\n");
+                }
+                else {
+                    if(dif > 0){
+                        if(dif > step){
+                            tmp -= step;
+                        }
+                        else 
+                        {
+                            tmp -= dif;
+                        }
+                    }
+                    else {
+                        if( abs(dif) > step){
+                            tmp += step;
+                        }
+                        else
+                        {
+                            tmp -= dif;
+                        }
+                    }
+                    eng[i]->setAccel(tmp);
+                }
+                //sp.printf("val=%d\r\n" , gf_AxReq[i].bf.val);
+            }
+            else
+            {// 直前でセットされた値をセット
+                eng[i]->setAccel(accelVal[i]);
+            }
+        }
+    }
+}
+
 void HbManager::clearHvAxl(){
     for(int i = 0; i < 2; ++i){
         eng[i]->clearHoverAccel();
@@ -574,7 +656,7 @@
     if(chkSWUserOpeRE(HbUserOpe::F_ENG_UP)){
         if(stat == TAKE_OFF || stat == HOVER || stat == DRIVE){
             gf_BlinkLED = true;
-            INT16 step = 5;
+            INT16 step = 20;
             tmpAxl = getAccelVal(FRONT);
             tmpAxl+=step;
             if(tmpAxl > MAX_VAL_12BIT){
@@ -588,7 +670,7 @@
     if(chkSWUserOpeRE(HbUserOpe::F_ENG_DOWN)){
         if(stat == TAKE_OFF || stat == HOVER || stat == DRIVE){
             gf_BlinkLED = true;
-            INT16 step = 5;
+            INT16 step = 20;
             tmpAxl = getAccelVal(FRONT);
             tmpAxl-=step;
             if(tmpAxl < 0){
@@ -601,7 +683,7 @@
     //if(chkSWUserOpe(HbUserOpe::R_ENG_UP)){
     if(chkSWUserOpeRE(HbUserOpe::R_ENG_UP)){
         if(stat == TAKE_OFF || stat == HOVER || stat == DRIVE){
-            INT16 step = 5;
+            INT16 step = 20;
             gf_BlinkLED = true;
             tmpAxl = getAccelVal(REAR);
             tmpAxl+=step;
@@ -615,7 +697,7 @@
     //if(chkSWUserOpe(HbUserOpe::R_ENG_DOWN)){
     if(chkSWUserOpeRE(HbUserOpe::R_ENG_DOWN)){
         if(stat == TAKE_OFF || stat == HOVER || stat == DRIVE){
-            INT16 step = 5;
+            INT16 step = 20;
             gf_BlinkLED = true;
             tmpAxl = getAccelVal(REAR);
             tmpAxl-=step;