iranaikamo sirenaisi irukamo sirenai
pidの計算の部分だけをコトコト煮詰めてライブラリにしました。
PID.hpp@0:f6d51a9c6ce9, 2021-04-14 (annotated)
- Committer:
- hamohamo
- Date:
- Wed Apr 14 14:21:07 2021 +0000
- Revision:
- 0:f6d51a9c6ce9
- Child:
- 1:98493ab2f769
library for pid
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
hamohamo | 0:f6d51a9c6ce9 | 1 | #ifndef IG_PID_HPP_ |
hamohamo | 0:f6d51a9c6ce9 | 2 | #define IG_PID_HPP_ |
hamohamo | 0:f6d51a9c6ce9 | 3 | |
hamohamo | 0:f6d51a9c6ce9 | 4 | class PID{ |
hamohamo | 0:f6d51a9c6ce9 | 5 | public: |
hamohamo | 0:f6d51a9c6ce9 | 6 | PID(double KP,double KI,double KD,double DT):Kp(KP),Ki(KI),Kd(KD),dt(DT){ |
hamohamo | 0:f6d51a9c6ce9 | 7 | |
hamohamo | 0:f6d51a9c6ce9 | 8 | } |
hamohamo | 0:f6d51a9c6ce9 | 9 | void Update(double e){ |
hamohamo | 0:f6d51a9c6ce9 | 10 | PROP = e; |
hamohamo | 0:f6d51a9c6ce9 | 11 | INT += (curr_e + prev_e)*dt/2.0; |
hamohamo | 0:f6d51a9c6ce9 | 12 | DIFF = (curr_e - prev_e)/dt; |
hamohamo | 0:f6d51a9c6ce9 | 13 | mv = Kp*PROP + Ki*INT + Kd*DIFF; |
hamohamo | 0:f6d51a9c6ce9 | 14 | } |
hamohamo | 0:f6d51a9c6ce9 | 15 | double getmv(){ |
hamohamo | 0:f6d51a9c6ce9 | 16 | return mv; |
hamohamo | 0:f6d51a9c6ce9 | 17 | } |
hamohamo | 0:f6d51a9c6ce9 | 18 | private: |
hamohamo | 0:f6d51a9c6ce9 | 19 | double Kp,Ki,Kd; /* Pゲイン Iゲイン Dゲイン */ |
hamohamo | 0:f6d51a9c6ce9 | 20 | double dt; /* 微小時間 */ |
hamohamo | 0:f6d51a9c6ce9 | 21 | double prev_e,curr_e; /* 前回の誤差 現在の誤差 */ |
hamohamo | 0:f6d51a9c6ce9 | 22 | double PROP,INT,DIFF; /* 比例 積分 微分 */ |
hamohamo | 0:f6d51a9c6ce9 | 23 | double mv; /* manipulating variable */ |
hamohamo | 0:f6d51a9c6ce9 | 24 | }; |
hamohamo | 0:f6d51a9c6ce9 | 25 | |
hamohamo | 0:f6d51a9c6ce9 | 26 | #endif /* IG_PID_HPP_ */ |