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 }