Alexander Vallejo / Mbed 2 deprecated Tarea3

Control PID, Planta primer orden

Dependencies:   Debounced TextLCD mbed

main.cpp@0:e95309256f50, 2013-11-15 (annotated)

Committer:
avallejopo
Date:
Fri Nov 15 17:10:00 2013 +0000
Revision:
0:e95309256f50
Control PID, planta de primer orden

Who changed what in which revision?

UserRevisionLine numberNew contents of line
avallejopo 0:e95309256f50 1 #include "mbed.h"
avallejopo 0:e95309256f50 2 #include "DebouncedIn.h"
avallejopo 0:e95309256f50 3 #include "TextLCD.h"
avallejopo 0:e95309256f50 4
avallejopo 0:e95309256f50 5 AnalogIn Vin(PTC2);
avallejopo 0:e95309256f50 6 TextLCD lcd(PTB10, PTB11, PTE2, PTE3, PTE4, PTE5); // rs, e, d4-d7
avallejopo 0:e95309256f50 7
avallejopo 0:e95309256f50 8 AnalogIn y(PTB0);
avallejopo 0:e95309256f50 9 AnalogOut u(PTE30);
avallejopo 0:e95309256f50 10
avallejopo 0:e95309256f50 11 DigitalOut led1(LED1);
avallejopo 0:e95309256f50 12 DigitalOut led2(LED2);
avallejopo 0:e95309256f50 13 DigitalOut led3(LED3);
avallejopo 0:e95309256f50 14 DigitalIn button1(PTC12);
avallejopo 0:e95309256f50 15 DigitalIn button2(PTC13);
avallejopo 0:e95309256f50 16 DigitalIn button3(PTC16);
avallejopo 0:e95309256f50 17 DigitalIn button4(PTC17);
avallejopo 0:e95309256f50 18
avallejopo 0:e95309256f50 19
avallejopo 0:e95309256f50 20 //codigos movimiento del curzor
avallejopo 0:e95309256f50 21
avallejopo 0:e95309256f50 22 //int C1=0x0E; // solo muestra el curzor
avallejopo 0:e95309256f50 23 int C2=0x18; // desplaza izquierda
avallejopo 0:e95309256f50 24 int C3=0x1A; // desplaza derecha
avallejopo 0:e95309256f50 25 int C4=0x0C; // quito cursor bajo
avallejopo 0:e95309256f50 26
avallejopo 0:e95309256f50 27 int C1=0x0F;
avallejopo 0:e95309256f50 28 int err, med, yr, pid, ap, ai, ad, err_v, cycle;
avallejopo 0:e95309256f50 29 float pidn;
avallejopo 0:e95309256f50 30 int spnum=0,kinum=0,kpnum=0,kdnum=0,pos=1;
avallejopo 0:e95309256f50 31 int incremento=1,flagt=0;
avallejopo 0:e95309256f50 32 Timer t;
avallejopo 0:e95309256f50 33
avallejopo 0:e95309256f50 34 int main()
avallejopo 0:e95309256f50 35 {
avallejopo 0:e95309256f50 36 lcd.cls(); // Borrar Pantalla
avallejopo 0:e95309256f50 37 lcd.writeCommand(C1);//escribimos un comando segun el manual del modulo LCD
avallejopo 0:e95309256f50 38
avallejopo 0:e95309256f50 39 lcd.locate(8,0);
avallejopo 0:e95309256f50 40 lcd.printf("Kp=%d",kpnum);
avallejopo 0:e95309256f50 41 lcd.locate(0,1);
avallejopo 0:e95309256f50 42 lcd.printf("Ki=%d",kinum);
avallejopo 0:e95309256f50 43 lcd.locate(8,1);
avallejopo 0:e95309256f50 44 lcd.printf("Kd=%d",kdnum);
avallejopo 0:e95309256f50 45 lcd.locate(0,0);
avallejopo 0:e95309256f50 46 lcd.printf("Sp=%d",spnum);
avallejopo 0:e95309256f50 47
avallejopo 0:e95309256f50 48 while(1)
avallejopo 0:e95309256f50 49 {
avallejopo 0:e95309256f50 50 if (!button1)
avallejopo 0:e95309256f50 51 {
avallejopo 0:e95309256f50 52 if(flagt==0)
avallejopo 0:e95309256f50 53 {
avallejopo 0:e95309256f50 54 t.start();
avallejopo 0:e95309256f50 55 flagt=1;
avallejopo 0:e95309256f50 56 }
avallejopo 0:e95309256f50 57
avallejopo 0:e95309256f50 58 led1 =!led1;
avallejopo 0:e95309256f50 59 wait(0.25);
avallejopo 0:e95309256f50 60 if(!button1)
avallejopo 0:e95309256f50 61 {
avallejopo 0:e95309256f50 62 if(t.read()>5)
avallejopo 0:e95309256f50 63 {
avallejopo 0:e95309256f50 64 incremento=100;
avallejopo 0:e95309256f50 65 }
avallejopo 0:e95309256f50 66 else if(t.read()>2.75)
avallejopo 0:e95309256f50 67 {
avallejopo 0:e95309256f50 68 incremento=10;
avallejopo 0:e95309256f50 69 }
avallejopo 0:e95309256f50 70
avallejopo 0:e95309256f50 71 if(pos==1)
avallejopo 0:e95309256f50 72 {
avallejopo 0:e95309256f50 73 if(spnum+incremento>=999)
avallejopo 0:e95309256f50 74 {
avallejopo 0:e95309256f50 75 spnum=999;
avallejopo 0:e95309256f50 76 lcd.locate(3,0);
avallejopo 0:e95309256f50 77 lcd.printf(" ");
avallejopo 0:e95309256f50 78 lcd.locate(3,0);
avallejopo 0:e95309256f50 79 lcd.printf("%d", spnum);
avallejopo 0:e95309256f50 80 }
avallejopo 0:e95309256f50 81 else
avallejopo 0:e95309256f50 82 {
avallejopo 0:e95309256f50 83 spnum+=incremento;
avallejopo 0:e95309256f50 84 lcd.locate(3,0);
avallejopo 0:e95309256f50 85 lcd.printf("%d", spnum);
avallejopo 0:e95309256f50 86 }
avallejopo 0:e95309256f50 87 }
avallejopo 0:e95309256f50 88 else if(pos==2)
avallejopo 0:e95309256f50 89 {
avallejopo 0:e95309256f50 90 if(kpnum+incremento>=999)
avallejopo 0:e95309256f50 91 {
avallejopo 0:e95309256f50 92 kpnum=999;
avallejopo 0:e95309256f50 93 lcd.locate(11,0);
avallejopo 0:e95309256f50 94 lcd.printf(" ");
avallejopo 0:e95309256f50 95 lcd.locate(11,0);
avallejopo 0:e95309256f50 96 lcd.printf("%d", kpnum);
avallejopo 0:e95309256f50 97 }
avallejopo 0:e95309256f50 98 else
avallejopo 0:e95309256f50 99 {
avallejopo 0:e95309256f50 100 kpnum+=incremento;
avallejopo 0:e95309256f50 101 lcd.locate(11,0);
avallejopo 0:e95309256f50 102 lcd.printf("%d", kpnum);
avallejopo 0:e95309256f50 103 }
avallejopo 0:e95309256f50 104 }
avallejopo 0:e95309256f50 105 else if(pos==3)
avallejopo 0:e95309256f50 106 {
avallejopo 0:e95309256f50 107 if(kinum+incremento>=999)
avallejopo 0:e95309256f50 108 {
avallejopo 0:e95309256f50 109 kinum=999;
avallejopo 0:e95309256f50 110 lcd.locate(3,1);
avallejopo 0:e95309256f50 111 lcd.printf(" ");
avallejopo 0:e95309256f50 112 lcd.locate(3,1);
avallejopo 0:e95309256f50 113 lcd.printf("%d", kinum);
avallejopo 0:e95309256f50 114 }
avallejopo 0:e95309256f50 115 else
avallejopo 0:e95309256f50 116 {
avallejopo 0:e95309256f50 117 kinum+=incremento;
avallejopo 0:e95309256f50 118 lcd.locate(3,1);
avallejopo 0:e95309256f50 119 lcd.printf("%d", kinum);
avallejopo 0:e95309256f50 120 }
avallejopo 0:e95309256f50 121 }
avallejopo 0:e95309256f50 122 else if(pos==4)
avallejopo 0:e95309256f50 123 {
avallejopo 0:e95309256f50 124 if(kdnum+incremento>=999)
avallejopo 0:e95309256f50 125 {
avallejopo 0:e95309256f50 126 kdnum=999;
avallejopo 0:e95309256f50 127 lcd.locate(11,1);
avallejopo 0:e95309256f50 128 lcd.printf(" ");
avallejopo 0:e95309256f50 129 lcd.locate(11,1);
avallejopo 0:e95309256f50 130 lcd.printf("%d", kdnum);
avallejopo 0:e95309256f50 131 }
avallejopo 0:e95309256f50 132 else
avallejopo 0:e95309256f50 133 {
avallejopo 0:e95309256f50 134 kdnum+=incremento;
avallejopo 0:e95309256f50 135 lcd.locate(11,1);
avallejopo 0:e95309256f50 136 lcd.printf("%d", kdnum);
avallejopo 0:e95309256f50 137 }
avallejopo 0:e95309256f50 138 }
avallejopo 0:e95309256f50 139 }
avallejopo 0:e95309256f50 140 }
avallejopo 0:e95309256f50 141
avallejopo 0:e95309256f50 142 if (!button2)
avallejopo 0:e95309256f50 143 {
avallejopo 0:e95309256f50 144 if(flagt==0)
avallejopo 0:e95309256f50 145 {
avallejopo 0:e95309256f50 146 //t.reset();
avallejopo 0:e95309256f50 147 t.start();
avallejopo 0:e95309256f50 148 flagt=1;
avallejopo 0:e95309256f50 149 }
avallejopo 0:e95309256f50 150
avallejopo 0:e95309256f50 151 led1 =!led1;
avallejopo 0:e95309256f50 152 wait(0.25);
avallejopo 0:e95309256f50 153 if(!button2)
avallejopo 0:e95309256f50 154 {
avallejopo 0:e95309256f50 155 if(t.read()>5)
avallejopo 0:e95309256f50 156 {
avallejopo 0:e95309256f50 157 incremento=100;
avallejopo 0:e95309256f50 158 }
avallejopo 0:e95309256f50 159 else if(t.read()>2.75)
avallejopo 0:e95309256f50 160 {
avallejopo 0:e95309256f50 161 incremento=10;
avallejopo 0:e95309256f50 162 }
avallejopo 0:e95309256f50 163
avallejopo 0:e95309256f50 164 led2 =!led2;
avallejopo 0:e95309256f50 165 if(pos==1)
avallejopo 0:e95309256f50 166 {
avallejopo 0:e95309256f50 167 if(spnum-incremento<0)
avallejopo 0:e95309256f50 168 {
avallejopo 0:e95309256f50 169 //No ocurre nada
avallejopo 0:e95309256f50 170 }
avallejopo 0:e95309256f50 171 else
avallejopo 0:e95309256f50 172 {
avallejopo 0:e95309256f50 173 spnum-=incremento;
avallejopo 0:e95309256f50 174 lcd.locate(3,0);
avallejopo 0:e95309256f50 175 lcd.printf(" ");
avallejopo 0:e95309256f50 176 lcd.locate(3,0);
avallejopo 0:e95309256f50 177 lcd.printf("%d", spnum);
avallejopo 0:e95309256f50 178 }
avallejopo 0:e95309256f50 179 }
avallejopo 0:e95309256f50 180 else if(pos==2)
avallejopo 0:e95309256f50 181 {
avallejopo 0:e95309256f50 182 if(kpnum-incremento<0)
avallejopo 0:e95309256f50 183 {
avallejopo 0:e95309256f50 184 //No ocurre nada
avallejopo 0:e95309256f50 185 }
avallejopo 0:e95309256f50 186 else
avallejopo 0:e95309256f50 187 {
avallejopo 0:e95309256f50 188 kpnum-=incremento;
avallejopo 0:e95309256f50 189 lcd.locate(11,0);
avallejopo 0:e95309256f50 190 lcd.printf(" ");
avallejopo 0:e95309256f50 191 lcd.locate(11,0);
avallejopo 0:e95309256f50 192 lcd.printf("%d", kpnum);
avallejopo 0:e95309256f50 193 }
avallejopo 0:e95309256f50 194 }
avallejopo 0:e95309256f50 195 else if(pos==3)
avallejopo 0:e95309256f50 196 {
avallejopo 0:e95309256f50 197 if(kinum-incremento<0)
avallejopo 0:e95309256f50 198 {
avallejopo 0:e95309256f50 199 //No ocurre nada
avallejopo 0:e95309256f50 200 }
avallejopo 0:e95309256f50 201 else
avallejopo 0:e95309256f50 202 {
avallejopo 0:e95309256f50 203 kinum-=incremento;
avallejopo 0:e95309256f50 204 lcd.locate(3,1);
avallejopo 0:e95309256f50 205 lcd.printf(" ");
avallejopo 0:e95309256f50 206 lcd.locate(3,1);
avallejopo 0:e95309256f50 207 lcd.printf("%d", kinum);
avallejopo 0:e95309256f50 208 }
avallejopo 0:e95309256f50 209 }
avallejopo 0:e95309256f50 210 else if(pos==4)
avallejopo 0:e95309256f50 211 {
avallejopo 0:e95309256f50 212 if(kdnum-incremento<0)
avallejopo 0:e95309256f50 213 {
avallejopo 0:e95309256f50 214 //No ocurre nada
avallejopo 0:e95309256f50 215 }
avallejopo 0:e95309256f50 216 else
avallejopo 0:e95309256f50 217 {
avallejopo 0:e95309256f50 218 kdnum-=incremento;
avallejopo 0:e95309256f50 219 lcd.locate(11,1);
avallejopo 0:e95309256f50 220 lcd.printf(" ");
avallejopo 0:e95309256f50 221 lcd.locate(11,1);
avallejopo 0:e95309256f50 222 lcd.printf("%d", kdnum);
avallejopo 0:e95309256f50 223 }
avallejopo 0:e95309256f50 224 }
avallejopo 0:e95309256f50 225 }
avallejopo 0:e95309256f50 226 }
avallejopo 0:e95309256f50 227
avallejopo 0:e95309256f50 228 if (!button3)
avallejopo 0:e95309256f50 229 {
avallejopo 0:e95309256f50 230 led3 =!led3;
avallejopo 0:e95309256f50 231 if(pos==4)
avallejopo 0:e95309256f50 232 {
avallejopo 0:e95309256f50 233 pos=1;
avallejopo 0:e95309256f50 234 lcd.locate(3,0);
avallejopo 0:e95309256f50 235 lcd.printf("%d", spnum);
avallejopo 0:e95309256f50 236 }
avallejopo 0:e95309256f50 237 else if (pos==1)
avallejopo 0:e95309256f50 238 {
avallejopo 0:e95309256f50 239 pos++;
avallejopo 0:e95309256f50 240 lcd.locate(11,0);
avallejopo 0:e95309256f50 241 lcd.printf("%d", kpnum);
avallejopo 0:e95309256f50 242 }
avallejopo 0:e95309256f50 243 else if(pos==2)
avallejopo 0:e95309256f50 244 {
avallejopo 0:e95309256f50 245 pos++;
avallejopo 0:e95309256f50 246 lcd.locate(3,1);
avallejopo 0:e95309256f50 247 lcd.printf("%d", kinum);
avallejopo 0:e95309256f50 248 }
avallejopo 0:e95309256f50 249 else if(pos==3)
avallejopo 0:e95309256f50 250 {
avallejopo 0:e95309256f50 251 pos++;
avallejopo 0:e95309256f50 252 lcd.locate(11,1);
avallejopo 0:e95309256f50 253 lcd.printf("%d", kdnum);
avallejopo 0:e95309256f50 254 }
avallejopo 0:e95309256f50 255 wait(0.25);
avallejopo 0:e95309256f50 256
avallejopo 0:e95309256f50 257 }
avallejopo 0:e95309256f50 258
avallejopo 0:e95309256f50 259 if (button1 && button2)
avallejopo 0:e95309256f50 260 {
avallejopo 0:e95309256f50 261 flagt=0;
avallejopo 0:e95309256f50 262 t.reset();
avallejopo 0:e95309256f50 263 incremento=1;
avallejopo 0:e95309256f50 264 }
avallejopo 0:e95309256f50 265 if (!button4)
avallejopo 0:e95309256f50 266 {
avallejopo 0:e95309256f50 267 break; //sale del bucle si pisan suiche4
avallejopo 0:e95309256f50 268 }
avallejopo 0:e95309256f50 269 }
avallejopo 0:e95309256f50 270
avallejopo 0:e95309256f50 271
avallejopo 0:e95309256f50 272 //Transicion
avallejopo 0:e95309256f50 273 lcd.writeCommand(C4);//escribimos un comando segun el manual del modulo LCD para quitar cursor bajo
avallejopo 0:e95309256f50 274 lcd.cls(); //borra la pantalla
avallejopo 0:e95309256f50 275 lcd.printf(" GUARDADOS!");
avallejopo 0:e95309256f50 276 wait(1);
avallejopo 0:e95309256f50 277 lcd.cls();
avallejopo 0:e95309256f50 278 lcd.printf(" INICIA EL PID");
avallejopo 0:e95309256f50 279 wait(1);
avallejopo 0:e95309256f50 280 // se imprimen los parches del control *****************************************
avallejopo 0:e95309256f50 281 lcd.cls();
avallejopo 0:e95309256f50 282 lcd.printf("Er=%d",err);
avallejopo 0:e95309256f50 283 lcd.locate(8,0);
avallejopo 0:e95309256f50 284 lcd.printf("Me=%d",med);
avallejopo 0:e95309256f50 285 lcd.locate(0,1);
avallejopo 0:e95309256f50 286 lcd.printf("Sp=%d",spnum);
avallejopo 0:e95309256f50 287 lcd.locate(8,1);
avallejopo 0:e95309256f50 288 lcd.printf("Co=%d",pid);
avallejopo 0:e95309256f50 289 wait(2);
avallejopo 0:e95309256f50 290
avallejopo 0:e95309256f50 291 // CICLO PRINCIPAL CONTROLADOR PID
avallejopo 0:e95309256f50 292 flagt=0;
avallejopo 0:e95309256f50 293 while(1)
avallejopo 0:e95309256f50 294 {
avallejopo 0:e95309256f50 295 med=999*y.read(); //leer puerto analogo y asignar a med
avallejopo 0:e95309256f50 296 err = (spnum-med); //se calcula el error
avallejopo 0:e95309256f50 297
avallejopo 0:e95309256f50 298 ap = kpnum*err; //se calcula la accion proporcinal
avallejopo 0:e95309256f50 299
avallejopo 0:e95309256f50 300 // se verifica que la accion integral no sea muy grande
avallejopo 0:e95309256f50 301 if(ai<100)
avallejopo 0:e95309256f50 302 {
avallejopo 0:e95309256f50 303 ai =(kinum*err)+ai; //calculo de la integral del error
avallejopo 0:e95309256f50 304 }
avallejopo 0:e95309256f50 305
avallejopo 0:e95309256f50 306 ad = kdnum*(err-err_v); //calculo de la accion derivativa
avallejopo 0:e95309256f50 307
avallejopo 0:e95309256f50 308 pid = (ap+ai+ad);
avallejopo 0:e95309256f50 309
avallejopo 0:e95309256f50 310 // se verifica que pid sea positivo **************************************
avallejopo 0:e95309256f50 311 if(pid<=0)
avallejopo 0:e95309256f50 312 {
avallejopo 0:e95309256f50 313 pid=0;
avallejopo 0:e95309256f50 314 }
avallejopo 0:e95309256f50 315
avallejopo 0:e95309256f50 316 // se verifica que pid sea menor o igual la valor maximo *****************
avallejopo 0:e95309256f50 317 if (pid > 999)
avallejopo 0:e95309256f50 318 {
avallejopo 0:e95309256f50 319 pid=999;
avallejopo 0:e95309256f50 320 }
avallejopo 0:e95309256f50 321
avallejopo 0:e95309256f50 322 // se actualizan las variables *******************************************
avallejopo 0:e95309256f50 323 err_v = err;
avallejopo 0:e95309256f50 324
avallejopo 0:e95309256f50 325 //se muestran las variables******************************************
avallejopo 0:e95309256f50 326 if(flagt==0)
avallejopo 0:e95309256f50 327 {
avallejopo 0:e95309256f50 328 t.start();
avallejopo 0:e95309256f50 329 flagt=1;
avallejopo 0:e95309256f50 330 }
avallejopo 0:e95309256f50 331 if(t>=0.3)
avallejopo 0:e95309256f50 332 {
avallejopo 0:e95309256f50 333 lcd.locate(3,0);lcd.printf(" ");
avallejopo 0:e95309256f50 334 lcd.locate(3,0);lcd.printf("%d",err);
avallejopo 0:e95309256f50 335 lcd.locate(11,0);lcd.printf(" ");
avallejopo 0:e95309256f50 336 lcd.locate(11,0);lcd.printf("%d",med);
avallejopo 0:e95309256f50 337 lcd.locate(3,1);lcd.printf(" ");
avallejopo 0:e95309256f50 338 lcd.locate(3,1);lcd.printf("%d",spnum);
avallejopo 0:e95309256f50 339 lcd.locate(11,1);lcd.printf(" ");
avallejopo 0:e95309256f50 340 lcd.locate(11,1);lcd.printf("%d",pid);
avallejopo 0:e95309256f50 341 flagt=0;
avallejopo 0:e95309256f50 342 t.reset();
avallejopo 0:e95309256f50 343 }
avallejopo 0:e95309256f50 344
avallejopo 0:e95309256f50 345 //Normalizacion de la salida
avallejopo 0:e95309256f50 346 pidn=pid/999;
avallejopo 0:e95309256f50 347 // se envia el valor pid a puerto analogico de salida (D/A) **************
avallejopo 0:e95309256f50 348 u.write(pidn);
avallejopo 0:e95309256f50 349 // se repite el ciclo
avallejopo 0:e95309256f50 350 wait(0.005);
avallejopo 0:e95309256f50 351 }
avallejopo 0:e95309256f50 352
avallejopo 0:e95309256f50 353 }