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@24:c5945aaae777, 2018-12-10 (annotated)

Committer:
MasashiNomura
Date:
Mon Dec 10 12:29:37 2018 +0000
Revision:
24:c5945aaae777
Parent:
19:4b0fe9a5ec38
Child:
31:56c554c560c1
2018/12/10  dbg etc

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 19:4b0fe9a5ec38 38 //=========================================================
takeru0x1103 19:4b0fe9a5ec38 39 //パラメータゲッター
takeru0x1103 19:4b0fe9a5ec38 40 //=========================================================
takeru0x1103 18:5aa48aec9cae 41 float HbAttitude::getPp(){return p;}
takeru0x1103 18:5aa48aec9cae 42 float HbAttitude::getP() {return kp;}
takeru0x1103 18:5aa48aec9cae 43 float HbAttitude::getI() {return ki;}
takeru0x1103 18:5aa48aec9cae 44 float HbAttitude::getD() {return kd;}
takeru0x1103 18:5aa48aec9cae 45
takeru0x1103 19:4b0fe9a5ec38 46 //=========================================================
takeru0x1103 19:4b0fe9a5ec38 47 //パラメータセッター
takeru0x1103 19:4b0fe9a5ec38 48 //=========================================================
takeru0x1103 18:5aa48aec9cae 49 void HbAttitude::setPp(float iPp){p = iPp;}
takeru0x1103 18:5aa48aec9cae 50 void HbAttitude::setP(float iP) {kp= iP;}
takeru0x1103 18:5aa48aec9cae 51 void HbAttitude::setI(float iI) {ki= iI;}
takeru0x1103 18:5aa48aec9cae 52 void HbAttitude::setD(float iD) {kd= iD;}
MasashiNomura 24:c5945aaae777 53 void HbAttitude::setIMax(float val){limitH = val;}
MasashiNomura 24:c5945aaae777 54 void HbAttitude::setIMin(float val){limitL = val;}
takeru0x1103 18:5aa48aec9cae 55
takeru0x1103 17:f9610f3cfa1b 56 //=========================================================
takeru0x1103 17:f9610f3cfa1b 57 //コンストラクタ
takeru0x1103 17:f9610f3cfa1b 58 //=========================================================
takeru0x1103 17:f9610f3cfa1b 59 HbAttitude::HbAttitude(float iPo , float iP , float iI , float iD){
takeru0x1103 17:f9610f3cfa1b 60 //パラメータ初期化
takeru0x1103 19:4b0fe9a5ec38 61 p =iPo ;//アウターループP制御系数
takeru0x1103 19:4b0fe9a5ec38 62 kp =iP ;//インナーループP制御系数
takeru0x1103 19:4b0fe9a5ec38 63 ki =iI ;//インナーループI制御系数
takeru0x1103 19:4b0fe9a5ec38 64 kd =iD ;//インナーループD制御系数
takeru0x1103 18:5aa48aec9cae 65 limitH=2000 ;//積分上限
takeru0x1103 18:5aa48aec9cae 66 limitL=-2000;//
takeru0x1103 19:4b0fe9a5ec38 67 sum =0 ;//積分値
takeru0x1103 19:4b0fe9a5ec38 68 old =0 ;//1サンプル前のデータ(微分用)
takeru0x1103 17:f9610f3cfa1b 69 }
takeru0x1103 17:f9610f3cfa1b 70