Uso del encoder para un PID
Dependencies: QEI TextLCD mbed
main.cpp
- Committer:
- ikortegag
- Date:
- 2013-12-11
- Revision:
- 0:9575323f2bb3
File content as of revision 0:9575323f2bb3:
#include "mbed.h" #include "TextLCD.h" #include "QEI.h" //Use X4 encoding. //QEI wheel(p29, p30, NC, 624, QEI::X4_ENCODING); //Use X2 encoding by default. AnalogIn Vin(PTC2); TextLCD lcd(PTB10, PTB11, PTE2, PTE3, PTE4, PTE5); // rs, e, d4-d7 QEI wheel (PTA1, PTA2, NC, 624); AnalogIn y(PTB0); AnalogOut u(PTE30); DigitalOut l1(LED1); DigitalOut l2(LED2); DigitalOut l3(LED3); DigitalIn b1(PTA1); DigitalIn b2(PTA2); DigitalIn b3(PTC16); DigitalIn b4(PTC17); int spnum=0,kpnum=0,kinum=0,kdnum=0,k=1; int bandera; Timer t; float tiempo; //codigos movimiento del curzor //int C1=0x0E; // solo muestra el curzor int C2=0x18; // desplaza izquierda int C3=0x1A; // desplaza derecha int C4=0x0C; // quito cursor bajo int C1=0x0F; int err, med, yr, pid, ap, ai, ad, err_v, cycle; float pidn; int pos=1; //int spnum=500, kinum=2, kpnum=1, kdnum=0, pos=1; //int incremento=1,flagt=0; int main() { lcd.cls(); // Borrar Pantalla lcd.writeCommand(C1);//escribimos un comando segun el manual del modulo LCD lcd.locate(8,0); lcd.printf("Kp=%d",kpnum); lcd.locate(0,1); lcd.printf("Ki=%d",kinum); lcd.locate(8,1); lcd.printf("Kd=%d",kdnum); lcd.locate(0,0); lcd.printf("Sp=%d",spnum); while(1) { wait(0.1); lcd.printf("Pulses is: %i\n", wheel.getPulses()); while(k==1) { if(b1 & spnum<=1000) { l1 = !l1; spnum++; lcd.locate(3,0); lcd.printf("%d",spnum); } if(b2 & spnum>=1) { l2 = !l2; wait(0.50); spnum--; lcd.locate(3,0); lcd.printf("%d",spnum); } if (b3) { k+=1; wait(0.25); lcd.locate(12,0); lcd.printf("%d",kpnum); } } while(k==2) { if(b1 & kpnum<=1000) { l1 = !l1; wait(0.50); kpnum++; lcd.locate(12,0); lcd.printf("%d",kpnum); } if(b2 & kpnum>=1) { l2 = !l2; wait(0.50); kpnum--; lcd.locate(12,0); lcd.printf("%d",kpnum); } if (b3) { k+=1; wait(0.25); lcd.locate(3,1); lcd.printf("%d",kinum); } } while(k==3) { if(b1 & kinum<=1000) { l1 = !l1; wait(0.50); kinum++; lcd.locate(3,1); lcd.printf("%d",kinum); } if(b2 & kinum>=1) { l2 = !l2; wait(0.50); kinum--; lcd.locate(3,1); lcd.printf("%d",kinum); } if (b3) { k+=1; wait(0.25); lcd.locate(12,1); lcd.printf("%d",kdnum); } } while(k==4) { if(b1 & kdnum<=1000) { l1 = !l1; wait(0.50); kdnum++; lcd.locate(12,1); lcd.printf("%d",kdnum); } if(b2 & kdnum>=1) { l2 = !l2; wait(0.50); kdnum--; lcd.locate(12,1); lcd.printf("%d",kdnum); } if (b3) { k=1; wait(0.25); lcd.locate(3,0); lcd.printf("%d",spnum); } } if (b4) { break; //sale del bucle si pisan suiche4 } continue; } //Transicion lcd.writeCommand(C4);//escribimos un comando segun el manual del modulo LCD para quitar cursor bajo lcd.cls(); //borra la pantalla lcd.printf("GUARDADOS!"); wait(1); lcd.cls(); lcd.printf(" INICIA EL PID"); wait(1); // se imprimen los parches del control ***************************************** lcd.cls(); lcd.printf("Er=%d",err); lcd.locate(8,0); lcd.printf("Me=%d",med); lcd.locate(0,1); lcd.printf("Sp=%d",spnum); lcd.locate(8,1); lcd.printf("Co=%d",pid); wait(2); // CICLO PRINCIPAL CONTROLADOR PID bandera=0; while(1) { med=999*y.read(); //leer puerto analogo y asignar a med err = (spnum-med); //se calcula el error ap = kpnum*err; //se calcula la accion proporcinal // se verifica que la accion integral no sea muy grande if(ai<100) { ai =(kinum*err)+ai; //calculo de la integral del error } ad = kdnum*(err-err_v); //calculo de la accion derivativa pid = (ap+ai+ad); // se verifica que pid sea positivo ************************************** if(pid<=0) { pid=0; } // se verifica que pid sea menor o igual la valor maximo ***************** if (pid > 999) { pid=999; } // se actualizan las variables ******************************************* err_v = err; //se muestran las variables****************************************** if(bandera==0) { t.start(); bandera=1; } if(t>=0.3) { lcd.locate(3,0);lcd.printf(" "); lcd.locate(3,0);lcd.printf("%d",err); lcd.locate(11,0);lcd.printf(" "); lcd.locate(11,0);lcd.printf("%d",med); lcd.locate(3,1);lcd.printf(" "); lcd.locate(3,1);lcd.printf("%d",spnum); lcd.locate(11,1);lcd.printf(" "); lcd.locate(11,1);lcd.printf("%d",pid); bandera=0; t.reset(); } //Normalizacion de la salida pidn=pid/999; // se envia el valor pid a puerto analogico de salida (D/A) ************** u.write(pidn); // se repite el ciclo wait(0.005); } }