Diff: HbAttitude.cpp

Revision:

48:71aec693a7dc

Parent:

47:d3fa874f336e

Child:

52:33fa8060dd8c

--- a/HbAttitude.cpp	Wed Feb 20 12:54:25 2019 +0000
+++ b/HbAttitude.cpp	Sun Feb 24 10:33:34 2019 +0000
@@ -6,34 +6,62 @@
 //=========================================================
 float HbAttitude::pid(float iCmdAng, float iCurAng , float iRate)
 {
-    //エラー量：指令値との差を求める
-    float errAng = iCmdAng - iCurAng;
-    //アウターループのPゲインを掛けて目標角速度とする
-    float cmdRate= errAng * p;
+    // //エラー量：指令値との差を求める
+    // float errAng = iCmdAng - iCurAng;
+    // //アウターループのPゲインを掛けて目標角速度とする
+    // float cmdRate= errAng * p;
+    
+    // //▼角速度偏差（指令値と現在値との差）
+    // float devRate = cmdRate - iRate ;
+
+    // //▼比例項
+    // float clcP = devRate * kp; 
     
-    //▼角速度偏差（指令値と現在値との差）
-    float devRate = cmdRate - iRate ;
+    // //▼積分項
+    // float xi = ki * devRate;  //係数をかける
+    // float tmpInteg = sum + xi;//積分して
+
+    // //リミット掛ける
+    // if(tmpInteg > limitH){tmpInteg = limitH;}
+    // if(tmpInteg < limitL){tmpInteg = limitL;}
 
+    // //積分値を次回計算用に保存
+    // sum = tmpInteg;
+    
+    // //▼微分項
+    // float clcD = kd * (devRate - old);
+    // //過去データ書き換え
+    // old = devRate;
+    // //
+    // return  clcP + tmpInteg + clcD;
+
+    //エラー量：FeedBack量のためact-ref
+    float errAng = iCurAng - iCmdAng;
     //▼比例項
-    float clcP = devRate * kp; 
-    
+    float clcP= errAng * kp;
+
     //▼積分項
-    float xi = ki * devRate;  //係数をかける
-    float tmpInteg = sum + xi;//積分して
+    float delta_t = 1 / UPDATE_RATE; ////50HzとしているがUpdateRateのdefineからもってくるべき
+    float xi = ki * delta_t * (errAng + old) / 2;  //係数をかける
+    sum = sum + xi;//積分して
 
     //リミット掛ける
-    if(tmpInteg > limitH){tmpInteg = limitH;}
-    if(tmpInteg < limitL){tmpInteg = limitL;}
+    if(sum > limitH){sum = limitH;}
+    if(sum < limitL){sum = limitL;}
+
+    //▼微分項
+    float clcD = kd * (iRate);
+    //エラー量保存
+    old =errAng;
 
-    //積分値を次回計算用に保存
-    sum = tmpInteg;
-    
-    //▼微分項
-    float clcD = kd * (devRate - old);
-    //過去データ書き換え
-    old = devRate;
-    //
-    return  clcP + tmpInteg + clcD;
+    //位置に対するFB量=回転モーメント=回転数2乗の比例量として、回転数を求める
+    float calc_sum = clcP + sum + clcD;
+    float fb_value = 0.0;
+    if(0 < calc_sum){fb_value = sqrtf(calc_sum);}
+    else{fb_value = -sqrtf(-calc_sum);}
+
+    //元のプログラムと正負逆転していることに注意
+    return  fb_value;
 }
 
 //=========================================================
@@ -42,6 +70,7 @@
 float HbAttitude::pid2(float iCmdAng, float iCurAng , float iRate)
 {
     //エラー量：指令値との差を求める
+    // FeedBack量のためact-ref
     float errAng = iCmdAng - iCurAng;
     //アウターループPpゲインを掛けて、角速度にkvゲインをかけて、目標角速度とする
     float AngVelo = (iCurAng - oAng) * 50;//サンプリング周期をかけて、角速度(deg/s)に