Procesadores 2017 / Mbed 2 deprecated PID_ZN_ENCODER

PID que funciona con encoder y tiene autotunning.

Dependencies:   QEI TextLCD mbed

Fork of TAREA_4_PROCESADORES by john lopez

main.cpp@2:f854af43e0c2, 2017-11-28 (annotated)

Committer:
PROCESADORES_2017_2
Date:
Tue Nov 28 19:47:14 2017 +0000
Revision:
2:f854af43e0c2
Parent:
1:9ca362d07dd0 
PID  que funciona con encoder y tiene autotunning.

Who changed what in which revision?

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