Software para realizar control PID en tarjeta Freescale KL25Z con ENCODER y Display 16x2
Dependencies: DebouncedIna QEI TextLCD1 mbed
main.cpp
- Committer:
- cmorab
- Date:
- 2013-11-20
- Revision:
- 0:10fa9fae4bf9
File content as of revision 0:10fa9fae4bf9:
//Software para realizar control PID en tarjeta Freescale KL25Z con ENCODER y Display 16x2 #include "mbed.h" #include "DebouncedIn.h" #include "TextLCD.h" #include "QEI.h" AnalogIn Vin(PTC2); // AnalogOut Vout(PTE30); AnalogIn y(PTB0); // Revizar su funcion AnalogOut u(PTE30); // Revizar su funcion TextLCD lcd(PTB10, PTB11, PTE2, PTE3, PTE4, PTE5); // rs, e, d4-d7 QEI wheel (PTA1, PTA2, NC, 100); DigitalOut led1(LED1); DigitalOut led2(LED2); DigitalOut led3(LED3); DebouncedIn button1(PTC12); DebouncedIn button2(PTC13); DebouncedIn button3(PTD4); DebouncedIn button4(PTA12); //int C1=0x0E; // solo muestra el curzor int C1=0x0F; // PORQUE???? int C2=0x18; // desplaza izquierda int C3=0x1A; // desplaza derecha int C4=0x0C; // quito cursor bajo int i; // Indice de la variable int j; // Variable controla cambio 4 posiciones int kp, ki, kd, sp, err, med, err_v, ap, ai, ad, pid; int cont, sal, yr , cycle; float pidn; int main() { lcd.cls(); lcd.writeCommand(C1); lcd.locate(0,0); lcd.printf("Sp%d",sp); lcd.locate(8,0); lcd.printf("Kp%d",kp); lcd.locate(0,1); lcd.printf("Ki%d",ki); lcd.locate(8,1); lcd.printf("Kd%d",kd); while(1) { led3 =1; if (button3.falling()){ //Posición Encoder Boton 3. led3 =!led3; ++j; } if (j==0){ sp=sp+wheel.getPulses(); wheel.reset(); if (sp>1000){ sp=1000; } if (sp<0){ sp=0; } lcd.locate(3,0); lcd.printf(" "); //En futuro probrar sin Sp. lcd.locate(3,0); lcd.printf("%d",sp); wait(0.2); if(button3.falling()){ j=1; led3=0; wait(0.3); wheel.reset(); } } if (j==1) { kp=kp+wheel.getPulses(); wheel.reset(); if (kp>1000){ kp=1000; } if (kp<0){ kp=0; } lcd.locate(11,0); lcd.printf(" "); lcd.locate(11,0); lcd.printf("%d",kp); wait(0.3); if(button3.falling()){ j=2; led3=0; wait(0.3); wheel.reset(); } } if (j==2) { ki=ki+wheel.getPulses(); wheel.reset(); if (ki>1000){ ki=1000; } if (ki<0){ ki=0; } lcd.locate(3,1); lcd.printf(" "); lcd.locate(3,1); lcd.printf("%d",ki); wait(0.3); if(button3.falling()){ j=3; led3=0; wait(0.3); wheel.reset(); } } if (j==3) { kd=kd+wheel.getPulses(); wheel.reset(); if (kd>1000){ kd=1000; } if (kd<0){ kd=0; } lcd.locate(11,1); lcd.printf(" "); lcd.locate(11,1); lcd.printf("%d",kd); wait(0.3); if(button3.falling()){ j=4; // CAMBIADO CERO POR 4. led3=0; wait(0.3); wheel.reset(); } } if (j==4) { j=0; } if (!button4){ break; //sale del bucle si pisan suiche4 } } //cierro while(1) /////////////////////////////////////////////////////////////////////////////////////////// lcd.writeCommand(C4);//escribimos un comando segun el manual del modulo LCD para quitar cursor bajo lcd.cls(); //borra la pantalla lcd.printf("Guardando '_'"); wait(2); // Se imprimen los parches del control. lcd.cls(); lcd.printf("Err %d",err); lcd.locate(8,0); lcd.printf("Med %d",med); lcd.locate(0,1); lcd.printf("Sp %d",sp); lcd.locate(8,1); lcd.printf("Co %d",pid); wait(1); // CICLO PRINCIPAL CONTROLADOR PID while(1) { med=999*y.read(); //leer puerto analogo y asignar a med err = (sp-med); ap = kp*err; // se verifica que la accion integral no sea muy grande if(ai<1000){ ai =(ki*err)+ai; //calculo de la integral del error } ad = kd*(err-err_v); //calculo de la accion derivativa pid = (ap+ai+ad); // PID siempre positivo. if(pid<=0){ pid=0; } // Limite máximo PID if (pid > 9000){ pid=9000; } //se muestran las variables****************************************** wait(0.3); lcd.locate(3,0); lcd.printf(" "); lcd.locate(3,0); lcd.printf("%d",err); lcd.locate(12,0); lcd.printf(" "); lcd.locate(12,0); lcd.printf("%d",med); lcd.locate(3,1); lcd.printf(" "); lcd.locate(3,1); lcd.printf("%d",sp); lcd.locate(12,1); lcd.printf(" "); lcd.locate(12,1); lcd.printf("%d",pid); //Normalizacion de la salida pidn=pid/999; // se envia el valor pid a puerto analogico de salida (D/A) ************** u.write(pidn); err_v = err; // se repite el ciclo } }