Akito / cal_PID

PID制御計算用ライブラリ

Dependents:   ROBOCON2017_archan ROBOCON2017_suchan

cal_PID.cpp@1:f1f6c98a4b12, 2016-09-28 (annotated)

Committer:
Akito914
Date:
Wed Sep 28 01:42:33 2016 +0000
Revision:
1:f1f6c98a4b12
Parent:
0:52549f6dd024
Child:
2:e340bdcb9bca
???????????????????

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Akito914 0:52549f6dd024 1 #include "mbed.h"
Akito914 0:52549f6dd024 2 #include "cal_PID.hpp"
Akito914 0:52549f6dd024 3
Akito914 0:52549f6dd024 4 cal_pid::cal_pid(){
Akito914 0:52549f6dd024 5 past_error=0;
Akito914 0:52549f6dd024 6 integral_error=0;
Akito914 0:52549f6dd024 7 p_gain=1.0;
Akito914 0:52549f6dd024 8 i_gain=0.0;
Akito914 0:52549f6dd024 9 d_gain=0.0;
Akito914 0:52549f6dd024 10 control_period=0.001;
Akito914 0:52549f6dd024 11 output_min=-128;
Akito914 0:52549f6dd024 12 output_max=127;
Akito914 0:52549f6dd024 13 }
Akito914 0:52549f6dd024 14
Akito914 0:52549f6dd024 15 cal_pid::~cal_pid(){
Akito914 0:52549f6dd024 16 }
Akito914 0:52549f6dd024 17
Akito914 0:52549f6dd024 18 void cal_pid::param(float _p_gain,float _i_gain,float _d_gain){
Akito914 0:52549f6dd024 19 p_gain=_p_gain;
Akito914 0:52549f6dd024 20 i_gain=_i_gain;
Akito914 0:52549f6dd024 21 d_gain=_d_gain;
Akito914 0:52549f6dd024 22 }
Akito914 0:52549f6dd024 23 void cal_pid::period(float _control_period){
Akito914 0:52549f6dd024 24 if(_control_period<=0)return;
Akito914 0:52549f6dd024 25 control_period=_control_period;
Akito914 0:52549f6dd024 26 }
Akito914 0:52549f6dd024 27 void cal_pid::output(signed long _min,signed long _max){
Akito914 0:52549f6dd024 28 output_min=_min;
Akito914 0:52549f6dd024 29 output_max=_max;
Akito914 0:52549f6dd024 30 }
Akito914 0:52549f6dd024 31 void cal_pid::reset(){
Akito914 0:52549f6dd024 32 past_error=0;
Akito914 0:52549f6dd024 33 integral_error=0;
Akito914 0:52549f6dd024 34 }
Akito914 1:f1f6c98a4b12 35 signed long cal_pid::get_pid(signed long position,signed long targ,int rd){
Akito914 0:52549f6dd024 36 error=targ-position;
Akito914 0:52549f6dd024 37 integral_error+=error*control_period;
Akito914 0:52549f6dd024 38 differential_error=(error-past_error)/control_period;
Akito914 0:52549f6dd024 39 past_error=error;
Akito914 0:52549f6dd024 40 p_part=error*p_gain;
Akito914 0:52549f6dd024 41 i_part=integral_error*i_gain;
Akito914 0:52549f6dd024 42 d_part=differential_error*d_gain;
Akito914 0:52549f6dd024 43 operate=p_part+i_part+d_part;
Akito914 1:f1f6c98a4b12 44 operate*=rd;
Akito914 0:52549f6dd024 45 if(operate<output_min)operate=output_min;
Akito914 0:52549f6dd024 46 else if(operate>output_max)operate=output_max;
Akito914 0:52549f6dd024 47
Akito914 0:52549f6dd024 48 return operate;
Akito914 0:52549f6dd024 49
Akito914 0:52549f6dd024 50 }