Procesadores 2017
/
PID_ZN_ENCODER
Important changes to repositories hosted on mbed.com
Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.
To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.
It is also possible to export all your personal repositories from the account settings page.
Dependencies: QEI TextLCD mbed
Fork of
TAREA_4_PROCESADORES
by 
john lopez
Embed:
(wiki syntax)
Show/hide line numbers
main.cpp
00001 #include "mbed.h" 00002 #include "TextLCD.h" 00003 #include "DebouncedIn.h" 00004 #include "QEI.h" 00005 00006 AnalogIn Vin(PTC2); 00007 AnalogOut Vout(PTE30); 00008 00009 00010 TextLCD lcd(PTB8, PTB9,PTB10,PTB11,PTE2, PTE3); //Puertos LCD rs, e, d4, d5, d6, d7 00011 QEI Encoder (PTA1, PTA2, NC, 624); //Puertos de la tarjeta asignados para el Encoder 00012 00013 DigitalOut led1(LED1); //led de cambio de posición 00014 DigitalOut led2(LED2); //led incremento de parámetros 00015 DigitalOut led3(LED3); //led decremento de parámetros 00016 00017 Timer t; 00018 int B,t2,tim; 00019 DebouncedIn bot1(PTC17); //cambiar la posición 00020 //salida de bucle 00021 00022 //Códigos LCD 00023 int C1=0x0E; // Muestra el cursor 00024 // 0x18; // desplazamiento izquierda 00025 // 0x1A; // desplazamiento derecha 00026 int C4=0x0C; 00027 //Variables autotuning---------------------------------------------------------- 00028 int TC; 00029 float kc; 00030 float ko; 00031 float v2; 00032 int AT=0; 00033 int i=1; 00034 int j=1; 00035 int k=1; 00036 int q; 00037 int q2; 00038 char D[30]; 00039 int v6; 00040 00041 //------------------------------------------------------------------------------ 00042 int a, kp, ki, kd, sp, ciclo, valor, v; // indice de la variable 00043 float med, sp0, pid, ap, err, ai, ad,err_v; 00044 Timer o; 00045 int o2; 00046 int p=1; 00047 int main() { 00048 float Kp,Kd,Ki; 00049 t.reset(); 00050 00051 led1=led2=led3=1; 00052 lcd.printf("Control PID"); 00053 wait(1.5); 00054 lcd.cls(); 00055 00056 lcd.writeCommand(C1); //comando para mostrar el cursor en el LCD 00057 00058 lcd.locate(0,0); // Ubica e imprime nombre de los parámetros en del PID en la pantalla LCD 00059 lcd.printf("Sp="); 00060 lcd.locate(8,0); 00061 lcd.printf("Kp="); 00062 lcd.locate(0,1); 00063 lcd.printf("Ki="); 00064 lcd.locate(8,1); 00065 lcd.printf("Kd="); 00066 00067 00068 //Inicio del ciclo 00069 00070 while(1) { if (bot1==0 && kp==0 && ki==0 && kd==0 && sp==0) 00071 {AT=1; } 00072 00073 if (AT==1){ 00074 if(p==1){ 00075 lcd.cls(); //limpio la pantalla 00076 lcd.locate(5,1); 00077 lcd.printf("AUTOTUNING"); 00078 wait(0.5); 00079 lcd.cls(); 00080 p=2; 00081 Kp=0.5; 00082 sp=3; 00083 00084 } 00085 if (p==2){ 00086 for (Kp=0.5;Kp<2;Kp=Kp+0.1){ 00087 00088 if (j==1 && k==1 && v6==0){ 00089 for (i=1;i<20;i++){ 00090 pid=999; 00091 Vout=(pid/999); 00092 v2=(Vin.read())*3300;//la medicion es convertida a un valor en mV,siendo el máximo valor esperado 3300mV (3.3V) 00093 float kp0; 00094 sp0 = sp; 00095 err = (sp0*1000-v2); //Estoy ingresando un valor en V, pero opero internamente en mV 00096 kp0 = Kp; 00097 ap = kp0*err; 00098 pid = ap; 00099 00100 if (pid>999){pid=999;} 00101 if (pid<0){pid=0;} 00102 00103 Vout=(pid/999); 00104 00105 lcd.locate(5,1); 00106 lcd.printf("Leyendo.."); 00107 D[i]=v2/1000; 00108 00109 led1=0; 00110 wait(1); 00111 led1=1; 00112 } 00113 } 00114 lcd.cls(); 00115 if (i==20 && k==1 && v6==0){ 00116 00117 for (j=1;j<20;j=j+2) { 00118 // lcd.locate(0,0); 00119 // lcd.printf("j %d",j); 00120 00121 if (D[j]==D[j+2]){ 00122 led2=0; 00123 q=q++; 00124 //lcd.locate(8,0); 00125 //lcd.printf("Q %d",q); 00126 lcd.locate(5,1); 00127 lcd.printf("Operando.."); 00128 wait(1); 00129 00130 } 00131 } 00132 00133 } 00134 lcd.cls(); 00135 00136 if(j==21 && i==20 && k!=21 && v6==0){ 00137 for(k=1;k<20;k=k+2) { 00138 // lcd.locate(0,0); 00139 // lcd.printf("k %d",k); 00140 if (D[k+1]==D[k+3]){ 00141 led3=0; 00142 led2=1; 00143 q2=q2++; 00144 //lcd.locate(0,1); 00145 //lcd.printf("Q2 %d",q2); 00146 lcd.locate(5,1); 00147 lcd.printf("Analisis..."); 00148 wait(1);} 00149 00150 } 00151 00152 } 00153 00154 lcd.cls(); 00155 00156 00157 if (i==20 && j==21 && k==21 && q>8 && q2>8){ 00158 led2=0; 00159 kc=Kp; 00160 p=3; 00161 v6=1; 00162 lcd.locate(5,1); 00163 lcd.printf("Terminado"); 00164 wait(1); 00165 lcd.cls(); 00166 lcd.printf("KC: %4.2f",Kp); 00167 wait(1); 00168 00169 break; 00170 } 00171 if(i==20 && j>20 && k==21 && q<=8 && q2<=8){ 00172 lcd.locate(5,1); 00173 lcd.printf("Terminado"); 00174 wait(1); 00175 lcd.cls(); 00176 lcd.locate(5,1); 00177 lcd.printf("NO ES LA KC"); 00178 wait(1); 00179 lcd.cls(); 00180 00181 i=1; 00182 j=1; 00183 k=1; 00184 q=0; 00185 q2=0; 00186 } 00187 } 00188 00189 } 00190 00191 if (v6==1){ 00192 lcd.cls(); 00193 lcd.locate(5,1); 00194 lcd.printf("Calculando.."); 00195 TC=1; 00196 Kp=0.59*kc; 00197 Ki=1.18*kc*(1/TC); 00198 Kd=0.074*kc*TC; 00199 00200 lcd.cls(); 00201 lcd.printf("INICIA EL PID"); 00202 wait(2); 00203 // se imprimen los parches del control ***************************************** 00204 00205 lcd.locate(0,0); 00206 lcd.printf("E:%d"); 00207 lcd.locate(8,0); 00208 lcd.printf("Y:%d"); 00209 lcd.locate(0,1); 00210 lcd.printf("Sp:%d"); 00211 lcd.locate(8,1); 00212 lcd.printf("Co:%0.1f"); 00213 // CICLO PRINCIPAL CONTROLADOR PID 00214 00215 while(1) { 00216 00217 wait(0.001); 00218 // 00219 v2 = (Vin.read()*3300);//la medicion es convertida a un valor en mV,siendo el máximo valor esperado 3300mV (3.3V) 00220 sp0 = sp; 00221 err = (sp0*1000-v2); //Estoy ingresando un valor en V, pero opero internamente en mV 00222 float kp0; 00223 kp0 = Kp; 00224 ap = kp0*err; 00225 float ki0; 00226 ki0 = Ki; 00227 ai = (ki0*err)+ai; //calculo de la integral del error 00228 float kd0; 00229 kd0 =Kd; 00230 ad = kd0*(err-err_v); //calculo de la accion derivativa 00231 pid = (ap+ai+ad); 00232 // se verifica que pid sea menor o igual la valor maximo ***************** 00233 if (pid > 999){ pid=999;} 00234 // se verifica que pid sea positivo ************************************** 00235 if (pid < 0){ pid=0;} 00236 // se verifica que la accion integral no sea muy grande 00237 if (ai > 999){ai=1000;} 00238 // 00239 00240 Vout=(pid/999); 00241 //Mostrar resultados PID 00242 if(ciclo>700) { 00243 lcd.locate(2,0); 00244 lcd.printf(" "); 00245 lcd.locate(0,0); 00246 lcd.printf("E:%4.2f",err/1000); 00247 lcd.locate(10,0); 00248 lcd.printf(" "); 00249 lcd.locate(8,0); 00250 lcd.printf("Y:%4.2f",v2/1000); 00251 lcd.locate(2,1); 00252 lcd.printf(" "); 00253 lcd.locate(0,1); 00254 lcd.printf("Sp:%4.2f",sp0); 00255 lcd.locate(10,1); 00256 lcd.printf(" "); 00257 lcd.locate(11,1); 00258 lcd.printf(":%0.1f",pid*3.3/999); 00259 ciclo=0; 00260 } 00261 else 00262 ciclo++; 00263 err_v = err; //guarda el error 00264 } // Envía parámetro pid al puerto analogico de salida (D/A) y se repite el cicl 00265 } 00266 } 00267 else{ 00268 if (AT==0){ 00269 00270 if (bot1.falling()&&B==0 && kp==0 && ki==0 && kd==0) { //----------------- Aumenta de posición el cursor a la primera línea de menu 00271 00272 00273 led1=0; 00274 wait(.15); 00275 led1=1; 00276 ++a; 00277 } 00278 00279 valor = Encoder.getPulses(); //------------- Asigna el valor de los pulsos del encoder a una variable llamada "valor" 00280 if(B==0 && t2==0){ 00281 switch(a) { 00282 00283 case 0: 00284 00285 sp = sp + valor; //------------- Asigna el valor del encoder al parámetro sp y tiene en cuenta el valor anterior 00286 Encoder.reset(); //------------- Resetea el valor del encoder 00287 if (sp<0){ 00288 sp=0; //------------- No se admite valores negativos 00289 } 00290 if (sp>3){ //Limito el valor del Set Point; 00291 sp=3; 00292 } 00293 00294 lcd.locate(2,0); //------------- Ubica e imprime el parámetro "sp" en la pantalla LCD 00295 lcd.printf("= ",sp); 00296 lcd.locate(3,0); 00297 lcd.printf("%i",sp); 00298 wait(0.15); 00299 00300 if (bot1.falling() && sp>0 && a==0 &&B==0) { //--------------Aumenta de posición el cursor a la segunda línea de menu 00301 00302 a=1; 00303 led1=0; 00304 wait(.15); 00305 led1=1; 00306 Encoder.reset(); //------------- Resetea el valor del encoder 00307 } 00308 break; 00309 00310 case 1: 00311 00312 kp = kp + valor; //------------- Asigna el valor del encoder al parámetro kp y tiene en cuenta el valor anterior 00313 Encoder.reset(); //------------- Resetea el valor del encoder 00314 if (kp<0){ 00315 kp=0; //------------- No se admite valores negativos 00316 } 00317 if (kp>999){ kp=999;} //Limito el valor de KP 00318 00319 lcd.locate(10,0); //------------- Ubica e imprime el parámetro "sp" en la pantalla LCD 00320 lcd.printf("= ",kp); 00321 lcd.locate(11,0); 00322 lcd.printf("%i",kp); 00323 wait(0.15); 00324 00325 if (bot1.falling()&& kp>0 && a==1 && B==0) { //--------------Aumenta de posición el cursor a la tercera línea de menu 00326 00327 00328 00329 a=2; 00330 led1=0; 00331 wait(.15); 00332 led1=1; 00333 Encoder.reset(); //------------- Resetea el valor del encoder 00334 } 00335 break; 00336 00337 case 2: 00338 00339 ki = ki + valor; //------------- Asigna el valor del encoder al parámetro ki y tiene en cuenta el valor anterior 00340 Encoder.reset(); //------------- Resetea el valor del encoder 00341 if (ki<0){ 00342 ki=0; //------------- No se admite valores negativos 00343 } 00344 if (ki>999){ ki=999;} //Limito el valor de KI 00345 lcd.locate(2,1); //------------- Ubica e imprime el parámetro "sp" en la pantalla LCD 00346 lcd.printf("= ",ki); 00347 lcd.locate(3,1); 00348 lcd.printf("%i",ki); 00349 wait(0.15); 00350 00351 if (bot1.falling()&&ki>0 && a==2 && B==0) { //--------------Aumenta de posición el cursor a la cuarta línea de menu 00352 00353 a=3; 00354 led1=0; 00355 wait(.15); 00356 led1=1; 00357 Encoder.reset(); //------------- Resetea el valor del encoder 00358 } 00359 break; 00360 00361 case 3: 00362 kd = kd + valor; //------------- Asigna el valor del encoder al parámetro kd y tiene en cuenta el valor anterior 00363 Encoder.reset(); //------------- Resetea el valor del encoder 00364 if (kd<0){ 00365 kd=0; //------------- No se admite valores negativos 00366 } 00367 if (kd>999){ kd=999;} //Limito el valor de KD 00368 lcd.locate(10,1);//------------- Ubica e imprime el parámetro "kd" en la pantalla LCD 00369 lcd.printf("= ",kd); 00370 lcd.locate(11,1); 00371 lcd.printf("%i",kd); 00372 wait(0.15); 00373 00374 00375 if (bot1.falling()&&kd>0 &&a==3 && B==0) { //--------------Aumenta de posición el cursor a la cuarta línea de menu 00376 00377 00378 a=0; 00379 led1=0; 00380 wait(.15); 00381 led1=1; 00382 Encoder.reset(); //------------- Resetea el valor del encoder 00383 } 00384 break; 00385 } 00386 } 00387 //PARA GUARDAR LOS PARAMETROS SE PRESIONA EL PULSADOR DEEL ENCODER 5 SEG 00388 if (bot1==0 && kp>0 && ki>0 && kd>0) 00389 { 00390 t.start(); //inicio el timer 00391 t2=t.read();//leo el valor del timer y lo almaceno en una variable entera "t2" 00392 B=1; 00393 00394 00395 if(t2==5){ 00396 t.stop(); //paro el timer 00397 t.reset();//lo reseteo 00398 led3=0; //led indicador-- enciendo led azul 00399 B=0; 00400 lcd.cls(); 00401 } 00402 00403 if (t2==5 && B==0) 00404 { 00405 lcd.cls(); //borra la pantalla 00406 lcd.printf("GUARDADOS!"); 00407 wait(2); 00408 lcd.cls(); 00409 lcd.printf("INICIA EL PID"); 00410 wait(2); 00411 lcd.cls(); 00412 //Se imprimen los parches de control 00413 lcd.printf("E=%d"); //Error 00414 lcd.locate(8,0); 00415 lcd.printf("Y=%d"); //Salida 00416 lcd.locate(0,1); 00417 lcd.printf("Sp=%d"); //Set point 00418 lcd.locate(8,1); 00419 lcd.printf("Co=%3.0f"); //Esfuerzo de control 00420 wait(1); 00421 00422 00423 //CICLO PID 00424 00425 while (true){ 00426 wait(0.001); 00427 med = (Vin.read()*3300); //la medicion es convertida a un valor en mV,siendo el máximo valor esperado 3300mV (3.3V) 00428 sp0 = sp; 00429 err = (sp0*1000-med); //Estoy ingresando un valor en V, pero opero internamente en mV 00430 float kp0; 00431 kp0 = kp; 00432 ap = kp0*err; 00433 float ki0; 00434 ki0 = ki; 00435 ai = (ki0*err)+ai; //calculo de la integral del error 00436 float kd0; 00437 kd0 = kd; 00438 ad = kd0*(err_v-err); //calculo de la accion derivativa 00439 pid = (ap+ai+ad); 00440 00441 00442 // se verifica que pid sea positivo 00443 if (pid < 0){ 00444 pid=0;} 00445 // Se limita el valor del PID 00446 if (pid > 999) 00447 { pid=999; } 00448 00449 // se verifica que la accion integral no sea muy grande 00450 if (ai > 999){ 00451 ai=1000; 00452 } 00453 Vout=(pid/999); 00454 00455 //Mostrar resultados PID 00456 if(ciclo>700) { 00457 lcd.locate(2,0); 00458 lcd.printf(" "); 00459 lcd.locate(0,0); 00460 lcd.printf("E=%4.2f",err/1000); 00461 lcd.locate(10,0); 00462 lcd.printf(" "); 00463 lcd.locate(8,0); 00464 lcd.printf("Y=%4.2f",med/1000); 00465 lcd.locate(2,1); 00466 lcd.printf(" "); 00467 lcd.locate(0,1); 00468 lcd.printf("Sp=%4.2f",sp0); 00469 lcd.locate(11,1); 00470 lcd.printf(" "); 00471 lcd.locate(11,1); 00472 lcd.printf("%0.1f ",pid*3.3/999); 00473 00474 ciclo=0; 00475 } 00476 else 00477 ciclo++; 00478 err_v = err; //guarda el error 00479 00480 // se repite el ciclo 00481 00482 } 00483 } 00484 } 00485 } 00486 } 00487 } 00488 } 00489 00490 00491 00492 00493
Generated on Thu Jul 14 2022 05:01:52 by
1.7.2