Team repo
Embed:
(wiki syntax)
Show/hide line numbers
pid.cpp
00001 #ifndef _PID_SOURCE_ 00002 #define _PID_SOURCE_ 00003 00004 #include <iostream> 00005 #include <cmath> 00006 #include "pid.h" 00007 00008 using namespace std; 00009 00010 PID::PID( float dt, float max, float min, float kp, float ki, float kd ) 00011 { 00012 dt_ = dt; 00013 max_ = max; 00014 min_ = min; 00015 kp_ = kp; 00016 ki_ = ki; 00017 kd_ = kd; 00018 pre_error_ = 0; 00019 integral_ = 0; 00020 00021 } 00022 00023 float PID::calculate(float setpoint, float pv) 00024 { 00025 // Calculate time changed since last PID calculation 00026 // dt_ = current_time - dt_; 00027 00028 // Calculate error 00029 float error = setpoint - pv; 00030 00031 // Proportional term 00032 float Pout = kp_ * error; 00033 00034 // Integral term 00035 integral_ += error * dt_; 00036 float Iout = ki_ * integral_; 00037 00038 // Derivative term 00039 float derivative = (error - pre_error_) / dt_; 00040 float Dout = kd_ * derivative; 00041 00042 // Calculate total output 00043 float output = Pout + Iout + Dout; 00044 00045 // Restrict to max/min 00046 00047 if( output > max_ ){ 00048 output = max_; 00049 } 00050 else if( output < min_ ){ 00051 output = min_; 00052 } 00053 00054 00055 // Save error to previous error 00056 pre_error_ = error; 00057 00058 return output; 00059 } 00060 00061 void PID::testError(float setpoint, float pv){ 00062 float error = setpoint - pv; 00063 printf("Error: %f\n", error); 00064 printf("Change in Time: %f\n", dt_); 00065 } 00066 00067 PID::~PID() {} 00068 00069 00070 /** 00071 * Implementation 00072 */ 00073 /* 00074 PIDImpl::PIDImpl( float dt, float max, float min, float Kp, float Kd, float Ki ) : 00075 _dt(dt), 00076 _max(max), 00077 _min(min), 00078 _Kp(Kp), 00079 _Kd(Kd), 00080 _Ki(Ki), 00081 _pre_error(0), 00082 _integral(0) 00083 { 00084 } 00085 00086 float PIDImpl::calculate( float setpoint, float pv ) 00087 { 00088 00089 // Calculate error 00090 float error = setpoint - pv; 00091 00092 // Proportional term 00093 float Pout = _Kp * error; 00094 00095 // Integral term 00096 _integral += error * _dt; 00097 float Iout = _Ki * _integral; 00098 00099 // Derivative term 00100 float derivative = (error - _pre_error) / _dt; 00101 float Dout = _Kd * derivative; 00102 00103 // Calculate total output 00104 float output = Pout + Iout + Dout; 00105 00106 // Restrict to max/min 00107 if( output > _max ) 00108 output = _max; 00109 else if( output < _min ) 00110 output = _min; 00111 00112 // Save error to previous error 00113 _pre_error = error; 00114 00115 return output; 00116 } 00117 00118 PIDImpl::~PIDImpl() 00119 { 00120 } 00121 */ 00122 #endif
Generated on Sat Jul 16 2022 15:10:49 by
1.7.2