teamALI / Mbed 2 deprecated HB2018

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   mbed FreeRTOS

HbAttitude.cpp@18:5aa48aec9cae, 2018-12-01 (annotated)

Committer:
takeru0x1103
Date:
Sat Dec 01 14:03:08 2018 +0000
Revision:
18:5aa48aec9cae
Parent:
17:f9610f3cfa1b
Child:
19:4b0fe9a5ec38
??????????PID????????????

Who changed what in which revision?

UserRevisionLine numberNew contents of line
takeru0x1103 17:f9610f3cfa1b 1 #include "HbAttitude.h"
takeru0x1103 17:f9610f3cfa1b 2
takeru0x1103 17:f9610f3cfa1b 3 //=========================================================
takeru0x1103 17:f9610f3cfa1b 4 //PID制御
takeru0x1103 17:f9610f3cfa1b 5 //=========================================================
takeru0x1103 17:f9610f3cfa1b 6 float HbAttitude::pid(float iCmdAng, float iCurAng , float iRate)
takeru0x1103 17:f9610f3cfa1b 7 {
takeru0x1103 17:f9610f3cfa1b 8 //エラー量：指令値との差を求める
takeru0x1103 17:f9610f3cfa1b 9 float errAng = iCmdAng - iCurAng;
takeru0x1103 17:f9610f3cfa1b 10 //アウターループのPゲインを掛けて目標角速度とする
takeru0x1103 17:f9610f3cfa1b 11 float cmdRate= errAng * p;
takeru0x1103 17:f9610f3cfa1b 12
takeru0x1103 17:f9610f3cfa1b 13 //▼角速度偏差（指令値と現在値との差）
takeru0x1103 17:f9610f3cfa1b 14 float devRate = cmdRate - iRate ;
takeru0x1103 17:f9610f3cfa1b 15
takeru0x1103 17:f9610f3cfa1b 16 //▼比例項
takeru0x1103 17:f9610f3cfa1b 17 float clcP = devRate * kp;
takeru0x1103 17:f9610f3cfa1b 18
takeru0x1103 17:f9610f3cfa1b 19 //▼積分項
takeru0x1103 17:f9610f3cfa1b 20 float xi = ki * devRate; //係数をかける
takeru0x1103 17:f9610f3cfa1b 21 float tmpInteg = sum + xi;//積分して
takeru0x1103 17:f9610f3cfa1b 22
takeru0x1103 17:f9610f3cfa1b 23 //リミット掛ける
takeru0x1103 17:f9610f3cfa1b 24 if(tmpInteg > limitH){tmpInteg = limitH;}
takeru0x1103 17:f9610f3cfa1b 25 if(tmpInteg < limitL){tmpInteg = limitL;}
takeru0x1103 17:f9610f3cfa1b 26
takeru0x1103 17:f9610f3cfa1b 27 //積分値を次回計算用に保存
takeru0x1103 17:f9610f3cfa1b 28 sum = tmpInteg;
takeru0x1103 17:f9610f3cfa1b 29
takeru0x1103 17:f9610f3cfa1b 30 //▼微分項
takeru0x1103 17:f9610f3cfa1b 31 float clcD = kd * (devRate - old);
takeru0x1103 17:f9610f3cfa1b 32 //過去データ書き換え
takeru0x1103 17:f9610f3cfa1b 33 old = devRate;
takeru0x1103 18:5aa48aec9cae 34 //
takeru0x1103 17:f9610f3cfa1b 35 return clcP + tmpInteg + clcD;
takeru0x1103 17:f9610f3cfa1b 36 }
takeru0x1103 17:f9610f3cfa1b 37
takeru0x1103 18:5aa48aec9cae 38 //パラメータアクセス
takeru0x1103 18:5aa48aec9cae 39 float HbAttitude::getPp(){return p;}
takeru0x1103 18:5aa48aec9cae 40 float HbAttitude::getP() {return kp;}
takeru0x1103 18:5aa48aec9cae 41 float HbAttitude::getI() {return ki;}
takeru0x1103 18:5aa48aec9cae 42 float HbAttitude::getD() {return kd;}
takeru0x1103 18:5aa48aec9cae 43
takeru0x1103 18:5aa48aec9cae 44 void HbAttitude::setPp(float iPp){p = iPp;}
takeru0x1103 18:5aa48aec9cae 45 void HbAttitude::setP(float iP) {kp= iP;}
takeru0x1103 18:5aa48aec9cae 46 void HbAttitude::setI(float iI) {ki= iI;}
takeru0x1103 18:5aa48aec9cae 47 void HbAttitude::setD(float iD) {kd= iD;}
takeru0x1103 18:5aa48aec9cae 48
takeru0x1103 17:f9610f3cfa1b 49 //=========================================================
takeru0x1103 17:f9610f3cfa1b 50 //コンストラクタ
takeru0x1103 17:f9610f3cfa1b 51 //=========================================================
takeru0x1103 17:f9610f3cfa1b 52 HbAttitude::HbAttitude(float iPo , float iP , float iI , float iD){
takeru0x1103 17:f9610f3cfa1b 53 //パラメータ初期化
takeru0x1103 17:f9610f3cfa1b 54 p =iPo ;//アウターループP制御系数
takeru0x1103 17:f9610f3cfa1b 55 kp =iP ;//インナーループP制御系数
takeru0x1103 17:f9610f3cfa1b 56 ki =iI ;//インナーループI制御系数
takeru0x1103 17:f9610f3cfa1b 57 kd =iD ;//インナーループD制御系数
takeru0x1103 18:5aa48aec9cae 58 limitH=2000 ;//積分上限
takeru0x1103 18:5aa48aec9cae 59 limitL=-2000;//
takeru0x1103 17:f9610f3cfa1b 60 sum =0 ;//積分値
takeru0x1103 17:f9610f3cfa1b 61 old =0 ;//1サンプル前のデータ(微分用)
takeru0x1103 17:f9610f3cfa1b 62 }
takeru0x1103 17:f9610f3cfa1b 63