Alejandro Marin
En este programa se presenta un PID modificando los valores a traves de un encoder
Dependencies: QEI TextLCD mbed
Diff: main.cpp
- Revision:
- 0:fb37cd782170
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp Wed Nov 13 02:59:07 2013 +0000
@@ -0,0 +1,216 @@
+#include "mbed.h"
+#include "DebouncedIn.h"
+#include "TextLCD.h"
+#include "QEI.h"
+
+AnalogIn Vin(PTC2);
+AnalogOut Vout(PTE30);
+
+QEI wheel (PTA16, PTA17, NC, 24);
+TextLCD lcd(PTB10, PTB11, PTE2, PTE3, PTE4, PTE5); // rs, e, d4-d7
+
+DigitalOut led1(LED1);
+DigitalOut led2(LED2);
+DebouncedIn button1(PTC12);
+DebouncedIn button2(PTC13);
+DebouncedIn button3(PTC16);
+DebouncedIn button4(PTC17);
+
+/* codigos movimiento del curzor
+ 18 para izquierda
+ 1A para derecha
+*/
+
+
+int C1=0x0E; // solo muestra el curzor
+int C2=0x18; // desplaza izquierda
+int C3=0x1A; // desplaza derecha
+int C4=0x0C; // quito cursor bajo
+int i; // indice de la variable
+int j;
+int Kp, Ki, Kd, Sp, yr, cycle, med2 , c,v,m;
+float med,pid,ap, err, ai, ad,err_v;
+int main() {
+
+ lcd.writeCommand(0x0E);
+ lcd.printf("Sp=0 Kp=0");
+ lcd.locate(0,1);
+ lcd.printf("Ki=0 Kd=0");
+ lcd.locate(2,0);
+ lcd.printf("=");
+
+ while(1) {
+
+ m=wheel.getPulses();
+ if(m<0){
+ wheel.reset();
+ m=0;
+ }
+ if(m!=v) {
+
+ switch(i) {
+
+ case 0:
+
+ lcd.locate(2,0);
+ lcd.printf("= ");
+ lcd.locate(3,0);
+ lcd.printf("%d",m);
+ Sp=m;
+
+ break;
+ case 1:
+
+ lcd.locate(10,0);
+ lcd.printf("= ");
+ lcd.locate(11,0);
+ lcd.printf("%d",m);
+ Kp=m;
+ break;
+ case 2:
+ lcd.locate(2,1);
+ lcd.printf("= ");
+ lcd.locate(3,1);
+ lcd.printf("%d",m);
+ Ki=m;
+ break;
+ case 3:
+ lcd.locate(10,1);
+ lcd.printf("= ");
+ lcd.locate(11,1);
+ lcd.printf("%d",m);
+ Kd=m;
+ break;
+ }
+ v=m;
+
+ }
+
+
+ if(button3.falling()) {
+ wheel.reset();
+ i++;
+ if(i>3) {
+ i=0;
+ }
+ switch (i) {
+ case 0:
+ lcd.locate(2,0);
+ lcd.printf("=");
+ break;
+ case 1:
+ lcd.locate(10,0);
+ lcd.printf("=");
+ break;
+ case 2:
+ lcd.locate(2,1);
+ lcd.printf("=");
+ break;
+ case 3:
+ lcd.locate(10,1);
+ lcd.printf("=");
+ break;
+ }
+ }
+
+
+ if (button4.falling()){
+ break; //sale del bucle si pisan suiche4
+ }
+
+ }
+ lcd.writeCommand(C1);//escribimos un comando segun el manual del modulo LCD para quitar cursor bajo
+ lcd.cls(); //borra la pantalla
+ lcd.printf(" GUARDADOS!");
+ wait(2);
+ lcd.cls();
+ lcd.printf(" INICIA EL PID");
+ wait(2);
+ // se imprimen los parches del control *****************************************
+ lcd.cls();
+
+
+ lcd.printf("Er%d",err);
+ lcd.locate(8,0);
+ lcd.printf("Me%d",med2);
+ lcd.locate(0,1);
+ lcd.printf("Sp%d",Sp);
+ lcd.locate(8,1);
+ lcd.printf("Pid%d",pid);
+ //wait(5);
+
+ // CICLO PRINCIPAL CONTROLADOR PID
+
+ while(1) {
+ //leer puerto analogo y asignar a med
+ med=Vin.read();
+ med2=med*100;
+ err = (Sp-med2);
+ float kp2;
+ kp2=Kp*0.001;
+ ap = kp2*err;
+ float ki2;
+ ki2=Ki*0.001;
+ ai =(ki2*err)+ai; //calculo de la integral del error
+ float kd2;
+ kd2=Kd*0.0001;
+ ad = kd2*(err-err_v); //calculo de la accion derivativa
+ err_v=err; //guarda el error
+ pid = (ap+ai+ad);
+
+ // se verifica que pid sea menor o igual la valor maximo *****************
+ if (pid > .99999){
+ pid=1;
+ }
+
+ // se verifica que pid sea positivo **************************************
+ if (pid <0){
+ pid=0;
+ }
+
+ //wait(.5);
+
+
+
+ // se verifica que la accion integral no sea muy grande
+ if (ai > 999){
+ ai=1000;
+ }
+
+
+ Vout=pid;
+
+
+ //****se muestran las variables******************************************
+ if(c>600){
+ lcd.locate(2,0);
+ lcd.printf(" ");
+ lcd.locate(0,0);
+ lcd.printf("Er%2.2f",err);
+ lcd.locate(10,0);
+ lcd.printf(" ");
+ lcd.locate(8,0);
+ lcd.printf("Me%d",med2);
+ lcd.locate(2,1);
+ lcd.printf(" ");
+ lcd.locate(0,1);
+ lcd.printf("Sp%d",Sp);
+ lcd.locate(10,1);
+ lcd.printf(" ");
+ lcd.locate(8,1);
+ lcd.printf("Pid%4.3f",pid);
+ c=0;
+ }
+ else
+ c++;
+
+
+
+
+
+
+
+ // se envia el valor pid a puerto analogico de salida (D/A) **************
+ // se repite el ciclo
+ }
+}
\ No newline at end of file