Diff: cal_PID.cpp

Revision:

0:52549f6dd024

Child:

1:f1f6c98a4b12

diff -r 000000000000 -r 52549f6dd024 cal_PID.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/cal_PID.cpp	Fri Aug 26 11:30:08 2016 +0000
@@ -0,0 +1,49 @@
+#include "mbed.h"
+#include "cal_PID.hpp"
+
+cal_pid::cal_pid(){
+    past_error=0;
+    integral_error=0;
+    p_gain=1.0;
+    i_gain=0.0;
+    d_gain=0.0;
+    control_period=0.001;
+    output_min=-128;
+    output_max=127;
+}
+
+cal_pid::~cal_pid(){    
+}
+
+void cal_pid::param(float _p_gain,float _i_gain,float _d_gain){
+    p_gain=_p_gain;
+    i_gain=_i_gain;
+    d_gain=_d_gain;
+}
+void cal_pid::period(float _control_period){
+    if(_control_period<=0)return;
+    control_period=_control_period;
+}
+void cal_pid::output(signed long _min,signed long _max){
+    output_min=_min;
+    output_max=_max;
+}
+void cal_pid::reset(){
+    past_error=0;
+    integral_error=0;
+}
+signed long cal_pid::get_pid(signed long position,signed long targ){
+    error=targ-position;
+    integral_error+=error*control_period;
+    differential_error=(error-past_error)/control_period;
+    past_error=error;
+    p_part=error*p_gain;
+    i_part=integral_error*i_gain;
+    d_part=differential_error*d_gain;
+    operate=p_part+i_part+d_part;
+    if(operate<output_min)operate=output_min;
+    else if(operate>output_max)operate=output_max;
+    
+    return operate;
+    
+}