Alexander Vallejo / Mbed 2 deprecated Tarea4

Encoder

Dependencies:   QEI TextLCD mbed

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

Committer:
avallejopo
Date:
Fri Nov 15 17:12:21 2013 +0000
Revision:
0:b4697bcfb555
Encoder

Who changed what in which revision?

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