Ximena Cifuentes / Mbed 2 deprecated TareaEncoder

Se utilizo un encoder para un PID

Dependencies:   QEI TextLCD mbed

main.cpp@0:9925883eec45, 2013-11-29 (annotated)

Committer:
xccifuentess
Date:
Fri Nov 29 16:14:49 2013 +0000
Revision:
0:9925883eec45
Utilizacion del Encoder para un PID

Who changed what in which revision?

UserRevisionLine numberNew contents of line
xccifuentess 0:9925883eec45 1 #include "mbed.h"
xccifuentess 0:9925883eec45 2 #include "TextLCD.h"
xccifuentess 0:9925883eec45 3 #include "QEI.h"
xccifuentess 0:9925883eec45 4
xccifuentess 0:9925883eec45 5
xccifuentess 0:9925883eec45 6 TextLCD lcd(PTB10, PTB11, PTE2, PTE3, PTE4, PTE5); // rs, e, d4-d7
xccifuentess 0:9925883eec45 7 QEI leftQei(PTA1, PTA2, NC, 624);
xccifuentess 0:9925883eec45 8
xccifuentess 0:9925883eec45 9 AnalogIn Vin(PTC2);
xccifuentess 0:9925883eec45 10
xccifuentess 0:9925883eec45 11 AnalogIn y(PTB0); //entrada análogica de la planta RC
xccifuentess 0:9925883eec45 12 AnalogOut u(PTE30);
xccifuentess 0:9925883eec45 13
xccifuentess 0:9925883eec45 14 DigitalOut l1(LED1);
xccifuentess 0:9925883eec45 15 DigitalOut l2(LED2);
xccifuentess 0:9925883eec45 16 DigitalOut l3(LED3);
xccifuentess 0:9925883eec45 17 DigitalIn b3(PTD4);
xccifuentess 0:9925883eec45 18 DigitalIn b4(PTC17);
xccifuentess 0:9925883eec45 19
xccifuentess 0:9925883eec45 20 int spnum=0,kpnum=0,kinum=0,kdnum=0,k=1;
xccifuentess 0:9925883eec45 21 int bandera;
xccifuentess 0:9925883eec45 22 Timer t;
xccifuentess 0:9925883eec45 23 float tiempo;
xccifuentess 0:9925883eec45 24
xccifuentess 0:9925883eec45 25 //codigos movimiento del curzor
xccifuentess 0:9925883eec45 26
xccifuentess 0:9925883eec45 27 //int C1=0x0E; // solo muestra el curzor
xccifuentess 0:9925883eec45 28 int C2=0x18; // desplaza izquierda
xccifuentess 0:9925883eec45 29 int C3=0x1A; // desplaza derecha
xccifuentess 0:9925883eec45 30 int C4=0x0C; // quito cursor bajo
xccifuentess 0:9925883eec45 31
xccifuentess 0:9925883eec45 32 int C1=0x0F;
xccifuentess 0:9925883eec45 33 int err, med, yr, pid, ap, ai, ad, err_v, cycle,i;
xccifuentess 0:9925883eec45 34 float pidn;
xccifuentess 0:9925883eec45 35 int pos=1;
xccifuentess 0:9925883eec45 36 //int spnum=500, kinum=2, kpnum=1, kdnum=0, pos=1;
xccifuentess 0:9925883eec45 37 //int incremento=1,flagt=0;
xccifuentess 0:9925883eec45 38
xccifuentess 0:9925883eec45 39 int main()
xccifuentess 0:9925883eec45 40 {
xccifuentess 0:9925883eec45 41 lcd.cls(); // Borrar Pantalla
xccifuentess 0:9925883eec45 42 lcd.writeCommand(C1);//escribimos un comando segun el manual del modulo LCD
xccifuentess 0:9925883eec45 43 //A continuación localizamos en el LCD los nombres de las variables del PID.
xccifuentess 0:9925883eec45 44 lcd.locate(8,0);
xccifuentess 0:9925883eec45 45 lcd.printf("Kp=%d",kpnum);
xccifuentess 0:9925883eec45 46 lcd.locate(0,1);
xccifuentess 0:9925883eec45 47 lcd.printf("Ki=%d",kinum);
xccifuentess 0:9925883eec45 48 lcd.locate(8,1);
xccifuentess 0:9925883eec45 49 lcd.printf("Kd=%d",kdnum);
xccifuentess 0:9925883eec45 50 lcd.locate(0,0);
xccifuentess 0:9925883eec45 51 lcd.printf("Sp=%d",spnum);
xccifuentess 0:9925883eec45 52
xccifuentess 0:9925883eec45 53 while(1)
xccifuentess 0:9925883eec45 54 {
xccifuentess 0:9925883eec45 55 while(k==1) // k será la variable que nos localice en que posición del LCD nos encontramos.
xccifuentess 0:9925883eec45 56 {
xccifuentess 0:9925883eec45 57 spnum=leftQei.getPulses(); // Comando que permite aumentar o disminuir el valor de "SP" por medio del encoder.
xccifuentess 0:9925883eec45 58 if (spnum>=0 & spnum<=999) //acotamos los valores de "SP" entre 0 y 999.
xccifuentess 0:9925883eec45 59 {
xccifuentess 0:9925883eec45 60 lcd.locate(3,0);
xccifuentess 0:9925883eec45 61 lcd.printf(" ");
xccifuentess 0:9925883eec45 62 lcd.locate(3,0);
xccifuentess 0:9925883eec45 63 lcd.printf("%i",spnum); //imprime un número para "SP"
xccifuentess 0:9925883eec45 64 wait(0.2);
xccifuentess 0:9925883eec45 65 }
xccifuentess 0:9925883eec45 66 if (!b3) //Al oprimir el pulsador del encoder, permita el cambio de posición desde "SP" a "KP"
xccifuentess 0:9925883eec45 67 {
xccifuentess 0:9925883eec45 68 k+=1;
xccifuentess 0:9925883eec45 69 wait(0.25);
xccifuentess 0:9925883eec45 70 lcd.locate(11,0);
xccifuentess 0:9925883eec45 71 lcd.printf("%d",kpnum);
xccifuentess 0:9925883eec45 72 }
xccifuentess 0:9925883eec45 73
xccifuentess 0:9925883eec45 74 }
xccifuentess 0:9925883eec45 75
xccifuentess 0:9925883eec45 76 while(k==2) // nos encontramos en la posición 2 (KP).
xccifuentess 0:9925883eec45 77 {
xccifuentess 0:9925883eec45 78 wait(0.2);
xccifuentess 0:9925883eec45 79 kpnum=leftQei.getPulses()-spnum;
xccifuentess 0:9925883eec45 80 if (kpnum>=0 & kpnum<=999)
xccifuentess 0:9925883eec45 81 {
xccifuentess 0:9925883eec45 82 lcd.locate(11,0);
xccifuentess 0:9925883eec45 83 lcd.printf(" ");
xccifuentess 0:9925883eec45 84 lcd.locate(11,0);
xccifuentess 0:9925883eec45 85 lcd.printf("%i",kpnum);
xccifuentess 0:9925883eec45 86 wait(0.2);
xccifuentess 0:9925883eec45 87 }
xccifuentess 0:9925883eec45 88
xccifuentess 0:9925883eec45 89 if (!b3)
xccifuentess 0:9925883eec45 90 {
xccifuentess 0:9925883eec45 91 kpnum=leftQei.getPulses();
xccifuentess 0:9925883eec45 92 k+=1;
xccifuentess 0:9925883eec45 93 wait(0.25);
xccifuentess 0:9925883eec45 94 lcd.locate(3,1);
xccifuentess 0:9925883eec45 95 lcd.printf("%d",kinum);
xccifuentess 0:9925883eec45 96 }
xccifuentess 0:9925883eec45 97 }
xccifuentess 0:9925883eec45 98
xccifuentess 0:9925883eec45 99 while(k==3) // nos encontramos en la posición 3 (KI).
xccifuentess 0:9925883eec45 100 {
xccifuentess 0:9925883eec45 101 wait(0.2);
xccifuentess 0:9925883eec45 102 kinum=leftQei.getPulses()-kpnum;
xccifuentess 0:9925883eec45 103 if (kinum>=0 & kinum<=999)
xccifuentess 0:9925883eec45 104 {
xccifuentess 0:9925883eec45 105 lcd.locate(3,1);
xccifuentess 0:9925883eec45 106 lcd.printf(" ");
xccifuentess 0:9925883eec45 107 lcd.locate(3,1);
xccifuentess 0:9925883eec45 108 lcd.printf("%i",kinum);
xccifuentess 0:9925883eec45 109 wait(0.2);
xccifuentess 0:9925883eec45 110 }
xccifuentess 0:9925883eec45 111 if (!b3)
xccifuentess 0:9925883eec45 112 {
xccifuentess 0:9925883eec45 113 kinum=leftQei.getPulses();
xccifuentess 0:9925883eec45 114 k+=1;
xccifuentess 0:9925883eec45 115 wait(0.25);
xccifuentess 0:9925883eec45 116 lcd.locate(11,1);
xccifuentess 0:9925883eec45 117 lcd.printf("%d",kdnum);
xccifuentess 0:9925883eec45 118 }
xccifuentess 0:9925883eec45 119 }
xccifuentess 0:9925883eec45 120
xccifuentess 0:9925883eec45 121 while(k==4)// nos encontramos en la posición 4 (KD).
xccifuentess 0:9925883eec45 122 {
xccifuentess 0:9925883eec45 123 kdnum=leftQei.getPulses()-kinum;
xccifuentess 0:9925883eec45 124 if (kdnum>=0 & kdnum<=999)
xccifuentess 0:9925883eec45 125 {
xccifuentess 0:9925883eec45 126 lcd.locate(11,1);
xccifuentess 0:9925883eec45 127 lcd.printf(" ");
xccifuentess 0:9925883eec45 128 lcd.locate(11,1);
xccifuentess 0:9925883eec45 129 lcd.printf("%i",kdnum);
xccifuentess 0:9925883eec45 130 wait(0.2);
xccifuentess 0:9925883eec45 131 }
xccifuentess 0:9925883eec45 132 if (!b3)
xccifuentess 0:9925883eec45 133 {
xccifuentess 0:9925883eec45 134 k=1;
xccifuentess 0:9925883eec45 135 wait(0.25);
xccifuentess 0:9925883eec45 136 lcd.locate(3,0);
xccifuentess 0:9925883eec45 137 lcd.printf("%d",spnum);
xccifuentess 0:9925883eec45 138 }
xccifuentess 0:9925883eec45 139
xccifuentess 0:9925883eec45 140 }
xccifuentess 0:9925883eec45 141
xccifuentess 0:9925883eec45 142
xccifuentess 0:9925883eec45 143 if (b4)
xccifuentess 0:9925883eec45 144 {
xccifuentess 0:9925883eec45 145 break; //sale del bucle si pisan suiche4
xccifuentess 0:9925883eec45 146 }
xccifuentess 0:9925883eec45 147 continue;
xccifuentess 0:9925883eec45 148 }
xccifuentess 0:9925883eec45 149
xccifuentess 0:9925883eec45 150
xccifuentess 0:9925883eec45 151 //Transicion
xccifuentess 0:9925883eec45 152
xccifuentess 0:9925883eec45 153 lcd.writeCommand(C4);//escribimos un comando segun el manual del modulo LCD para quitar cursor bajo
xccifuentess 0:9925883eec45 154 lcd.cls(); //borra la pantalla
xccifuentess 0:9925883eec45 155 lcd.printf("GUARDADOS!");
xccifuentess 0:9925883eec45 156 wait(1);
xccifuentess 0:9925883eec45 157 lcd.cls();
xccifuentess 0:9925883eec45 158 lcd.printf(" INICIA EL PID");
xccifuentess 0:9925883eec45 159 wait(1);
xccifuentess 0:9925883eec45 160 // se imprimen los parches del control *****************************************
xccifuentess 0:9925883eec45 161 lcd.cls();
xccifuentess 0:9925883eec45 162 lcd.printf("Er=%d",err);
xccifuentess 0:9925883eec45 163 lcd.locate(8,0);
xccifuentess 0:9925883eec45 164 lcd.printf("Me=%d",med);
xccifuentess 0:9925883eec45 165 lcd.locate(0,1);
xccifuentess 0:9925883eec45 166 lcd.printf("Sp=%d",spnum);
xccifuentess 0:9925883eec45 167 lcd.locate(8,1);
xccifuentess 0:9925883eec45 168 lcd.printf("Co=%d",pid);
xccifuentess 0:9925883eec45 169 wait(2);
xccifuentess 0:9925883eec45 170
xccifuentess 0:9925883eec45 171 // CICLO PRINCIPAL CONTROLADOR PID
xccifuentess 0:9925883eec45 172 bandera=0;
xccifuentess 0:9925883eec45 173 while(1)
xccifuentess 0:9925883eec45 174 {
xccifuentess 0:9925883eec45 175 med=999*y.read(); //leer puerto analogo y asignar a med
xccifuentess 0:9925883eec45 176 err = (spnum-med); //se calcula el error
xccifuentess 0:9925883eec45 177
xccifuentess 0:9925883eec45 178 ap = kpnum*err; //se calcula la accion proporcinal
xccifuentess 0:9925883eec45 179
xccifuentess 0:9925883eec45 180 // se verifica que la accion integral no sea muy grande
xccifuentess 0:9925883eec45 181 if(ai<100)
xccifuentess 0:9925883eec45 182 {
xccifuentess 0:9925883eec45 183 ai =(kinum*err)+ai; //calculo de la integral del error
xccifuentess 0:9925883eec45 184 }
xccifuentess 0:9925883eec45 185
xccifuentess 0:9925883eec45 186 ad = kdnum*(err-err_v); //calculo de la accion derivativa
xccifuentess 0:9925883eec45 187
xccifuentess 0:9925883eec45 188 pid = (ap+ai+ad);
xccifuentess 0:9925883eec45 189
xccifuentess 0:9925883eec45 190 // se verifica que pid sea positivo **************************************
xccifuentess 0:9925883eec45 191 if(pid<=0)
xccifuentess 0:9925883eec45 192 {
xccifuentess 0:9925883eec45 193 pid=0;
xccifuentess 0:9925883eec45 194 }
xccifuentess 0:9925883eec45 195
xccifuentess 0:9925883eec45 196 // se verifica que pid sea menor o igual la valor maximo *****************
xccifuentess 0:9925883eec45 197 if (pid > 999)
xccifuentess 0:9925883eec45 198 {
xccifuentess 0:9925883eec45 199 pid=999;
xccifuentess 0:9925883eec45 200 }
xccifuentess 0:9925883eec45 201
xccifuentess 0:9925883eec45 202 // se actualizan las variables *******************************************
xccifuentess 0:9925883eec45 203 err_v = err;
xccifuentess 0:9925883eec45 204
xccifuentess 0:9925883eec45 205 //se muestran las variables******************************************
xccifuentess 0:9925883eec45 206 if(bandera==0)
xccifuentess 0:9925883eec45 207 {
xccifuentess 0:9925883eec45 208 t.start();
xccifuentess 0:9925883eec45 209 bandera=1;
xccifuentess 0:9925883eec45 210 }
xccifuentess 0:9925883eec45 211 if(t>=0.3)
xccifuentess 0:9925883eec45 212 {
xccifuentess 0:9925883eec45 213 lcd.locate(3,0);lcd.printf(" ");
xccifuentess 0:9925883eec45 214 lcd.locate(3,0);lcd.printf("%d",err);
xccifuentess 0:9925883eec45 215 lcd.locate(11,0);lcd.printf(" ");
xccifuentess 0:9925883eec45 216 lcd.locate(11,0);lcd.printf("%d",med);
xccifuentess 0:9925883eec45 217 lcd.locate(3,1);lcd.printf(" ");
xccifuentess 0:9925883eec45 218 lcd.locate(3,1);lcd.printf("%d",spnum);
xccifuentess 0:9925883eec45 219 lcd.locate(11,1);lcd.printf(" ");
xccifuentess 0:9925883eec45 220 lcd.locate(11,1);lcd.printf("%d",pid);
xccifuentess 0:9925883eec45 221 bandera=0;
xccifuentess 0:9925883eec45 222 t.reset();
xccifuentess 0:9925883eec45 223 }
xccifuentess 0:9925883eec45 224
xccifuentess 0:9925883eec45 225 //Normalizacion de la salida
xccifuentess 0:9925883eec45 226 pidn=pid/999;
xccifuentess 0:9925883eec45 227 // se envia el valor pid a puerto analogico de salida (D/A) **************
xccifuentess 0:9925883eec45 228 u.write(pidn);
xccifuentess 0:9925883eec45 229 // se repite el ciclo
xccifuentess 0:9925883eec45 230 wait(0.005);
xccifuentess 0:9925883eec45 231 }
xccifuentess 0:9925883eec45 232
xccifuentess 0:9925883eec45 233 }