Gustavo Ramirez
Controlador PID Genérico que se parametriza por medio de un encoder rotatorio- Sp (Set-point), Kp(Ganancia proporcional) ,Ki(Constante integral), Kd(Constante Derivativa), usa dos pulsadores adicionales
Dependencies: QEI TextLCD mbed
Embed:
(wiki syntax)
Show/hide line numbers
main.cpp
00001 #include "mbed.h" 00002 #include "QEI.h" 00003 #include "TextLCD.h" 00004 00005 TextLCD lcd(PTB10, PTB11, PTE2, PTE3, PTE4, PTE5); // rs, e, d4-d7 00006 QEI encoder (PTA13, PTD5, NC, 624); 00007 AnalogIn y(PTB3);//entrada analoga 00008 AnalogOut u(PTE30);//salida analoga OJO solo se le pueden drenar 1.5mA en circuitos use un Buffer 00009 //si se ignora esto se arruina la FRDMKL25Z 00010 DigitalOut led1(LED1); 00011 DigitalOut led2(LED2); 00012 DigitalOut led3(LED3); 00013 00014 DigitalIn button3(PTC16);//cambia ingreso de los 4 parametros 00015 DigitalIn button4(PTC17);//termina y consolida valores de 4 parametros y sale del loop 00016 00017 00018 //codigos movimiento del curzor 00019 00020 //int C1=0x0E; // solo muestra el curzor 00021 int C2=0x18; // desplaza izquierda 00022 int C3=0x1A; // desplaza derecha 00023 int C4=0x0C; // quito cursor bajo 00024 00025 int C1=0x0F; 00026 int cambio=0, diferencia=0; 00027 float pid,o,ai,ad,ap,med,err; 00028 float err_v; 00029 int spnum=0,kinum=0,kpnum=0,kdnum=0,pos=1; 00030 00031 int main() 00032 { 00033 lcd.locate(0,1); 00034 lcd.printf("**Control PID**"); 00035 wait(2); 00036 lcd.cls(); // Borrar Pantalla 00037 lcd.writeCommand(C1);//escribimos un comando segun el manual del modulo LCD 00038 00039 lcd.locate(8,0); 00040 lcd.printf("Kp=%d",kpnum); 00041 lcd.locate(0,1); 00042 lcd.printf("Ki=%d",kinum); 00043 lcd.locate(8,1); 00044 lcd.printf("Kd=%d",kdnum); 00045 lcd.locate(0,0); 00046 lcd.printf("Sp=%d",spnum); 00047 00048 while(1) 00049 { 00050 //lcd.locate(8,0); 00051 //lcd.printf("Kp=%d",encoder.getPulses()); 00052 //wait(.5); 00053 00054 diferencia=encoder.getPulses()-cambio; 00055 cambio=encoder.getPulses(); 00056 00057 if (diferencia==0) 00058 { 00059 //nada 00060 } 00061 else if(diferencia>0) 00062 { 00063 if(pos==1) 00064 { 00065 if(spnum+diferencia>=999) 00066 { 00067 spnum=999; 00068 lcd.locate(3,0); 00069 lcd.printf(" "); 00070 lcd.locate(3,0); 00071 lcd.printf("%d", spnum); 00072 } 00073 else 00074 { 00075 spnum+=diferencia; 00076 lcd.locate(3,0); 00077 lcd.printf("%d", spnum); 00078 } 00079 } 00080 else if(pos==2) 00081 { 00082 if(kpnum+diferencia>=999) 00083 { 00084 kpnum=999; 00085 lcd.locate(11,0); 00086 lcd.printf(" "); 00087 lcd.locate(11,0); 00088 lcd.printf("%d", kpnum); 00089 } 00090 else 00091 { 00092 kpnum+=diferencia; 00093 lcd.locate(11,0); 00094 lcd.printf("%d", kpnum); 00095 } 00096 } 00097 else if(pos==3) 00098 { 00099 if(kinum+diferencia>=999) 00100 { 00101 kinum=999; 00102 lcd.locate(3,1); 00103 lcd.printf(" "); 00104 lcd.locate(3,1); 00105 lcd.printf("%d", kinum); 00106 } 00107 else 00108 { 00109 kinum+=diferencia; 00110 lcd.locate(3,1); 00111 lcd.printf("%d", kinum); 00112 } 00113 } 00114 else if(pos==4) 00115 { 00116 if(kdnum+diferencia>=999) 00117 { 00118 kdnum=999; 00119 lcd.locate(11,1); 00120 lcd.printf(" "); 00121 lcd.locate(11,1); 00122 lcd.printf("%d", kdnum); 00123 } 00124 else 00125 { 00126 kdnum+=diferencia; 00127 lcd.locate(11,1); 00128 lcd.printf("%d", kdnum); 00129 } 00130 } 00131 } 00132 00133 else if(diferencia<0) 00134 { 00135 if(pos==1) 00136 { 00137 if(spnum+diferencia<0) 00138 { 00139 //No ocurre nada 00140 } 00141 else 00142 { 00143 spnum+=diferencia; 00144 lcd.locate(3,0); 00145 lcd.printf(" "); 00146 lcd.locate(3,0); 00147 lcd.printf("%d", spnum); 00148 } 00149 } 00150 else if(pos==2) 00151 { 00152 if(kpnum+diferencia<0) 00153 { 00154 //No ocurre nada 00155 } 00156 else 00157 { 00158 kpnum+=diferencia; 00159 lcd.locate(11,0); 00160 lcd.printf(" "); 00161 lcd.locate(11,0); 00162 lcd.printf("%d", kpnum); 00163 } 00164 } 00165 else if(pos==3) 00166 { 00167 if(kinum+diferencia<0) 00168 { 00169 //No ocurre nada 00170 } 00171 else 00172 { 00173 kinum+=diferencia; 00174 lcd.locate(3,1); 00175 lcd.printf(" "); 00176 lcd.locate(3,1); 00177 lcd.printf("%d", kinum); 00178 } 00179 } 00180 else if(pos==4) 00181 { 00182 if(kdnum+diferencia<0) 00183 { 00184 //No ocurre nada 00185 } 00186 else 00187 { 00188 kdnum+=diferencia; 00189 lcd.locate(11,1); 00190 lcd.printf(" "); 00191 lcd.locate(11,1); 00192 lcd.printf("%d", kdnum); 00193 } 00194 } 00195 } 00196 00197 if (!button3) //cambia la posicion de ingreso de parametros 00198 { 00199 led3 =!led3; 00200 if(pos==4) 00201 { 00202 pos=1; 00203 lcd.locate(3,0); 00204 lcd.printf("%d", spnum); 00205 } 00206 else if (pos==1) 00207 { 00208 pos++; 00209 lcd.locate(11,0); 00210 lcd.printf("%d", kpnum); 00211 } 00212 else if(pos==2) 00213 { 00214 pos++; 00215 lcd.locate(3,1); 00216 lcd.printf("%d", kinum); 00217 } 00218 else if(pos==3) 00219 { 00220 pos++; 00221 lcd.locate(11,1); 00222 lcd.printf("%d", kdnum); 00223 } 00224 wait(0.25); 00225 00226 } 00227 00228 if (!button4) 00229 { 00230 break; //sale del bucle si pisan suiche4 00231 } 00232 wait(0.1); 00233 } 00234 00235 00236 //Transicion 00237 lcd.writeCommand(C4);//escribimos un comando segun el manual del modulo LCD para quitar cursor bajo 00238 lcd.cls(); //borra la pantalla 00239 lcd.printf(" GUARDADOS!"); 00240 wait(1); 00241 lcd.cls(); 00242 lcd.printf(" INICIA EL PID"); 00243 wait(1); 00244 // se imprimen los parches del control ***************************************** 00245 lcd.cls(); 00246 lcd.printf("Er=%3.0f",err); 00247 lcd.locate(8,0); 00248 lcd.printf("Me=%3.0f",med); 00249 lcd.locate(0,1); 00250 lcd.printf("Sp=%3.0f",spnum); 00251 lcd.locate(8,1); 00252 lcd.printf("Co=%3.0f",pid); 00253 wait(1); 00254 00255 // CICLO PRINCIPAL CONTROLADOR PID 00256 lop1: med = y.read()*999; 00257 err = (spnum-med); //se calcula el error 00258 ap = kpnum*err*0.01f; //se calcula la accion proporcinal 00259 ai =(kinum*err*0.01f)+ai; //calculo de la integral del error 00260 ad = kdnum*(err-err_v)*0.01f; //calculo de la accion derivativa 00261 pid = (ap+ai+ad); 00262 // se verifica que pid sea positivo ************************************** 00263 if(pid<=0) 00264 { 00265 pid=0; 00266 } 00267 00268 // se verifica que pid sea menor o igual la valor maximo ***************** 00269 if (pid > 999) 00270 { 00271 pid=999; 00272 } 00273 00274 00275 //se muestran las variables****************************************** 00276 lcd.locate(3,0); 00277 lcd.printf(" "); 00278 lcd.locate(3,0); 00279 lcd.printf("%3.0f",err); 00280 lcd.locate(11,0); 00281 lcd.printf(" "); 00282 lcd.locate(11,0); 00283 lcd.printf("%3.0f",med); 00284 lcd.locate(3,1); 00285 lcd.printf(" "); 00286 lcd.locate(3,1); 00287 lcd.printf("%d",spnum); 00288 lcd.locate(11,1); 00289 lcd.printf(" "); 00290 lcd.locate(11,1); 00291 lcd.printf("%3.0f",pid); 00292 00293 00294 00295 00296 //Normalizacion de la salida 00297 // se actualizan las variables ******************************************* 00298 err_v = err; 00299 o = pid/999; 00300 u.write(o); 00301 // se envia el valor pid a puerto analogico de salida (D/A) ************** 00302 00303 // se repite el ciclo 00304 wait_ms(300); 00305 goto lop1; 00306 }
Generated on Sun Jul 17 2022 00:33:46 by
1.7.2