![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
PID que funciona con pulsadores y tiene autotunning.
Dependencies: Debounced QEI TextLCD mbed
PID.CPP@0:2d05e443a44d, 2017-11-28 (annotated)
- Committer:
- PROCESADORES_2017_2
- Date:
- Tue Nov 28 19:48:57 2017 +0000
- Revision:
- 0:2d05e443a44d
PID que funciona con pulsadores y tiene autotunning.
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
PROCESADORES_2017_2 | 0:2d05e443a44d | 1 | #include "mbed.h" |
PROCESADORES_2017_2 | 0:2d05e443a44d | 2 | #include "TextLCD.h" |
PROCESADORES_2017_2 | 0:2d05e443a44d | 3 | #include "DebouncedIn.h" |
PROCESADORES_2017_2 | 0:2d05e443a44d | 4 | |
PROCESADORES_2017_2 | 0:2d05e443a44d | 5 | TextLCD lcd(PTB8, PTB9,PTB10,PTB11,PTE2, PTE3); |
PROCESADORES_2017_2 | 0:2d05e443a44d | 6 | |
PROCESADORES_2017_2 | 0:2d05e443a44d | 7 | AnalogIn Vin(PTC2); |
PROCESADORES_2017_2 | 0:2d05e443a44d | 8 | AnalogOut Vout(PTE30); |
PROCESADORES_2017_2 | 0:2d05e443a44d | 9 | |
PROCESADORES_2017_2 | 0:2d05e443a44d | 10 | DigitalOut led1(LED1); //led de cambio de posición |
PROCESADORES_2017_2 | 0:2d05e443a44d | 11 | DigitalOut led2(LED2); //led incremento de parámetros |
PROCESADORES_2017_2 | 0:2d05e443a44d | 12 | DigitalOut led3(LED3); //led decremento de parámetros |
PROCESADORES_2017_2 | 0:2d05e443a44d | 13 | |
PROCESADORES_2017_2 | 0:2d05e443a44d | 14 | DebouncedIn P1(PTC16); //Pulsador que aumenta |
PROCESADORES_2017_2 | 0:2d05e443a44d | 15 | DebouncedIn P2(PTC13); //SET |
PROCESADORES_2017_2 | 0:2d05e443a44d | 16 | DebouncedIn P3(PTC12); //Pulsador que disminuye |
PROCESADORES_2017_2 | 0:2d05e443a44d | 17 | int t2; |
PROCESADORES_2017_2 | 0:2d05e443a44d | 18 | int o2=0; |
PROCESADORES_2017_2 | 0:2d05e443a44d | 19 | float t1; |
PROCESADORES_2017_2 | 0:2d05e443a44d | 20 | int B; |
PROCESADORES_2017_2 | 0:2d05e443a44d | 21 | |
PROCESADORES_2017_2 | 0:2d05e443a44d | 22 | float ko,Kp,Ki,Kd,kc,TC; |
PROCESADORES_2017_2 | 0:2d05e443a44d | 23 | int AT=0; |
PROCESADORES_2017_2 | 0:2d05e443a44d | 24 | int i=1; |
PROCESADORES_2017_2 | 0:2d05e443a44d | 25 | int j=1; |
PROCESADORES_2017_2 | 0:2d05e443a44d | 26 | int k=1; |
PROCESADORES_2017_2 | 0:2d05e443a44d | 27 | int v6; |
PROCESADORES_2017_2 | 0:2d05e443a44d | 28 | int q; |
PROCESADORES_2017_2 | 0:2d05e443a44d | 29 | int q2; |
PROCESADORES_2017_2 | 0:2d05e443a44d | 30 | char D[30]; |
PROCESADORES_2017_2 | 0:2d05e443a44d | 31 | float v2; |
PROCESADORES_2017_2 | 0:2d05e443a44d | 32 | //------------------------------------------------------------------------------ |
PROCESADORES_2017_2 | 0:2d05e443a44d | 33 | int a, sp, kp, ki, kd, ciclo; // indice de la variable |
PROCESADORES_2017_2 | 0:2d05e443a44d | 34 | float med, sp0, ap, err, ai, ad, pid, err_v; |
PROCESADORES_2017_2 | 0:2d05e443a44d | 35 | |
PROCESADORES_2017_2 | 0:2d05e443a44d | 36 | //Códigos LCD |
PROCESADORES_2017_2 | 0:2d05e443a44d | 37 | int C1=0x0E; // Muestra el cursor |
PROCESADORES_2017_2 | 0:2d05e443a44d | 38 | // 0x18; // desplazamiento izquierda |
PROCESADORES_2017_2 | 0:2d05e443a44d | 39 | // 0x1A; // desplazamiento derecha |
PROCESADORES_2017_2 | 0:2d05e443a44d | 40 | int C4=0x0C; // Quita el cursor |
PROCESADORES_2017_2 | 0:2d05e443a44d | 41 | |
PROCESADORES_2017_2 | 0:2d05e443a44d | 42 | |
PROCESADORES_2017_2 | 0:2d05e443a44d | 43 | Timer t; |
PROCESADORES_2017_2 | 0:2d05e443a44d | 44 | |
PROCESADORES_2017_2 | 0:2d05e443a44d | 45 | int p=1; |
PROCESADORES_2017_2 | 0:2d05e443a44d | 46 | int main() |
PROCESADORES_2017_2 | 0:2d05e443a44d | 47 | { |
PROCESADORES_2017_2 | 0:2d05e443a44d | 48 | led1=led2=led3=1; |
PROCESADORES_2017_2 | 0:2d05e443a44d | 49 | lcd.cls(); //limpio la pantalla |
PROCESADORES_2017_2 | 0:2d05e443a44d | 50 | lcd.locate(5,1); |
PROCESADORES_2017_2 | 0:2d05e443a44d | 51 | lcd.printf("BIENVENIDO"); |
PROCESADORES_2017_2 | 0:2d05e443a44d | 52 | wait(1.5); |
PROCESADORES_2017_2 | 0:2d05e443a44d | 53 | lcd.cls(); |
PROCESADORES_2017_2 | 0:2d05e443a44d | 54 | //lcd.writeCommand(int C1); //Comando para mostrar el cursor en el LCD |
PROCESADORES_2017_2 | 0:2d05e443a44d | 55 | lcd.locate(0,0); //Ubica e imprime nombre de las variables en la LCD |
PROCESADORES_2017_2 | 0:2d05e443a44d | 56 | lcd.printf("Sp=%d",sp); |
PROCESADORES_2017_2 | 0:2d05e443a44d | 57 | lcd.locate(8,0); |
PROCESADORES_2017_2 | 0:2d05e443a44d | 58 | lcd.printf("Kp=%d",kp); |
PROCESADORES_2017_2 | 0:2d05e443a44d | 59 | lcd.locate(0,1); |
PROCESADORES_2017_2 | 0:2d05e443a44d | 60 | lcd.printf("Ki=%d",ki); |
PROCESADORES_2017_2 | 0:2d05e443a44d | 61 | lcd.locate(8,1); |
PROCESADORES_2017_2 | 0:2d05e443a44d | 62 | lcd.printf("Kd=%d",kd); |
PROCESADORES_2017_2 | 0:2d05e443a44d | 63 | |
PROCESADORES_2017_2 | 0:2d05e443a44d | 64 | while(true){ if ((P1.rising())&&(P3.rising())){ |
PROCESADORES_2017_2 | 0:2d05e443a44d | 65 | AT=1; |
PROCESADORES_2017_2 | 0:2d05e443a44d | 66 | if (AT==1){ |
PROCESADORES_2017_2 | 0:2d05e443a44d | 67 | if(p==1){ |
PROCESADORES_2017_2 | 0:2d05e443a44d | 68 | lcd.cls(); //limpio la pantalla |
PROCESADORES_2017_2 | 0:2d05e443a44d | 69 | lcd.locate(5,1); |
PROCESADORES_2017_2 | 0:2d05e443a44d | 70 | lcd.printf("AUTOTUNING"); |
PROCESADORES_2017_2 | 0:2d05e443a44d | 71 | wait(1.5); |
PROCESADORES_2017_2 | 0:2d05e443a44d | 72 | lcd.cls(); |
PROCESADORES_2017_2 | 0:2d05e443a44d | 73 | |
PROCESADORES_2017_2 | 0:2d05e443a44d | 74 | sp=2; //Set point inicial |
PROCESADORES_2017_2 | 0:2d05e443a44d | 75 | p=2; |
PROCESADORES_2017_2 | 0:2d05e443a44d | 76 | } |
PROCESADORES_2017_2 | 0:2d05e443a44d | 77 | if (p==2){ |
PROCESADORES_2017_2 | 0:2d05e443a44d | 78 | for (Kp=0.5;Kp<2;Kp=Kp+0.1){ |
PROCESADORES_2017_2 | 0:2d05e443a44d | 79 | |
PROCESADORES_2017_2 | 0:2d05e443a44d | 80 | if (j==1 && k==1 && v6==0){ |
PROCESADORES_2017_2 | 0:2d05e443a44d | 81 | for (i=1;i<20;i++){ |
PROCESADORES_2017_2 | 0:2d05e443a44d | 82 | pid=999; |
PROCESADORES_2017_2 | 0:2d05e443a44d | 83 | Vout=(pid/999); |
PROCESADORES_2017_2 | 0:2d05e443a44d | 84 | v2=(Vin.read())*3300;//la medicion es convertida a un valor en mV,siendo el máximo valor esperado 3300mV (3.3V) |
PROCESADORES_2017_2 | 0:2d05e443a44d | 85 | float kp0; |
PROCESADORES_2017_2 | 0:2d05e443a44d | 86 | sp0 = sp; |
PROCESADORES_2017_2 | 0:2d05e443a44d | 87 | err = (sp0*1000-v2); //Estoy ingresando un valor en V, pero opero internamente en mV |
PROCESADORES_2017_2 | 0:2d05e443a44d | 88 | kp0 = Kp; |
PROCESADORES_2017_2 | 0:2d05e443a44d | 89 | ap = kp0*err; |
PROCESADORES_2017_2 | 0:2d05e443a44d | 90 | pid = ap; |
PROCESADORES_2017_2 | 0:2d05e443a44d | 91 | |
PROCESADORES_2017_2 | 0:2d05e443a44d | 92 | if (pid>999){pid=999;} |
PROCESADORES_2017_2 | 0:2d05e443a44d | 93 | if (pid<0){pid=0;} |
PROCESADORES_2017_2 | 0:2d05e443a44d | 94 | |
PROCESADORES_2017_2 | 0:2d05e443a44d | 95 | Vout=(pid/999); |
PROCESADORES_2017_2 | 0:2d05e443a44d | 96 | // lcd.locate(5,1); |
PROCESADORES_2017_2 | 0:2d05e443a44d | 97 | // lcd.printf("y %4.2f",v2/1000); |
PROCESADORES_2017_2 | 0:2d05e443a44d | 98 | // lcd.locate(0,0); |
PROCESADORES_2017_2 | 0:2d05e443a44d | 99 | // lcd.printf("i %d",i); |
PROCESADORES_2017_2 | 0:2d05e443a44d | 100 | lcd.locate(5,1); |
PROCESADORES_2017_2 | 0:2d05e443a44d | 101 | lcd.printf("Leyendo.."); |
PROCESADORES_2017_2 | 0:2d05e443a44d | 102 | D[i]=v2/1000; |
PROCESADORES_2017_2 | 0:2d05e443a44d | 103 | |
PROCESADORES_2017_2 | 0:2d05e443a44d | 104 | led1=0; |
PROCESADORES_2017_2 | 0:2d05e443a44d | 105 | wait(1); |
PROCESADORES_2017_2 | 0:2d05e443a44d | 106 | led1=1; |
PROCESADORES_2017_2 | 0:2d05e443a44d | 107 | } |
PROCESADORES_2017_2 | 0:2d05e443a44d | 108 | } |
PROCESADORES_2017_2 | 0:2d05e443a44d | 109 | lcd.cls(); |
PROCESADORES_2017_2 | 0:2d05e443a44d | 110 | if (i==20 && k==1 && v6==0){ |
PROCESADORES_2017_2 | 0:2d05e443a44d | 111 | |
PROCESADORES_2017_2 | 0:2d05e443a44d | 112 | for (j=1;j<20;j=j+2) { |
PROCESADORES_2017_2 | 0:2d05e443a44d | 113 | //lcd.locate(0,0); |
PROCESADORES_2017_2 | 0:2d05e443a44d | 114 | //lcd.printf("j %d",j); |
PROCESADORES_2017_2 | 0:2d05e443a44d | 115 | |
PROCESADORES_2017_2 | 0:2d05e443a44d | 116 | if (D[j]==D[j+2]){ |
PROCESADORES_2017_2 | 0:2d05e443a44d | 117 | led2=0; |
PROCESADORES_2017_2 | 0:2d05e443a44d | 118 | q=q++; |
PROCESADORES_2017_2 | 0:2d05e443a44d | 119 | lcd.locate(5,1); |
PROCESADORES_2017_2 | 0:2d05e443a44d | 120 | lcd.printf("Operando.."); |
PROCESADORES_2017_2 | 0:2d05e443a44d | 121 | wait(1); |
PROCESADORES_2017_2 | 0:2d05e443a44d | 122 | } |
PROCESADORES_2017_2 | 0:2d05e443a44d | 123 | } |
PROCESADORES_2017_2 | 0:2d05e443a44d | 124 | |
PROCESADORES_2017_2 | 0:2d05e443a44d | 125 | } |
PROCESADORES_2017_2 | 0:2d05e443a44d | 126 | lcd.cls(); |
PROCESADORES_2017_2 | 0:2d05e443a44d | 127 | |
PROCESADORES_2017_2 | 0:2d05e443a44d | 128 | if(j==21 && i==20 && k!=21 && v6==0){ |
PROCESADORES_2017_2 | 0:2d05e443a44d | 129 | for(k=1;k<20;k=k+2) { |
PROCESADORES_2017_2 | 0:2d05e443a44d | 130 | //lcd.locate(0,0); |
PROCESADORES_2017_2 | 0:2d05e443a44d | 131 | // lcd.printf("k %d",k); |
PROCESADORES_2017_2 | 0:2d05e443a44d | 132 | if (D[k+1]==D[k+3]){ |
PROCESADORES_2017_2 | 0:2d05e443a44d | 133 | led3=0; |
PROCESADORES_2017_2 | 0:2d05e443a44d | 134 | led2=1; |
PROCESADORES_2017_2 | 0:2d05e443a44d | 135 | q2=q2++; |
PROCESADORES_2017_2 | 0:2d05e443a44d | 136 | lcd.locate(5,1); |
PROCESADORES_2017_2 | 0:2d05e443a44d | 137 | lcd.printf("Analisis..."); |
PROCESADORES_2017_2 | 0:2d05e443a44d | 138 | wait(1);} |
PROCESADORES_2017_2 | 0:2d05e443a44d | 139 | |
PROCESADORES_2017_2 | 0:2d05e443a44d | 140 | } |
PROCESADORES_2017_2 | 0:2d05e443a44d | 141 | |
PROCESADORES_2017_2 | 0:2d05e443a44d | 142 | } |
PROCESADORES_2017_2 | 0:2d05e443a44d | 143 | |
PROCESADORES_2017_2 | 0:2d05e443a44d | 144 | lcd.cls(); |
PROCESADORES_2017_2 | 0:2d05e443a44d | 145 | |
PROCESADORES_2017_2 | 0:2d05e443a44d | 146 | |
PROCESADORES_2017_2 | 0:2d05e443a44d | 147 | if (i==20 && j==21 && k==21 && q>8 && q2>8){ |
PROCESADORES_2017_2 | 0:2d05e443a44d | 148 | led2=0; |
PROCESADORES_2017_2 | 0:2d05e443a44d | 149 | kc=Kp; |
PROCESADORES_2017_2 | 0:2d05e443a44d | 150 | p=3; |
PROCESADORES_2017_2 | 0:2d05e443a44d | 151 | v6=1; |
PROCESADORES_2017_2 | 0:2d05e443a44d | 152 | lcd.locate(5,1); |
PROCESADORES_2017_2 | 0:2d05e443a44d | 153 | lcd.printf("Terminado"); |
PROCESADORES_2017_2 | 0:2d05e443a44d | 154 | wait(1); |
PROCESADORES_2017_2 | 0:2d05e443a44d | 155 | lcd.cls(); |
PROCESADORES_2017_2 | 0:2d05e443a44d | 156 | lcd.printf("KC: %4.2f",Kp); |
PROCESADORES_2017_2 | 0:2d05e443a44d | 157 | wait(1); |
PROCESADORES_2017_2 | 0:2d05e443a44d | 158 | //UTILIZO A ESTE KP COMO GANANCIA CRITICA |
PROCESADORES_2017_2 | 0:2d05e443a44d | 159 | |
PROCESADORES_2017_2 | 0:2d05e443a44d | 160 | break; |
PROCESADORES_2017_2 | 0:2d05e443a44d | 161 | } |
PROCESADORES_2017_2 | 0:2d05e443a44d | 162 | if(i==20 && j>20 && k==21 && q<=8 && q2<=8){ |
PROCESADORES_2017_2 | 0:2d05e443a44d | 163 | lcd.locate(5,1); |
PROCESADORES_2017_2 | 0:2d05e443a44d | 164 | lcd.printf("Terminado"); |
PROCESADORES_2017_2 | 0:2d05e443a44d | 165 | wait(1); |
PROCESADORES_2017_2 | 0:2d05e443a44d | 166 | lcd.cls(); |
PROCESADORES_2017_2 | 0:2d05e443a44d | 167 | lcd.printf("NO ES LA KC"); |
PROCESADORES_2017_2 | 0:2d05e443a44d | 168 | wait(1); |
PROCESADORES_2017_2 | 0:2d05e443a44d | 169 | lcd.cls(); |
PROCESADORES_2017_2 | 0:2d05e443a44d | 170 | i=1; |
PROCESADORES_2017_2 | 0:2d05e443a44d | 171 | j=1; |
PROCESADORES_2017_2 | 0:2d05e443a44d | 172 | k=1; |
PROCESADORES_2017_2 | 0:2d05e443a44d | 173 | q=0; |
PROCESADORES_2017_2 | 0:2d05e443a44d | 174 | q2=0; |
PROCESADORES_2017_2 | 0:2d05e443a44d | 175 | } |
PROCESADORES_2017_2 | 0:2d05e443a44d | 176 | } |
PROCESADORES_2017_2 | 0:2d05e443a44d | 177 | |
PROCESADORES_2017_2 | 0:2d05e443a44d | 178 | } |
PROCESADORES_2017_2 | 0:2d05e443a44d | 179 | } |
PROCESADORES_2017_2 | 0:2d05e443a44d | 180 | if (v6==1){ |
PROCESADORES_2017_2 | 0:2d05e443a44d | 181 | TC=1; |
PROCESADORES_2017_2 | 0:2d05e443a44d | 182 | Kp=0.59*kc; |
PROCESADORES_2017_2 | 0:2d05e443a44d | 183 | Ki=1.18*kc*(1/TC); |
PROCESADORES_2017_2 | 0:2d05e443a44d | 184 | Kd=0.074*kc*TC; |
PROCESADORES_2017_2 | 0:2d05e443a44d | 185 | |
PROCESADORES_2017_2 | 0:2d05e443a44d | 186 | lcd.cls(); |
PROCESADORES_2017_2 | 0:2d05e443a44d | 187 | lcd.printf("INICIA EL PID"); |
PROCESADORES_2017_2 | 0:2d05e443a44d | 188 | wait(2); |
PROCESADORES_2017_2 | 0:2d05e443a44d | 189 | // se imprimen los parches del control ***************************************** |
PROCESADORES_2017_2 | 0:2d05e443a44d | 190 | |
PROCESADORES_2017_2 | 0:2d05e443a44d | 191 | lcd.locate(0,0); |
PROCESADORES_2017_2 | 0:2d05e443a44d | 192 | lcd.printf("E:%d"); |
PROCESADORES_2017_2 | 0:2d05e443a44d | 193 | lcd.locate(8,0); |
PROCESADORES_2017_2 | 0:2d05e443a44d | 194 | lcd.printf("Y:%d"); |
PROCESADORES_2017_2 | 0:2d05e443a44d | 195 | lcd.locate(0,1); |
PROCESADORES_2017_2 | 0:2d05e443a44d | 196 | lcd.printf("Sp:%d"); |
PROCESADORES_2017_2 | 0:2d05e443a44d | 197 | lcd.locate(8,1); |
PROCESADORES_2017_2 | 0:2d05e443a44d | 198 | lcd.printf("Co:%0.1f"); |
PROCESADORES_2017_2 | 0:2d05e443a44d | 199 | // CICLO PRINCIPAL CONTROLADOR PID |
PROCESADORES_2017_2 | 0:2d05e443a44d | 200 | |
PROCESADORES_2017_2 | 0:2d05e443a44d | 201 | while(1) { |
PROCESADORES_2017_2 | 0:2d05e443a44d | 202 | |
PROCESADORES_2017_2 | 0:2d05e443a44d | 203 | wait(0.001); |
PROCESADORES_2017_2 | 0:2d05e443a44d | 204 | // |
PROCESADORES_2017_2 | 0:2d05e443a44d | 205 | v2 = (Vin.read()*3300);//la medicion es convertida a un valor en mV,siendo el máximo valor esperado 3300mV (3.3V) |
PROCESADORES_2017_2 | 0:2d05e443a44d | 206 | sp0 = sp; |
PROCESADORES_2017_2 | 0:2d05e443a44d | 207 | err = (sp0*1000-v2); //Estoy ingresando un valor en V, pero opero internamente en mV |
PROCESADORES_2017_2 | 0:2d05e443a44d | 208 | float kp0; |
PROCESADORES_2017_2 | 0:2d05e443a44d | 209 | kp0 = Kp; |
PROCESADORES_2017_2 | 0:2d05e443a44d | 210 | ap = kp0*err; |
PROCESADORES_2017_2 | 0:2d05e443a44d | 211 | float ki0; |
PROCESADORES_2017_2 | 0:2d05e443a44d | 212 | ki0 = Ki; |
PROCESADORES_2017_2 | 0:2d05e443a44d | 213 | ai = (ki0*err)+ai; //calculo de la integral del error |
PROCESADORES_2017_2 | 0:2d05e443a44d | 214 | float kd0; |
PROCESADORES_2017_2 | 0:2d05e443a44d | 215 | kd0 =Kd; |
PROCESADORES_2017_2 | 0:2d05e443a44d | 216 | ad = kd0*(err-err_v); //calculo de la accion derivativa |
PROCESADORES_2017_2 | 0:2d05e443a44d | 217 | pid = (ap+ai+ad); |
PROCESADORES_2017_2 | 0:2d05e443a44d | 218 | // se verifica que pid sea menor o igual la valor maximo ***************** |
PROCESADORES_2017_2 | 0:2d05e443a44d | 219 | if (pid > 999){ |
PROCESADORES_2017_2 | 0:2d05e443a44d | 220 | pid=999; |
PROCESADORES_2017_2 | 0:2d05e443a44d | 221 | } // se verifica que pid sea positivo ************************************** |
PROCESADORES_2017_2 | 0:2d05e443a44d | 222 | if (pid < 0){ |
PROCESADORES_2017_2 | 0:2d05e443a44d | 223 | pid=0; |
PROCESADORES_2017_2 | 0:2d05e443a44d | 224 | } |
PROCESADORES_2017_2 | 0:2d05e443a44d | 225 | // se verifica que la accion integral no sea muy grande |
PROCESADORES_2017_2 | 0:2d05e443a44d | 226 | if (ai > 999){ai=1000; |
PROCESADORES_2017_2 | 0:2d05e443a44d | 227 | } |
PROCESADORES_2017_2 | 0:2d05e443a44d | 228 | // |
PROCESADORES_2017_2 | 0:2d05e443a44d | 229 | Vout=(pid/999); |
PROCESADORES_2017_2 | 0:2d05e443a44d | 230 | //Mostrar resultados PID |
PROCESADORES_2017_2 | 0:2d05e443a44d | 231 | if(ciclo>700) { |
PROCESADORES_2017_2 | 0:2d05e443a44d | 232 | lcd.locate(2,0); |
PROCESADORES_2017_2 | 0:2d05e443a44d | 233 | lcd.printf(" "); |
PROCESADORES_2017_2 | 0:2d05e443a44d | 234 | lcd.locate(0,0); |
PROCESADORES_2017_2 | 0:2d05e443a44d | 235 | lcd.printf("E:%4.2f",err/1000); |
PROCESADORES_2017_2 | 0:2d05e443a44d | 236 | lcd.locate(10,0); |
PROCESADORES_2017_2 | 0:2d05e443a44d | 237 | lcd.printf(" "); |
PROCESADORES_2017_2 | 0:2d05e443a44d | 238 | lcd.locate(8,0); |
PROCESADORES_2017_2 | 0:2d05e443a44d | 239 | lcd.printf("Y:%4.2f",v2/1000); |
PROCESADORES_2017_2 | 0:2d05e443a44d | 240 | lcd.locate(2,1); |
PROCESADORES_2017_2 | 0:2d05e443a44d | 241 | lcd.printf(" "); |
PROCESADORES_2017_2 | 0:2d05e443a44d | 242 | lcd.locate(0,1); |
PROCESADORES_2017_2 | 0:2d05e443a44d | 243 | lcd.printf("Sp:%4.2f",sp0); |
PROCESADORES_2017_2 | 0:2d05e443a44d | 244 | lcd.locate(10,1); |
PROCESADORES_2017_2 | 0:2d05e443a44d | 245 | lcd.printf(" "); |
PROCESADORES_2017_2 | 0:2d05e443a44d | 246 | lcd.locate(11,1); |
PROCESADORES_2017_2 | 0:2d05e443a44d | 247 | lcd.printf(":%0.1f",pid*3.3/999); |
PROCESADORES_2017_2 | 0:2d05e443a44d | 248 | ciclo=0; |
PROCESADORES_2017_2 | 0:2d05e443a44d | 249 | } |
PROCESADORES_2017_2 | 0:2d05e443a44d | 250 | else |
PROCESADORES_2017_2 | 0:2d05e443a44d | 251 | ciclo++; |
PROCESADORES_2017_2 | 0:2d05e443a44d | 252 | err_v = err; //guarda el error |
PROCESADORES_2017_2 | 0:2d05e443a44d | 253 | } // Envía parámetro pid al puerto analogico de salida (D/A) y se repite el cicl |
PROCESADORES_2017_2 | 0:2d05e443a44d | 254 | } |
PROCESADORES_2017_2 | 0:2d05e443a44d | 255 | } |
PROCESADORES_2017_2 | 0:2d05e443a44d | 256 | |
PROCESADORES_2017_2 | 0:2d05e443a44d | 257 | |
PROCESADORES_2017_2 | 0:2d05e443a44d | 258 | |
PROCESADORES_2017_2 | 0:2d05e443a44d | 259 | else{ |
PROCESADORES_2017_2 | 0:2d05e443a44d | 260 | if (AT==0){ |
PROCESADORES_2017_2 | 0:2d05e443a44d | 261 | |
PROCESADORES_2017_2 | 0:2d05e443a44d | 262 | if (P2.rising()) { //Cambia la posición del cursor |
PROCESADORES_2017_2 | 0:2d05e443a44d | 263 | |
PROCESADORES_2017_2 | 0:2d05e443a44d | 264 | a++; |
PROCESADORES_2017_2 | 0:2d05e443a44d | 265 | led1=0; |
PROCESADORES_2017_2 | 0:2d05e443a44d | 266 | wait(0.15); //enciende el led rojo cada vez que se oprime este botón |
PROCESADORES_2017_2 | 0:2d05e443a44d | 267 | led1=1; |
PROCESADORES_2017_2 | 0:2d05e443a44d | 268 | if (a>3){ |
PROCESADORES_2017_2 | 0:2d05e443a44d | 269 | a = 0; |
PROCESADORES_2017_2 | 0:2d05e443a44d | 270 | } |
PROCESADORES_2017_2 | 0:2d05e443a44d | 271 | switch (a){ |
PROCESADORES_2017_2 | 0:2d05e443a44d | 272 | case 0: |
PROCESADORES_2017_2 | 0:2d05e443a44d | 273 | |
PROCESADORES_2017_2 | 0:2d05e443a44d | 274 | lcd.locate(2,0); |
PROCESADORES_2017_2 | 0:2d05e443a44d | 275 | lcd.printf("="); |
PROCESADORES_2017_2 | 0:2d05e443a44d | 276 | break; |
PROCESADORES_2017_2 | 0:2d05e443a44d | 277 | |
PROCESADORES_2017_2 | 0:2d05e443a44d | 278 | case 1: |
PROCESADORES_2017_2 | 0:2d05e443a44d | 279 | lcd.locate(10,0); |
PROCESADORES_2017_2 | 0:2d05e443a44d | 280 | lcd.printf("="); |
PROCESADORES_2017_2 | 0:2d05e443a44d | 281 | break; |
PROCESADORES_2017_2 | 0:2d05e443a44d | 282 | |
PROCESADORES_2017_2 | 0:2d05e443a44d | 283 | case 2: |
PROCESADORES_2017_2 | 0:2d05e443a44d | 284 | lcd.locate(2,1); |
PROCESADORES_2017_2 | 0:2d05e443a44d | 285 | lcd.printf("="); |
PROCESADORES_2017_2 | 0:2d05e443a44d | 286 | break; |
PROCESADORES_2017_2 | 0:2d05e443a44d | 287 | |
PROCESADORES_2017_2 | 0:2d05e443a44d | 288 | case 3: |
PROCESADORES_2017_2 | 0:2d05e443a44d | 289 | lcd.locate(10,1); |
PROCESADORES_2017_2 | 0:2d05e443a44d | 290 | lcd.printf("="); |
PROCESADORES_2017_2 | 0:2d05e443a44d | 291 | break; |
PROCESADORES_2017_2 | 0:2d05e443a44d | 292 | } |
PROCESADORES_2017_2 | 0:2d05e443a44d | 293 | } |
PROCESADORES_2017_2 | 0:2d05e443a44d | 294 | |
PROCESADORES_2017_2 | 0:2d05e443a44d | 295 | if (P1.rising()) { //Incrementa la variable |
PROCESADORES_2017_2 | 0:2d05e443a44d | 296 | led2=0; |
PROCESADORES_2017_2 | 0:2d05e443a44d | 297 | wait(0.15); //enciende el led verde cada vez que se oprime el botón de incremento |
PROCESADORES_2017_2 | 0:2d05e443a44d | 298 | led2=1; |
PROCESADORES_2017_2 | 0:2d05e443a44d | 299 | |
PROCESADORES_2017_2 | 0:2d05e443a44d | 300 | switch (a) { |
PROCESADORES_2017_2 | 0:2d05e443a44d | 301 | case 0: |
PROCESADORES_2017_2 | 0:2d05e443a44d | 302 | if (sp>3){ |
PROCESADORES_2017_2 | 0:2d05e443a44d | 303 | sp=3;} |
PROCESADORES_2017_2 | 0:2d05e443a44d | 304 | lcd.locate(2,0); //Ubica el parámetro Set-point |
PROCESADORES_2017_2 | 0:2d05e443a44d | 305 | lcd.printf("= "); |
PROCESADORES_2017_2 | 0:2d05e443a44d | 306 | lcd.locate(3,0); |
PROCESADORES_2017_2 | 0:2d05e443a44d | 307 | lcd.printf("%d", ++sp); |
PROCESADORES_2017_2 | 0:2d05e443a44d | 308 | |
PROCESADORES_2017_2 | 0:2d05e443a44d | 309 | break; |
PROCESADORES_2017_2 | 0:2d05e443a44d | 310 | |
PROCESADORES_2017_2 | 0:2d05e443a44d | 311 | case 1: |
PROCESADORES_2017_2 | 0:2d05e443a44d | 312 | if (kp>999){ |
PROCESADORES_2017_2 | 0:2d05e443a44d | 313 | kp=999;} |
PROCESADORES_2017_2 | 0:2d05e443a44d | 314 | lcd.locate(10,0); //Ubica el parámetro kp |
PROCESADORES_2017_2 | 0:2d05e443a44d | 315 | lcd.printf("= "); |
PROCESADORES_2017_2 | 0:2d05e443a44d | 316 | lcd.locate(11,0); |
PROCESADORES_2017_2 | 0:2d05e443a44d | 317 | lcd.printf("%d", ++kp); |
PROCESADORES_2017_2 | 0:2d05e443a44d | 318 | |
PROCESADORES_2017_2 | 0:2d05e443a44d | 319 | break; |
PROCESADORES_2017_2 | 0:2d05e443a44d | 320 | |
PROCESADORES_2017_2 | 0:2d05e443a44d | 321 | case 2: |
PROCESADORES_2017_2 | 0:2d05e443a44d | 322 | if (ki>999){ ki=999;} |
PROCESADORES_2017_2 | 0:2d05e443a44d | 323 | lcd.locate(2,1); //Ubica el parámetro ki |
PROCESADORES_2017_2 | 0:2d05e443a44d | 324 | lcd.printf("= "); |
PROCESADORES_2017_2 | 0:2d05e443a44d | 325 | lcd.locate(3,1); |
PROCESADORES_2017_2 | 0:2d05e443a44d | 326 | lcd.printf("%d", ++ki); |
PROCESADORES_2017_2 | 0:2d05e443a44d | 327 | |
PROCESADORES_2017_2 | 0:2d05e443a44d | 328 | break; |
PROCESADORES_2017_2 | 0:2d05e443a44d | 329 | |
PROCESADORES_2017_2 | 0:2d05e443a44d | 330 | case 3: |
PROCESADORES_2017_2 | 0:2d05e443a44d | 331 | if (kd>999){ kd=999;} |
PROCESADORES_2017_2 | 0:2d05e443a44d | 332 | lcd.locate(10,1); //Ubica el parámetro kd |
PROCESADORES_2017_2 | 0:2d05e443a44d | 333 | lcd.printf("= "); |
PROCESADORES_2017_2 | 0:2d05e443a44d | 334 | lcd.locate(11,1); |
PROCESADORES_2017_2 | 0:2d05e443a44d | 335 | lcd.printf("%d", ++kd); |
PROCESADORES_2017_2 | 0:2d05e443a44d | 336 | |
PROCESADORES_2017_2 | 0:2d05e443a44d | 337 | break; |
PROCESADORES_2017_2 | 0:2d05e443a44d | 338 | } |
PROCESADORES_2017_2 | 0:2d05e443a44d | 339 | } |
PROCESADORES_2017_2 | 0:2d05e443a44d | 340 | if (P3.rising()) { //Decrementa la variable |
PROCESADORES_2017_2 | 0:2d05e443a44d | 341 | wait(0.1); |
PROCESADORES_2017_2 | 0:2d05e443a44d | 342 | led3=0; |
PROCESADORES_2017_2 | 0:2d05e443a44d | 343 | wait(0.15); //enciende el led azul cada vez que se oprime este botón |
PROCESADORES_2017_2 | 0:2d05e443a44d | 344 | led3=1; |
PROCESADORES_2017_2 | 0:2d05e443a44d | 345 | |
PROCESADORES_2017_2 | 0:2d05e443a44d | 346 | switch (a) { |
PROCESADORES_2017_2 | 0:2d05e443a44d | 347 | case 0: |
PROCESADORES_2017_2 | 0:2d05e443a44d | 348 | if (sp<0) { //No se admite valores negativos |
PROCESADORES_2017_2 | 0:2d05e443a44d | 349 | sp=0; |
PROCESADORES_2017_2 | 0:2d05e443a44d | 350 | } |
PROCESADORES_2017_2 | 0:2d05e443a44d | 351 | lcd.locate(2,0); //Ubica el parámetro Set-point |
PROCESADORES_2017_2 | 0:2d05e443a44d | 352 | lcd.printf("= "); |
PROCESADORES_2017_2 | 0:2d05e443a44d | 353 | lcd.locate(3,0); |
PROCESADORES_2017_2 | 0:2d05e443a44d | 354 | lcd.printf("%d",sp--); |
PROCESADORES_2017_2 | 0:2d05e443a44d | 355 | break; |
PROCESADORES_2017_2 | 0:2d05e443a44d | 356 | |
PROCESADORES_2017_2 | 0:2d05e443a44d | 357 | case 1: |
PROCESADORES_2017_2 | 0:2d05e443a44d | 358 | if (kp<0) { //No se admite valores negativos |
PROCESADORES_2017_2 | 0:2d05e443a44d | 359 | kp=0; |
PROCESADORES_2017_2 | 0:2d05e443a44d | 360 | } |
PROCESADORES_2017_2 | 0:2d05e443a44d | 361 | lcd.locate(10,0); //Ubica el parámetro kp |
PROCESADORES_2017_2 | 0:2d05e443a44d | 362 | lcd.printf("= "); |
PROCESADORES_2017_2 | 0:2d05e443a44d | 363 | lcd.locate(11,0); |
PROCESADORES_2017_2 | 0:2d05e443a44d | 364 | lcd.printf("%d",kp--); |
PROCESADORES_2017_2 | 0:2d05e443a44d | 365 | break; |
PROCESADORES_2017_2 | 0:2d05e443a44d | 366 | |
PROCESADORES_2017_2 | 0:2d05e443a44d | 367 | case 2: |
PROCESADORES_2017_2 | 0:2d05e443a44d | 368 | if (ki<0) { //No se admite valores negativos |
PROCESADORES_2017_2 | 0:2d05e443a44d | 369 | ki=0; |
PROCESADORES_2017_2 | 0:2d05e443a44d | 370 | } |
PROCESADORES_2017_2 | 0:2d05e443a44d | 371 | lcd.locate(2,1); //Ubica el parámetro ki |
PROCESADORES_2017_2 | 0:2d05e443a44d | 372 | lcd.printf("= "); |
PROCESADORES_2017_2 | 0:2d05e443a44d | 373 | lcd.locate(3,1); |
PROCESADORES_2017_2 | 0:2d05e443a44d | 374 | lcd.printf("%d",ki--); |
PROCESADORES_2017_2 | 0:2d05e443a44d | 375 | break; |
PROCESADORES_2017_2 | 0:2d05e443a44d | 376 | |
PROCESADORES_2017_2 | 0:2d05e443a44d | 377 | case 3: |
PROCESADORES_2017_2 | 0:2d05e443a44d | 378 | if (kd<0) { //No se admite valores negativos |
PROCESADORES_2017_2 | 0:2d05e443a44d | 379 | kd=0; |
PROCESADORES_2017_2 | 0:2d05e443a44d | 380 | } |
PROCESADORES_2017_2 | 0:2d05e443a44d | 381 | lcd.locate(10,1); //Ubica el parámetro kd |
PROCESADORES_2017_2 | 0:2d05e443a44d | 382 | lcd.printf("= "); |
PROCESADORES_2017_2 | 0:2d05e443a44d | 383 | lcd.locate(11,1); |
PROCESADORES_2017_2 | 0:2d05e443a44d | 384 | lcd.printf("%d",kd--); |
PROCESADORES_2017_2 | 0:2d05e443a44d | 385 | break; |
PROCESADORES_2017_2 | 0:2d05e443a44d | 386 | } |
PROCESADORES_2017_2 | 0:2d05e443a44d | 387 | } |
PROCESADORES_2017_2 | 0:2d05e443a44d | 388 | //PARA GUARDAR LOS PARAMETROS SE PRESIONA EL PULSADOR SET DURANTE 5 SEGUNDOS |
PROCESADORES_2017_2 | 0:2d05e443a44d | 389 | if (P2==1 && kp>0 && ki>0 && kd>0 ) |
PROCESADORES_2017_2 | 0:2d05e443a44d | 390 | {B=1; |
PROCESADORES_2017_2 | 0:2d05e443a44d | 391 | t.start(); //empieza timer |
PROCESADORES_2017_2 | 0:2d05e443a44d | 392 | t2=t.read(); //lee el timer y lo almacena en una variable entera t2 |
PROCESADORES_2017_2 | 0:2d05e443a44d | 393 | |
PROCESADORES_2017_2 | 0:2d05e443a44d | 394 | } |
PROCESADORES_2017_2 | 0:2d05e443a44d | 395 | if(t2==5 && P2==1){ |
PROCESADORES_2017_2 | 0:2d05e443a44d | 396 | t.stop(); //para el timer |
PROCESADORES_2017_2 | 0:2d05e443a44d | 397 | B=0; |
PROCESADORES_2017_2 | 0:2d05e443a44d | 398 | lcd.cls(); |
PROCESADORES_2017_2 | 0:2d05e443a44d | 399 | } |
PROCESADORES_2017_2 | 0:2d05e443a44d | 400 | |
PROCESADORES_2017_2 | 0:2d05e443a44d | 401 | if (t2==5 && B==0) |
PROCESADORES_2017_2 | 0:2d05e443a44d | 402 | { |
PROCESADORES_2017_2 | 0:2d05e443a44d | 403 | t2=0; |
PROCESADORES_2017_2 | 0:2d05e443a44d | 404 | lcd.printf("GUARDADOS!"); |
PROCESADORES_2017_2 | 0:2d05e443a44d | 405 | wait(2); |
PROCESADORES_2017_2 | 0:2d05e443a44d | 406 | lcd.cls(); |
PROCESADORES_2017_2 | 0:2d05e443a44d | 407 | lcd.printf("INICIA EL PID"); |
PROCESADORES_2017_2 | 0:2d05e443a44d | 408 | wait(2); |
PROCESADORES_2017_2 | 0:2d05e443a44d | 409 | // se imprimen los parches del control |
PROCESADORES_2017_2 | 0:2d05e443a44d | 410 | lcd.cls(); |
PROCESADORES_2017_2 | 0:2d05e443a44d | 411 | |
PROCESADORES_2017_2 | 0:2d05e443a44d | 412 | lcd.printf("E:%d"); |
PROCESADORES_2017_2 | 0:2d05e443a44d | 413 | lcd.locate(8,0); |
PROCESADORES_2017_2 | 0:2d05e443a44d | 414 | lcd.printf("Y:%d"); |
PROCESADORES_2017_2 | 0:2d05e443a44d | 415 | lcd.locate(0,1); |
PROCESADORES_2017_2 | 0:2d05e443a44d | 416 | lcd.printf("Sp:%d"); |
PROCESADORES_2017_2 | 0:2d05e443a44d | 417 | lcd.locate(8,1); |
PROCESADORES_2017_2 | 0:2d05e443a44d | 418 | lcd.printf("Co:%0.1f"); |
PROCESADORES_2017_2 | 0:2d05e443a44d | 419 | wait(1); |
PROCESADORES_2017_2 | 0:2d05e443a44d | 420 | |
PROCESADORES_2017_2 | 0:2d05e443a44d | 421 | // CICLO PRINCIPAL CONTROLADOR PID |
PROCESADORES_2017_2 | 0:2d05e443a44d | 422 | |
PROCESADORES_2017_2 | 0:2d05e443a44d | 423 | while(1) { |
PROCESADORES_2017_2 | 0:2d05e443a44d | 424 | |
PROCESADORES_2017_2 | 0:2d05e443a44d | 425 | wait(0.001); |
PROCESADORES_2017_2 | 0:2d05e443a44d | 426 | med = (Vin.read()*3300);//la medicion es convertida a un valor en mV,siendo el máximo valor esperado 3300mV (3.3V) |
PROCESADORES_2017_2 | 0:2d05e443a44d | 427 | sp0 = sp; |
PROCESADORES_2017_2 | 0:2d05e443a44d | 428 | err = (sp0*1000-med); //Estoy ingresando un valor en V, pero opero internamente en mV |
PROCESADORES_2017_2 | 0:2d05e443a44d | 429 | float kp0; |
PROCESADORES_2017_2 | 0:2d05e443a44d | 430 | kp0 = kp; |
PROCESADORES_2017_2 | 0:2d05e443a44d | 431 | ap = kp0*err; |
PROCESADORES_2017_2 | 0:2d05e443a44d | 432 | float ki0; |
PROCESADORES_2017_2 | 0:2d05e443a44d | 433 | ki0 = ki; |
PROCESADORES_2017_2 | 0:2d05e443a44d | 434 | ai = (ki0*err)+ai; //calculo de la integral del error |
PROCESADORES_2017_2 | 0:2d05e443a44d | 435 | float kd0; |
PROCESADORES_2017_2 | 0:2d05e443a44d | 436 | kd0 = kd; |
PROCESADORES_2017_2 | 0:2d05e443a44d | 437 | ad = kd0*(err-err_v); //calculo de la accion derivativa |
PROCESADORES_2017_2 | 0:2d05e443a44d | 438 | pid = ap+ai+ad; |
PROCESADORES_2017_2 | 0:2d05e443a44d | 439 | |
PROCESADORES_2017_2 | 0:2d05e443a44d | 440 | |
PROCESADORES_2017_2 | 0:2d05e443a44d | 441 | // se verifica que pid sea menor o igual la valor maximo ***************** |
PROCESADORES_2017_2 | 0:2d05e443a44d | 442 | if (pid > 999){ |
PROCESADORES_2017_2 | 0:2d05e443a44d | 443 | pid=999; |
PROCESADORES_2017_2 | 0:2d05e443a44d | 444 | } |
PROCESADORES_2017_2 | 0:2d05e443a44d | 445 | |
PROCESADORES_2017_2 | 0:2d05e443a44d | 446 | // se verifica que pid sea positivo ************************************** |
PROCESADORES_2017_2 | 0:2d05e443a44d | 447 | if (pid < 0){ |
PROCESADORES_2017_2 | 0:2d05e443a44d | 448 | pid=0; |
PROCESADORES_2017_2 | 0:2d05e443a44d | 449 | } |
PROCESADORES_2017_2 | 0:2d05e443a44d | 450 | |
PROCESADORES_2017_2 | 0:2d05e443a44d | 451 | // se verifica que la accion integral no sea muy grande |
PROCESADORES_2017_2 | 0:2d05e443a44d | 452 | if (ai > 999){ |
PROCESADORES_2017_2 | 0:2d05e443a44d | 453 | ai=1000; |
PROCESADORES_2017_2 | 0:2d05e443a44d | 454 | } |
PROCESADORES_2017_2 | 0:2d05e443a44d | 455 | |
PROCESADORES_2017_2 | 0:2d05e443a44d | 456 | Vout=(pid/999); |
PROCESADORES_2017_2 | 0:2d05e443a44d | 457 | //Mostrar resultados PID |
PROCESADORES_2017_2 | 0:2d05e443a44d | 458 | if(ciclo>700) { |
PROCESADORES_2017_2 | 0:2d05e443a44d | 459 | lcd.locate(2,0); |
PROCESADORES_2017_2 | 0:2d05e443a44d | 460 | lcd.printf(" "); |
PROCESADORES_2017_2 | 0:2d05e443a44d | 461 | lcd.locate(0,0); |
PROCESADORES_2017_2 | 0:2d05e443a44d | 462 | lcd.printf("E=%2.1f",err/1000); //error |
PROCESADORES_2017_2 | 0:2d05e443a44d | 463 | lcd.locate(10,0); |
PROCESADORES_2017_2 | 0:2d05e443a44d | 464 | lcd.printf(" "); |
PROCESADORES_2017_2 | 0:2d05e443a44d | 465 | lcd.locate(8,0); |
PROCESADORES_2017_2 | 0:2d05e443a44d | 466 | lcd.printf("Y=%4.2f",med/1000); //Salida del circuito |
PROCESADORES_2017_2 | 0:2d05e443a44d | 467 | lcd.locate(2,1); |
PROCESADORES_2017_2 | 0:2d05e443a44d | 468 | lcd.printf(" "); |
PROCESADORES_2017_2 | 0:2d05e443a44d | 469 | lcd.locate(0,1); |
PROCESADORES_2017_2 | 0:2d05e443a44d | 470 | lcd.printf("Sp=%4.2f",sp0); //Set point |
PROCESADORES_2017_2 | 0:2d05e443a44d | 471 | lcd.locate(10,1); |
PROCESADORES_2017_2 | 0:2d05e443a44d | 472 | lcd.printf(" "); |
PROCESADORES_2017_2 | 0:2d05e443a44d | 473 | lcd.locate(10,1); |
PROCESADORES_2017_2 | 0:2d05e443a44d | 474 | lcd.printf("=%0.1f",pid*3.3/999); //Esfuerzo de control |
PROCESADORES_2017_2 | 0:2d05e443a44d | 475 | |
PROCESADORES_2017_2 | 0:2d05e443a44d | 476 | ciclo=0; |
PROCESADORES_2017_2 | 0:2d05e443a44d | 477 | } |
PROCESADORES_2017_2 | 0:2d05e443a44d | 478 | else |
PROCESADORES_2017_2 | 0:2d05e443a44d | 479 | ciclo++; |
PROCESADORES_2017_2 | 0:2d05e443a44d | 480 | err_v = err; //guarda el error |
PROCESADORES_2017_2 | 0:2d05e443a44d | 481 | } // Envía parámetro pid al puerto analogico de salida (D/A) y se repite el ciclo |
PROCESADORES_2017_2 | 0:2d05e443a44d | 482 | } |
PROCESADORES_2017_2 | 0:2d05e443a44d | 483 | |
PROCESADORES_2017_2 | 0:2d05e443a44d | 484 | } |
PROCESADORES_2017_2 | 0:2d05e443a44d | 485 | } |
PROCESADORES_2017_2 | 0:2d05e443a44d | 486 | } |
PROCESADORES_2017_2 | 0:2d05e443a44d | 487 | } |