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@48:71aec693a7dc, 2019-02-24 (annotated)

Committer:
MasashiNomura
Date:
Sun Feb 24 10:33:34 2019 +0000
Revision:
48:71aec693a7dc
Parent:
47:d3fa874f336e
Child:
52:33fa8060dd8c
20190224 add cmd pid mode etc.

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