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@47:d3fa874f336e, 2019-02-20 (annotated)

Committer:
MasashiNomura
Date:
Wed Feb 20 12:54:25 2019 +0000
Revision:
47:d3fa874f336e
Parent:
32:7f4145cc3551
Child:
48:71aec693a7dc
2019/02/20 Add State "UPPER_IDLE"

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 {
takeru0x1103 17:f9610f3cfa1b 9 //エラー量：指令値との差を求める
takeru0x1103 17:f9610f3cfa1b 10 float errAng = iCmdAng - iCurAng;
takeru0x1103 17:f9610f3cfa1b 11 //アウターループのPゲインを掛けて目標角速度とする
takeru0x1103 17:f9610f3cfa1b 12 float cmdRate= errAng * p;
takeru0x1103 17:f9610f3cfa1b 13
takeru0x1103 17:f9610f3cfa1b 14 //▼角速度偏差（指令値と現在値との差）
takeru0x1103 17:f9610f3cfa1b 15 float devRate = cmdRate - iRate ;
takeru0x1103 17:f9610f3cfa1b 16
takeru0x1103 17:f9610f3cfa1b 17 //▼比例項
takeru0x1103 17:f9610f3cfa1b 18 float clcP = devRate * kp;
takeru0x1103 17:f9610f3cfa1b 19
takeru0x1103 17:f9610f3cfa1b 20 //▼積分項
takeru0x1103 17:f9610f3cfa1b 21 float xi = ki * devRate; //係数をかける
takeru0x1103 17:f9610f3cfa1b 22 float tmpInteg = sum + xi;//積分して
takeru0x1103 17:f9610f3cfa1b 23
takeru0x1103 17:f9610f3cfa1b 24 //リミット掛ける
takeru0x1103 17:f9610f3cfa1b 25 if(tmpInteg > limitH){tmpInteg = limitH;}
takeru0x1103 17:f9610f3cfa1b 26 if(tmpInteg < limitL){tmpInteg = limitL;}
takeru0x1103 17:f9610f3cfa1b 27
takeru0x1103 17:f9610f3cfa1b 28 //積分値を次回計算用に保存
takeru0x1103 17:f9610f3cfa1b 29 sum = tmpInteg;
takeru0x1103 17:f9610f3cfa1b 30
takeru0x1103 17:f9610f3cfa1b 31 //▼微分項
takeru0x1103 17:f9610f3cfa1b 32 float clcD = kd * (devRate - old);
takeru0x1103 17:f9610f3cfa1b 33 //過去データ書き換え
takeru0x1103 17:f9610f3cfa1b 34 old = devRate;
takeru0x1103 18:5aa48aec9cae 35 //
takeru0x1103 17:f9610f3cfa1b 36 return clcP + tmpInteg + clcD;
takeru0x1103 17:f9610f3cfa1b 37 }
takeru0x1103 17:f9610f3cfa1b 38
takeru0x1103 19:4b0fe9a5ec38 39 //=========================================================
MasashiNomura 31:56c554c560c1 40 //PID制御+トルク
MasashiNomura 31:56c554c560c1 41 //=========================================================
MasashiNomura 31:56c554c560c1 42 float HbAttitude::pid2(float iCmdAng, float iCurAng , float iRate)
MasashiNomura 31:56c554c560c1 43 {
MasashiNomura 31:56c554c560c1 44 //エラー量：指令値との差を求める
MasashiNomura 31:56c554c560c1 45 float errAng = iCmdAng - iCurAng;
MasashiNomura 32:7f4145cc3551 46 //アウターループPpゲインを掛けて、角速度にkvゲインをかけて、目標角速度とする
MasashiNomura 31:56c554c560c1 47 float AngVelo = (iCurAng - oAng) * 50;//サンプリング周期をかけて、角速度(deg/s)に
MasashiNomura 31:56c554c560c1 48 float cmdRate= errAng * p - AngVelo * kv;
MasashiNomura 31:56c554c560c1 49
MasashiNomura 32:7f4145cc3551 50 //▼角速度偏差（指令値と現在値との差）
MasashiNomura 32:7f4145cc3551 51 float devRate = cmdRate - iRate ;
MasashiNomura 31:56c554c560c1 52
MasashiNomura 32:7f4145cc3551 53 //▼比例項
MasashiNomura 32:7f4145cc3551 54 float clcP = devRate * kp;
MasashiNomura 31:56c554c560c1 55
MasashiNomura 32:7f4145cc3551 56 //▼積分項
MasashiNomura 32:7f4145cc3551 57 float xi = ki * devRate; //係数をかける
MasashiNomura 32:7f4145cc3551 58 float tmpInteg = sum + xi;//積分して
MasashiNomura 31:56c554c560c1 59
MasashiNomura 32:7f4145cc3551 60 //リミット掛ける
MasashiNomura 32:7f4145cc3551 61 if(tmpInteg > limitH){tmpInteg = limitH;}
MasashiNomura 32:7f4145cc3551 62 if(tmpInteg < limitL){tmpInteg = limitL;}
MasashiNomura 31:56c554c560c1 63
MasashiNomura 32:7f4145cc3551 64 //積分値を次回計算用に保存
MasashiNomura 32:7f4145cc3551 65 sum = tmpInteg;
MasashiNomura 31:56c554c560c1 66
MasashiNomura 32:7f4145cc3551 67 //▼微分項
MasashiNomura 32:7f4145cc3551 68 float clcD = kd * (devRate - old);
MasashiNomura 32:7f4145cc3551 69 // 過去データ書き換え
MasashiNomura 32:7f4145cc3551 70 old = devRate;
MasashiNomura 31:56c554c560c1 71 oAng = iCurAng;
MasashiNomura 31:56c554c560c1 72 //
MasashiNomura 32:7f4145cc3551 73 return clcP + tmpInteg + clcD;
MasashiNomura 32:7f4145cc3551 74 //return cmdRate;
MasashiNomura 31:56c554c560c1 75 }
MasashiNomura 31:56c554c560c1 76
MasashiNomura 31:56c554c560c1 77 //=========================================================
takeru0x1103 19:4b0fe9a5ec38 78 //パラメータゲッター
takeru0x1103 19:4b0fe9a5ec38 79 //=========================================================
takeru0x1103 18:5aa48aec9cae 80 float HbAttitude::getPp(){return p;}
takeru0x1103 18:5aa48aec9cae 81 float HbAttitude::getP() {return kp;}
takeru0x1103 18:5aa48aec9cae 82 float HbAttitude::getI() {return ki;}
takeru0x1103 18:5aa48aec9cae 83 float HbAttitude::getD() {return kd;}
MasashiNomura 31:56c554c560c1 84 //float HbAttitude::getT() {return kt;}
MasashiNomura 31:56c554c560c1 85 float HbAttitude::getV() {return kv;}
takeru0x1103 18:5aa48aec9cae 86
takeru0x1103 19:4b0fe9a5ec38 87 //=========================================================
takeru0x1103 19:4b0fe9a5ec38 88 //パラメータセッター
takeru0x1103 19:4b0fe9a5ec38 89 //=========================================================
takeru0x1103 18:5aa48aec9cae 90 void HbAttitude::setPp(float iPp){p = iPp;}
takeru0x1103 18:5aa48aec9cae 91 void HbAttitude::setP(float iP) {kp= iP;}
takeru0x1103 18:5aa48aec9cae 92 void HbAttitude::setI(float iI) {ki= iI;}
takeru0x1103 18:5aa48aec9cae 93 void HbAttitude::setD(float iD) {kd= iD;}
MasashiNomura 31:56c554c560c1 94 //void HbAttitude::setT(float iT){kt = iT;}
MasashiNomura 31:56c554c560c1 95 void HbAttitude::setV(float iV){kv = iV;}
MasashiNomura 24:c5945aaae777 96 void HbAttitude::setIMax(float val){limitH = val;}
MasashiNomura 24:c5945aaae777 97 void HbAttitude::setIMin(float val){limitL = val;}
takeru0x1103 18:5aa48aec9cae 98
takeru0x1103 17:f9610f3cfa1b 99 //=========================================================
takeru0x1103 17:f9610f3cfa1b 100 //コンストラクタ
takeru0x1103 17:f9610f3cfa1b 101 //=========================================================
takeru0x1103 17:f9610f3cfa1b 102 HbAttitude::HbAttitude(float iPo , float iP , float iI , float iD){
takeru0x1103 17:f9610f3cfa1b 103 //パラメータ初期化
takeru0x1103 19:4b0fe9a5ec38 104 p =iPo ;//アウターループP制御系数
takeru0x1103 19:4b0fe9a5ec38 105 kp =iP ;//インナーループP制御系数
takeru0x1103 19:4b0fe9a5ec38 106 ki =iI ;//インナーループI制御系数
takeru0x1103 19:4b0fe9a5ec38 107 kd =iD ;//インナーループD制御系数
MasashiNomura 47:d3fa874f336e 108 kv = 4.0;
takeru0x1103 18:5aa48aec9cae 109 limitH=2000 ;//積分上限
takeru0x1103 18:5aa48aec9cae 110 limitL=-2000;//
takeru0x1103 19:4b0fe9a5ec38 111 sum =0 ;//積分値
takeru0x1103 19:4b0fe9a5ec38 112 old =0 ;//1サンプル前のデータ(微分用)
MasashiNomura 31:56c554c560c1 113 oAng = 0 ;//1サンプル前の角度
takeru0x1103 17:f9610f3cfa1b 114 }
takeru0x1103 17:f9610f3cfa1b 115