Diff: HbAttitude.cpp

Revision:

31:56c554c560c1

Parent:

24:c5945aaae777

Child:

32:7f4145cc3551

diff -r 13ada1a24c59 -r 56c554c560c1 HbAttitude.cpp
--- a/HbAttitude.cpp	Tue Dec 18 11:57:37 2018 +0000
+++ b/HbAttitude.cpp	Wed Dec 19 12:22:22 2018 +0000
@@ -1,4 +1,5 @@
 #include "HbAttitude.h"
+#include "globalFlags.h"
 
 //=========================================================
 //PID制御
@@ -36,12 +37,52 @@
 }
 
 //=========================================================
+//PID制御+トルク
+//=========================================================
+float HbAttitude::pid2(float iCmdAng, float iCurAng , float iRate)
+{
+    //エラー量：指令値との差を求める
+    float errAng = iCmdAng - iCurAng;
+    //アウターループKtゲインを掛けて、角速度にkvゲインをかけて、目標角速度とする
+    float AngVelo = (iCurAng - oAng) * 50;//サンプリング周期をかけて、角速度(deg/s)に
+    float cmdRate= errAng * p - AngVelo * kv;
+    
+    // //▼角速度偏差（指令値と現在値との差）
+    // 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;
+    oAng = iCurAng;
+    //
+    // return  clcP + tmpInteg + clcD;
+    return cmdRate;
+}
+
+//=========================================================
 //パラメータゲッター
 //=========================================================
 float HbAttitude::getPp(){return p;}
 float HbAttitude::getP() {return kp;}
 float HbAttitude::getI() {return ki;}
 float HbAttitude::getD() {return kd;}
+//float HbAttitude::getT() {return kt;}
+float HbAttitude::getV() {return kv;}
 
 //=========================================================
 //パラメータセッター
@@ -50,6 +91,8 @@
 void HbAttitude::setP(float iP)  {kp= iP;}
 void HbAttitude::setI(float iI)  {ki= iI;}
 void HbAttitude::setD(float iD)  {kd= iD;}
+//void HbAttitude::setT(float iT){kt = iT;}
+void HbAttitude::setV(float iV){kv = iV;}
 void HbAttitude::setIMax(float val){limitH = val;}
 void HbAttitude::setIMin(float val){limitL = val;}
 
@@ -66,5 +109,6 @@
     limitL=-2000;//
     sum   =0    ;//積分値
     old   =0    ;//1サンプル前のデータ(微分用)
+    oAng = 0    ;//1サンプル前の角度
 }