File content as of revision 32:7f4145cc3551:

#include "HbAttitude.h"
#include "globalFlags.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;
}

//=========================================================
//PID制御+トルク
//=========================================================
float HbAttitude::pid2(float iCmdAng, float iCurAng , float iRate)
{
    //エラー量：指令値との差を求める
    float errAng = iCmdAng - iCurAng;
    //アウターループPpゲインを掛けて、角速度に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;}

//=========================================================
//パラメータセッター
//=========================================================
void HbAttitude::setPp(float iPp){p = iPp;}
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;}

//=========================================================
//コンストラクタ
//=========================================================
HbAttitude::HbAttitude(float iPo , float iP , float iI , float iD){
    //パラメータ初期化
    p     =iPo  ;//アウターループP制御系数
    kp    =iP   ;//インナーループP制御系数
    ki    =iI   ;//インナーループI制御系数
    kd    =iD   ;//インナーループD制御系数
    limitH=2000 ;//積分上限
    limitL=-2000;//
    sum   =0    ;//積分値
    old   =0    ;//1サンプル前のデータ(微分用)
    oAng = 0    ;//1サンプル前の角度
}