Katherine Ortega / Mbed 2 deprecated TareaEncoder

Uso del encoder para un PID

Dependencies:   QEI TextLCD mbed

main.cpp@0:9575323f2bb3, 2013-12-11 (annotated)

Committer:
ikortegag
Date:
Wed Dec 11 21:30:11 2013 +0000
Revision:
0:9575323f2bb3
Implementaci?n de un encoder para un PID

Who changed what in which revision?

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