Se realiza un control PID sobre el voltaje de un condensador en un filtro RC. Se transmiten las señales de salida y set-point por medio de Bluetooth para luego ser graficadas en una aplicación de Android.

Dependencies:   QEI TextLCD mbed

Fork of PID by Nicolás Villegas Echavarría

Committer:
tony63
Date:
Fri Apr 22 23:11:38 2016 +0000
Revision:
0:4e0dfcf0e7ce
Child:
1:058b8f5c135d
PID_Controller_Encoder

Who changed what in which revision?

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