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@17:f9610f3cfa1b, 2018-11-30 (annotated)

Committer:
takeru0x1103
Date:
Fri Nov 30 05:24:27 2018 +0000
Revision:
17:f9610f3cfa1b
Child:
18:5aa48aec9cae
1130

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 17:f9610f3cfa1b 34
takeru0x1103 17:f9610f3cfa1b 35 return clcP + tmpInteg + clcD;
takeru0x1103 17:f9610f3cfa1b 36 }
takeru0x1103 17:f9610f3cfa1b 37
takeru0x1103 17:f9610f3cfa1b 38 //=========================================================
takeru0x1103 17:f9610f3cfa1b 39 //コンストラクタ
takeru0x1103 17:f9610f3cfa1b 40 //=========================================================
takeru0x1103 17:f9610f3cfa1b 41 HbAttitude::HbAttitude(float iPo , float iP , float iI , float iD){
takeru0x1103 17:f9610f3cfa1b 42 //パラメータ初期化
takeru0x1103 17:f9610f3cfa1b 43 p =iPo ;//アウターループP制御系数
takeru0x1103 17:f9610f3cfa1b 44 kp =iP ;//インナーループP制御系数
takeru0x1103 17:f9610f3cfa1b 45 ki =iI ;//インナーループI制御系数
takeru0x1103 17:f9610f3cfa1b 46 kd =iD ;//インナーループD制御系数
takeru0x1103 17:f9610f3cfa1b 47 limitH=255 ;//積分上限
takeru0x1103 17:f9610f3cfa1b 48 limitL=0 ;//
takeru0x1103 17:f9610f3cfa1b 49 sum =0 ;//積分値
takeru0x1103 17:f9610f3cfa1b 50 old =0 ;//1サンプル前のデータ(微分用)
takeru0x1103 17:f9610f3cfa1b 51 }
takeru0x1103 17:f9610f3cfa1b 52