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@52:33fa8060dd8c, 2019-03-04 (annotated)

Committer:
YutakaTakagi
Date:
Mon Mar 04 07:55:48 2019 +0000
Revision:
52:33fa8060dd8c
Parent:
48:71aec693a7dc
Child:
67:ad35921e3cdc
Child:
70:a7b1e3eec51b
Update PID controll

Who changed what in which revision?

UserRevisionLine numberNew contents of line
takeru0x1103 17:f9610f3cfa1b 1 #include "HbAttitude.h"
MasashiNomura 31:56c554c560c1 2 #include "globalFlags.h"
takeru0x1103 17:f9610f3cfa1b 3
takeru0x1103 17:f9610f3cfa1b 4 //=========================================================
takeru0x1103 17:f9610f3cfa1b 5 //PID制御
takeru0x1103 17:f9610f3cfa1b 6 //=========================================================
takeru0x1103 17:f9610f3cfa1b 7 float HbAttitude::pid(float iCmdAng, float iCurAng , float iRate)
takeru0x1103 17:f9610f3cfa1b 8 {
MasashiNomura 48:71aec693a7dc 9 // //エラー量：指令値との差を求める
MasashiNomura 48:71aec693a7dc 10 // float errAng = iCmdAng - iCurAng;
MasashiNomura 48:71aec693a7dc 11 // //アウターループのPゲインを掛けて目標角速度とする
MasashiNomura 48:71aec693a7dc 12 // float cmdRate= errAng * p;
MasashiNomura 48:71aec693a7dc 13
MasashiNomura 48:71aec693a7dc 14 // //▼角速度偏差（指令値と現在値との差）
MasashiNomura 48:71aec693a7dc 15 // float devRate = cmdRate - iRate ;
MasashiNomura 48:71aec693a7dc 16
MasashiNomura 48:71aec693a7dc 17 // //▼比例項
MasashiNomura 48:71aec693a7dc 18 // float clcP = devRate * kp;
takeru0x1103 17:f9610f3cfa1b 19
MasashiNomura 48:71aec693a7dc 20 // //▼積分項
MasashiNomura 48:71aec693a7dc 21 // float xi = ki * devRate; //係数をかける
MasashiNomura 48:71aec693a7dc 22 // float tmpInteg = sum + xi;//積分して
MasashiNomura 48:71aec693a7dc 23
MasashiNomura 48:71aec693a7dc 24 // //リミット掛ける
MasashiNomura 48:71aec693a7dc 25 // if(tmpInteg > limitH){tmpInteg = limitH;}
MasashiNomura 48:71aec693a7dc 26 // if(tmpInteg < limitL){tmpInteg = limitL;}
takeru0x1103 17:f9610f3cfa1b 27
MasashiNomura 48:71aec693a7dc 28 // //積分値を次回計算用に保存
MasashiNomura 48:71aec693a7dc 29 // sum = tmpInteg;
MasashiNomura 48:71aec693a7dc 30
MasashiNomura 48:71aec693a7dc 31 // //▼微分項
MasashiNomura 48:71aec693a7dc 32 // float clcD = kd * (devRate - old);
MasashiNomura 48:71aec693a7dc 33 // //過去データ書き換え
MasashiNomura 48:71aec693a7dc 34 // old = devRate;
MasashiNomura 48:71aec693a7dc 35 // //
MasashiNomura 48:71aec693a7dc 36 // return clcP + tmpInteg + clcD;
MasashiNomura 48:71aec693a7dc 37
MasashiNomura 48:71aec693a7dc 38 //エラー量：FeedBack量のためact-ref
MasashiNomura 48:71aec693a7dc 39 float errAng = iCurAng - iCmdAng;
takeru0x1103 17:f9610f3cfa1b 40 //▼比例項
MasashiNomura 48:71aec693a7dc 41 float clcP= errAng * kp;
MasashiNomura 48:71aec693a7dc 42
takeru0x1103 17:f9610f3cfa1b 43 //▼積分項
YutakaTakagi 52:33fa8060dd8c 44 float delta_t = 1 / static_cast<float>(UPDATE_RATE); ////50HzとしているがUpdateRateのdefineからもってくるべき
YutakaTakagi 52:33fa8060dd8c 45 float xi = ki * delta_t * (errAng + old) / 2.0; //係数をかける
MasashiNomura 48:71aec693a7dc 46 sum = sum + xi;//積分して
takeru0x1103 17:f9610f3cfa1b 47
takeru0x1103 17:f9610f3cfa1b 48 //リミット掛ける
MasashiNomura 48:71aec693a7dc 49 if(sum > limitH){sum = limitH;}
MasashiNomura 48:71aec693a7dc 50 if(sum < limitL){sum = limitL;}
MasashiNomura 48:71aec693a7dc 51
MasashiNomura 48:71aec693a7dc 52 //▼微分項
YutakaTakagi 52:33fa8060dd8c 53 //float clcD = kd * (iRate);
YutakaTakagi 52:33fa8060dd8c 54 //iRateの値が怪しいので位置から速度を計算する
YutakaTakagi 52:33fa8060dd8c 55 float clcD = kd * (errAng - old) * UPDATE_RATE;
MasashiNomura 48:71aec693a7dc 56 //エラー量保存
MasashiNomura 48:71aec693a7dc 57 old =errAng;
takeru0x1103 17:f9610f3cfa1b 58
MasashiNomura 48:71aec693a7dc 59 //位置に対するFB量=回転モーメント=回転数2乗の比例量として、回転数を求める
MasashiNomura 48:71aec693a7dc 60 float calc_sum = clcP + sum + clcD;
MasashiNomura 48:71aec693a7dc 61 float fb_value = 0.0;
MasashiNomura 48:71aec693a7dc 62 if(0 < calc_sum){fb_value = sqrtf(calc_sum);}
MasashiNomura 48:71aec693a7dc 63 else{fb_value = -sqrtf(-calc_sum);}
MasashiNomura 48:71aec693a7dc 64
MasashiNomura 48:71aec693a7dc 65 //元のプログラムと正負逆転していることに注意
MasashiNomura 48:71aec693a7dc 66 return fb_value;
takeru0x1103 17:f9610f3cfa1b 67 }
takeru0x1103 17:f9610f3cfa1b 68
takeru0x1103 19:4b0fe9a5ec38 69 //=========================================================
MasashiNomura 31:56c554c560c1 70 //PID制御+トルク
MasashiNomura 31:56c554c560c1 71 //=========================================================
MasashiNomura 31:56c554c560c1 72 float HbAttitude::pid2(float iCmdAng, float iCurAng , float iRate)
MasashiNomura 31:56c554c560c1 73 {
MasashiNomura 31:56c554c560c1 74 //エラー量：指令値との差を求める
MasashiNomura 48:71aec693a7dc 75 // FeedBack量のためact-ref
MasashiNomura 31:56c554c560c1 76 float errAng = iCmdAng - iCurAng;
MasashiNomura 32:7f4145cc3551 77 //アウターループPpゲインを掛けて、角速度にkvゲインをかけて、目標角速度とする
MasashiNomura 31:56c554c560c1 78 float AngVelo = (iCurAng - oAng) * 50;//サンプリング周期をかけて、角速度(deg/s)に
MasashiNomura 31:56c554c560c1 79 float cmdRate= errAng * p - AngVelo * kv;
MasashiNomura 31:56c554c560c1 80
MasashiNomura 32:7f4145cc3551 81 //▼角速度偏差（指令値と現在値との差）
MasashiNomura 32:7f4145cc3551 82 float devRate = cmdRate - iRate ;
MasashiNomura 31:56c554c560c1 83
MasashiNomura 32:7f4145cc3551 84 //▼比例項
MasashiNomura 32:7f4145cc3551 85 float clcP = devRate * kp;
MasashiNomura 31:56c554c560c1 86
MasashiNomura 32:7f4145cc3551 87 //▼積分項
MasashiNomura 32:7f4145cc3551 88 float xi = ki * devRate; //係数をかける
MasashiNomura 32:7f4145cc3551 89 float tmpInteg = sum + xi;//積分して
MasashiNomura 31:56c554c560c1 90
MasashiNomura 32:7f4145cc3551 91 //リミット掛ける
MasashiNomura 32:7f4145cc3551 92 if(tmpInteg > limitH){tmpInteg = limitH;}
MasashiNomura 32:7f4145cc3551 93 if(tmpInteg < limitL){tmpInteg = limitL;}
MasashiNomura 31:56c554c560c1 94
MasashiNomura 32:7f4145cc3551 95 //積分値を次回計算用に保存
MasashiNomura 32:7f4145cc3551 96 sum = tmpInteg;
MasashiNomura 31:56c554c560c1 97
MasashiNomura 32:7f4145cc3551 98 //▼微分項
MasashiNomura 32:7f4145cc3551 99 float clcD = kd * (devRate - old);
MasashiNomura 32:7f4145cc3551 100 // 過去データ書き換え
MasashiNomura 32:7f4145cc3551 101 old = devRate;
MasashiNomura 31:56c554c560c1 102 oAng = iCurAng;
MasashiNomura 31:56c554c560c1 103 //
MasashiNomura 32:7f4145cc3551 104 return clcP + tmpInteg + clcD;
MasashiNomura 32:7f4145cc3551 105 //return cmdRate;
MasashiNomura 31:56c554c560c1 106 }
MasashiNomura 31:56c554c560c1 107
MasashiNomura 31:56c554c560c1 108 //=========================================================
takeru0x1103 19:4b0fe9a5ec38 109 //パラメータゲッター
takeru0x1103 19:4b0fe9a5ec38 110 //=========================================================
takeru0x1103 18:5aa48aec9cae 111 float HbAttitude::getPp(){return p;}
takeru0x1103 18:5aa48aec9cae 112 float HbAttitude::getP() {return kp;}
takeru0x1103 18:5aa48aec9cae 113 float HbAttitude::getI() {return ki;}
takeru0x1103 18:5aa48aec9cae 114 float HbAttitude::getD() {return kd;}
MasashiNomura 31:56c554c560c1 115 //float HbAttitude::getT() {return kt;}
MasashiNomura 31:56c554c560c1 116 float HbAttitude::getV() {return kv;}
takeru0x1103 18:5aa48aec9cae 117
takeru0x1103 19:4b0fe9a5ec38 118 //=========================================================
takeru0x1103 19:4b0fe9a5ec38 119 //パラメータセッター
takeru0x1103 19:4b0fe9a5ec38 120 //=========================================================
takeru0x1103 18:5aa48aec9cae 121 void HbAttitude::setPp(float iPp){p = iPp;}
takeru0x1103 18:5aa48aec9cae 122 void HbAttitude::setP(float iP) {kp= iP;}
takeru0x1103 18:5aa48aec9cae 123 void HbAttitude::setI(float iI) {ki= iI;}
takeru0x1103 18:5aa48aec9cae 124 void HbAttitude::setD(float iD) {kd= iD;}
MasashiNomura 31:56c554c560c1 125 //void HbAttitude::setT(float iT){kt = iT;}
MasashiNomura 31:56c554c560c1 126 void HbAttitude::setV(float iV){kv = iV;}
MasashiNomura 24:c5945aaae777 127 void HbAttitude::setIMax(float val){limitH = val;}
MasashiNomura 24:c5945aaae777 128 void HbAttitude::setIMin(float val){limitL = val;}
takeru0x1103 18:5aa48aec9cae 129
takeru0x1103 17:f9610f3cfa1b 130 //=========================================================
takeru0x1103 17:f9610f3cfa1b 131 //コンストラクタ
takeru0x1103 17:f9610f3cfa1b 132 //=========================================================
takeru0x1103 17:f9610f3cfa1b 133 HbAttitude::HbAttitude(float iPo , float iP , float iI , float iD){
takeru0x1103 17:f9610f3cfa1b 134 //パラメータ初期化
takeru0x1103 19:4b0fe9a5ec38 135 p =iPo ;//アウターループP制御系数
takeru0x1103 19:4b0fe9a5ec38 136 kp =iP ;//インナーループP制御系数
takeru0x1103 19:4b0fe9a5ec38 137 ki =iI ;//インナーループI制御系数
takeru0x1103 19:4b0fe9a5ec38 138 kd =iD ;//インナーループD制御系数
MasashiNomura 47:d3fa874f336e 139 kv = 4.0;
takeru0x1103 18:5aa48aec9cae 140 limitH=2000 ;//積分上限
takeru0x1103 18:5aa48aec9cae 141 limitL=-2000;//
takeru0x1103 19:4b0fe9a5ec38 142 sum =0 ;//積分値
takeru0x1103 19:4b0fe9a5ec38 143 old =0 ;//1サンプル前のデータ(微分用)
MasashiNomura 31:56c554c560c1 144 oAng = 0 ;//1サンプル前の角度
takeru0x1103 17:f9610f3cfa1b 145 }
takeru0x1103 17:f9610f3cfa1b 146