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@70:a7b1e3eec51b, 2019-03-11 (annotated)

Committer:
YutakaTakagi
Date:
Mon Mar 11 08:58:28 2019 +0000
Revision:
70:a7b1e3eec51b
Parent:
52:33fa8060dd8c 
add attitude pid value reset function, if PID mode ON, call the function

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 //=========================================================
YutakaTakagi 70:a7b1e3eec51b 109 //前回値・積分地リセット
YutakaTakagi 70:a7b1e3eec51b 110 //=========================================================
YutakaTakagi 70:a7b1e3eec51b 111 void HbAttitude::resetValue()
YutakaTakagi 70:a7b1e3eec51b 112 {
YutakaTakagi 70:a7b1e3eec51b 113 sum = 0;//積分値
YutakaTakagi 70:a7b1e3eec51b 114 old = 0;//1サンプル前のデータ(微分用)
YutakaTakagi 70:a7b1e3eec51b 115 oAng = 0;//1サンプル前の角度
YutakaTakagi 70:a7b1e3eec51b 116 }
YutakaTakagi 70:a7b1e3eec51b 117
YutakaTakagi 70:a7b1e3eec51b 118 //=========================================================
takeru0x1103 19:4b0fe9a5ec38 119 //パラメータゲッター
takeru0x1103 19:4b0fe9a5ec38 120 //=========================================================
takeru0x1103 18:5aa48aec9cae 121 float HbAttitude::getPp(){return p;}
takeru0x1103 18:5aa48aec9cae 122 float HbAttitude::getP() {return kp;}
takeru0x1103 18:5aa48aec9cae 123 float HbAttitude::getI() {return ki;}
takeru0x1103 18:5aa48aec9cae 124 float HbAttitude::getD() {return kd;}
MasashiNomura 31:56c554c560c1 125 //float HbAttitude::getT() {return kt;}
MasashiNomura 31:56c554c560c1 126 float HbAttitude::getV() {return kv;}
takeru0x1103 18:5aa48aec9cae 127
takeru0x1103 19:4b0fe9a5ec38 128 //=========================================================
takeru0x1103 19:4b0fe9a5ec38 129 //パラメータセッター
takeru0x1103 19:4b0fe9a5ec38 130 //=========================================================
takeru0x1103 18:5aa48aec9cae 131 void HbAttitude::setPp(float iPp){p = iPp;}
takeru0x1103 18:5aa48aec9cae 132 void HbAttitude::setP(float iP) {kp= iP;}
takeru0x1103 18:5aa48aec9cae 133 void HbAttitude::setI(float iI) {ki= iI;}
takeru0x1103 18:5aa48aec9cae 134 void HbAttitude::setD(float iD) {kd= iD;}
MasashiNomura 31:56c554c560c1 135 //void HbAttitude::setT(float iT){kt = iT;}
MasashiNomura 31:56c554c560c1 136 void HbAttitude::setV(float iV){kv = iV;}
MasashiNomura 24:c5945aaae777 137 void HbAttitude::setIMax(float val){limitH = val;}
MasashiNomura 24:c5945aaae777 138 void HbAttitude::setIMin(float val){limitL = val;}
takeru0x1103 18:5aa48aec9cae 139
takeru0x1103 17:f9610f3cfa1b 140 //=========================================================
takeru0x1103 17:f9610f3cfa1b 141 //コンストラクタ
takeru0x1103 17:f9610f3cfa1b 142 //=========================================================
takeru0x1103 17:f9610f3cfa1b 143 HbAttitude::HbAttitude(float iPo , float iP , float iI , float iD){
takeru0x1103 17:f9610f3cfa1b 144 //パラメータ初期化
takeru0x1103 19:4b0fe9a5ec38 145 p =iPo ;//アウターループP制御系数
takeru0x1103 19:4b0fe9a5ec38 146 kp =iP ;//インナーループP制御系数
takeru0x1103 19:4b0fe9a5ec38 147 ki =iI ;//インナーループI制御系数
takeru0x1103 19:4b0fe9a5ec38 148 kd =iD ;//インナーループD制御系数
MasashiNomura 47:d3fa874f336e 149 kv = 4.0;
takeru0x1103 18:5aa48aec9cae 150 limitH=2000 ;//積分上限
takeru0x1103 18:5aa48aec9cae 151 limitL=-2000;//
takeru0x1103 19:4b0fe9a5ec38 152 sum =0 ;//積分値
takeru0x1103 19:4b0fe9a5ec38 153 old =0 ;//1サンプル前のデータ(微分用)
MasashiNomura 31:56c554c560c1 154 oAng = 0 ;//1サンプル前の角度
takeru0x1103 17:f9610f3cfa1b 155 }
takeru0x1103 17:f9610f3cfa1b 156