Diff: HbAttitude.cpp

Revision:

17:f9610f3cfa1b

Child:

18:5aa48aec9cae

diff -r 05b9e44889f1 -r f9610f3cfa1b HbAttitude.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/HbAttitude.cpp	Fri Nov 30 05:24:27 2018 +0000
@@ -0,0 +1,52 @@
+#include "HbAttitude.h"
+
+//=========================================================
+//PID制御
+//=========================================================
+float HbAttitude::pid(float iCmdAng, float iCurAng , float iRate)
+{
+    //エラー量：指令値との差を求める
+    float errAng = iCmdAng - iCurAng;
+    //アウターループのPゲインを掛けて目標角速度とする
+    float cmdRate= errAng * p;
+    
+    //▼角速度偏差（指令値と現在値との差）
+    float devRate = cmdRate - iRate ;
+
+    //▼比例項
+    float clcP = devRate * kp; 
+    
+    //▼積分項
+    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;
+}
+
+//=========================================================
+//コンストラクタ
+//=========================================================
+HbAttitude::HbAttitude(float iPo , float iP , float iI , float iD){
+    //パラメータ初期化
+    p     =iPo ;//アウターループP制御系数
+    kp    =iP  ;//インナーループP制御系数
+    ki    =iI  ;//インナーループI制御系数
+    kd    =iD  ;//インナーループD制御系数
+    limitH=255 ;//積分上限
+    limitL=0   ;//
+    sum   =0   ;//積分値
+    old   =0   ;//1サンプル前のデータ(微分用)
+}
+