123
Fork of Boboobooo by
Embed:
(wiki syntax)
Show/hide line numbers
controller.cpp
00001 #include "mbed.h" 00002 #include "controller.h" 00003 00004 00005 00006 00007 PID::PID(float in_min,float in_max,float out_min,float out_max,float Kc, float tauI, float tauD, float interval) { 00008 00009 //BX tune 00010 Kp = 0.0004; 00011 Ki = 0.0; 00012 Kd = 0.0; 00013 setInputLimits(in_min,in_max); 00014 setOutputLimits(out_min,out_max); 00015 00016 tSample_ = interval; 00017 00018 setTunings(Kc, tauI, tauD); 00019 00020 setPoint_ = 0.0; 00021 processVariable_ = 0.0; 00022 prevProcessVariable_ = 0.0; 00023 controllerOutput_ = 0.0; 00024 prevControllerOutput_ = 0.0; 00025 00026 00027 00028 } 00029 00030 void PID::setInputLimits(float inMin, float inMax) { 00031 00032 //Make sure we haven't been given impossible values. 00033 if (inMin >= inMax) { 00034 return; 00035 } 00036 00037 00038 00039 00040 00041 inMin_ = inMin; 00042 inMax_ = inMax; 00043 inSpan_ = (inMax - inMin); 00044 00045 00046 00047 00048 } 00049 00050 void PID::setOutputLimits(float outMin, float outMax) { 00051 00052 //Make sure we haven't been given impossible values. 00053 if (outMin >= outMax) { 00054 return; 00055 } 00056 00057 00058 00059 //ppp 00060 00061 outMin_ = outMin; 00062 outMax_ = outMax; 00063 outMid_ = (outMin+outMax)/2; 00064 00065 outSpan_ = (outMax - outMin); 00066 00067 00068 00069 00070 00071 00072 } 00073 00074 00075 00076 //-------------------------------------------------- 00077 00078 00079 00080 void PID::setTunings(float Kc, float tauI, float tauD) { 00081 00082 //Verify that the tunings make sense. 00083 if (Kc == 0.0 || tauI < 0.0 || tauD < 0.0) { 00084 return; 00085 } 00086 00087 //Store raw values to hand back to user on request. 00088 pParam_ = Kc; 00089 iParam_ = tauI; 00090 dParam_ = tauD; 00091 00092 00093 00094 00095 00096 Kc_ = Kc; 00097 tauI_=tauI; 00098 tauD_=tauD; 00099 00100 } 00101 00102 00103 00104 00105 00106 00107 00108 00109 00110 00111 00112 00113 00114 00115 00116 00117 00118 00119 00120 00121 00122 00123 float PID::compute(float center , float sp) { 00124 //turn right 122~64 122 00125 //turn left 64~6 8 00126 float C = center; 00127 float goal = sp; // center of black 00128 float error = goal - C;// 00129 return 0.073+Kp*error; 00130 00131 } 00132 00133 float PID::getInMin() { 00134 00135 return inMin_; 00136 00137 } 00138 00139 float PID::getInMax() { 00140 00141 return inMax_; 00142 00143 } 00144 00145 float PID::getOutMin() { 00146 00147 return outMin_; 00148 00149 } 00150 00151 float PID::getOutMax() { 00152 00153 return outMax_; 00154 00155 } 00156 00157 float PID::getInterval() { 00158 00159 return tSample_; 00160 00161 } 00162 00163 float PID::getPParam() { 00164 00165 return pParam_; 00166 00167 } 00168 00169 float PID::getIParam() { 00170 00171 return iParam_; 00172 00173 } 00174 00175 float PID::getDParam() { 00176 00177 return dParam_; 00178 00179 }
Generated on Wed Jul 20 2022 19:27:32 by
1.7.2
