demo san
PID DISCRETO
Dependencies: mbed QEI TextLCD DebouncedIn
Diff: main.cpp
- Revision:
- 1:f0fa90a6fb6e
- Parent:
- 0:694446246c35
- Child:
- 2:921bc4072c83
diff -r 694446246c35 -r f0fa90a6fb6e main.cpp
--- a/main.cpp Mon Sep 28 22:43:43 2015 +0000
+++ b/main.cpp Tue Oct 27 01:10:35 2015 +0000
@@ -4,33 +4,37 @@
#include "stdio.h"
#include "TextLCD.h"
#include "DebouncedIn.h"
+#include "QEI.h"
//Declaracion de entradas y salidas
-DebouncedIn Boton1(PTE20); //Selector
-DebouncedIn Boton2(PTE21); //Aumento
-DebouncedIn Boton3(PTE22); //Disminuye
-DebouncedIn Boton4(PTE23); //Confirma
+DebouncedIn Boton1(PTE20); //Boton para confirmar
+DebouncedIn Boton2(PTE21); //Boton para confirmar
+
+//Configuracion encoder
+QEI wheel (PTD5, PTD0, NC, 100);
PwmOut control(PTE29);
AnalogIn vcond(PTB0);
-
+
+//Configuracion pantalla LCD
TextLCD lcd(PTB8,PTB9,PTB10,PTB11,PTE2,PTE3); // rs, e, d4-d7
//codigos movimiento del cursor LCD
-int C2=0x18; // desplaza izquierda
-int C3=0x1A; // desplaza derecha
-int C4=0x0C; // quito cursor bajo
-int C1=0x0F; // solo muestra el cursor
+//int C2=0x18; // desplaza izquierda
+//int C3=0x1A; // desplaza derecha
+//int C4=0x0C; // quito cursor bajo
+//int C1=0x0F; // solo muestra el cursor
-int Kp=0,Ki=0,Kd=0,Sp=0,p,i;
-float rT,eT,iT,dT,yT,uT,iT0=0,eT0=0,iT_1=0,eT_1=0;
-Timer tu;
-Timer td;
+int Kp=0,Ki=0,Kd=0;
+float Sp=0;
+float K1,K2,K3,ek=0,ekm1=0,ekm2=0,Ts=0.1; //Ts Tiempo Muestreo
+float yT,uk=0,ukm1=0;
+
+
int main() {
lcd.cls();
- //escribimos un comando segun el manual del modulo LCD
lcd.locate(1,0);
lcd.printf("Kp=%d",Kp);
lcd.locate(9,0);
@@ -38,7 +42,7 @@
lcd.locate(1,1);
lcd.printf("Kd=%d",Kd);
lcd.locate(9,1);
- lcd.printf("Sp=%d",Sp);
+ lcd.printf("Sp=%.1f",Sp);
set_Kp:
@@ -51,49 +55,14 @@
lcd.locate(1,1);
lcd.printf("Kd=%d",Kd);
lcd.locate(9,1);
- lcd.printf("Sp=%d",Sp);
+ lcd.printf("Sp=%.1f",Sp);
- //Kp=0;
while(1){
- if(Boton2.falling()){
- Kp++;
- tu.start();
- }
- //AUMENTO*10 **************
- if(Boton2.rising()){
- tu.stop();
- tu.reset();
- }
- if(tu.read()>=2.0 && tu.read()<4.0){
- Kp=Kp+10;
- wait(0.3);
- }
- else if(tu.read()>=4.0){
- Kp=Kp+100;
- wait(0.3);
- }
- //*************************
- if(Boton3.falling()){
- Kp--;
- td.start();
- }
- //DISMINUYE*10 **************
- if(Boton3.rising()){
- td.stop();
- td.reset();
- }
- if(td.read()>=2.0 && td.read()<4.0){
- Kp=Kp-10;
- wait(0.3);
- }
- else if(td.read()>=4.0){
- Kp=Kp-100;
- wait(0.3);
- }
- //*************************
-
+ Kp=Kp+wheel.getPulses();
+ wheel.reset();
+
if(Kp>=999){
Kp=999;
}
@@ -104,11 +73,12 @@
lcd.locate(0,0);
lcd.printf(">Kp=%d ",Kp);
- if(Boton1.falling()){
+ if(Boton2.falling()){
goto set_Ki;
}
- if(Boton4.falling()){
+
+ if(Boton1.falling()){
goto PID;
}
@@ -125,47 +95,13 @@
lcd.locate(1,1);
lcd.printf("Kd=%d",Kd);
lcd.locate(9,1);
- lcd.printf("Sp=%d",Sp);
+ lcd.printf("Sp=%.1f",Sp);
//Ki=0;
while(1){
- if(Boton2.falling()){
- Ki++;
- tu.start();
- }
- //AUMENTO*10 **************
- if(Boton2.rising()){
- tu.stop();
- tu.reset();
- }
- if(tu.read()>=2.0 && tu.read()<4.0){
- Ki=Ki+10;
- wait(0.3);
- }
- else if(tu.read()>=4.0){
- Ki=Ki+100;
- wait(0.3);
- }
- //*************************
- if(Boton3.falling()){
- Ki--;
- td.start();
- }
- //DISMINUYE*10 **************
- if(Boton3.rising()){
- td.stop();
- td.reset();
- }
- if(td.read()>=2.0 && td.read()<4.0){
- Ki=Ki-10;
- wait(0.3);
- }
- else if(td.read()>=4.0){
- Ki=Ki-100;
- wait(0.3);
- }
- //*************************
+ Ki=Ki+wheel.getPulses();
+ wheel.reset();
if(Ki>=999){
Ki=999;
@@ -177,11 +113,11 @@
lcd.locate(8,0);
lcd.printf(">Ki=%d ",Ki);
- if(Boton1.falling()){
+ if(Boton2.falling()){
goto set_Kd;
}
- if(Boton4.falling()){
+ if(Boton1.falling()){
goto PID;
}
@@ -198,48 +134,14 @@
lcd.locate(9,0);
lcd.printf("Ki=%d",Ki);
lcd.locate(9,1);
- lcd.printf("Sp=%d",Sp);
+ lcd.printf("Sp=%.1f",Sp);
//Kd=0;
while(1){
- if(Boton2.falling()){
- Kd++;
- tu.start();
- }
- //AUMENTO*10 **************
- if(Boton2.rising()){
- tu.stop();
- tu.reset();
- }
- if(tu.read()>=2.0 && tu.read()<4.0){
- Kd=Kd+10;
- wait(0.3);
- }
- else if(tu.read()>=4.0){
- Kd=Kd+100;
- wait(0.3);
- }
- //*************************
- if(Boton3.falling()){
- Kd--;
- td.start();
- }
- //DISMINUYE*10 **************
- if(Boton3.rising()){
- td.stop();
- td.reset();
- }
- if(td.read()>=2.0 && td.read()<4.0){
- Kd=Kd-10;
- wait(0.3);
- }
- else if(td.read()>=4.0){
- Kd=Kd-100;
- wait(0.3);
- }
- //*************************
+ Kd=Kd+wheel.getPulses();
+ wheel.reset();
if(Kd>=999){
Kd=999;
@@ -251,11 +153,11 @@
lcd.locate(0,1);
lcd.printf(">Kd=%d ",Kd);
- if(Boton1.falling()){
+ if(Boton2.falling()){
goto set_Sp;
}
- if(Boton4.falling()){
+ if(Boton1.falling()){
goto PID;
}
@@ -277,97 +179,63 @@
//Sp=0;
while(1){
- if(Boton2.falling()){
- Sp++;
- tu.start();
- }
- //AUMENTO*10 **************
- if(Boton2.rising()){
- tu.stop();
- tu.reset();
- }
- if(tu.read()>=2.0 && tu.read()<4.0){
- Sp=Sp+10;
- wait(0.3);
- }
- else if(tu.read()>=4.0){
- Sp=Sp+100;
- wait(0.3);
- }
- //*************************
- if(Boton3.falling()){
- Sp--;
- td.start();
- }
- //DISMINUYE*10 **************
- if(Boton3.rising()){
- td.stop();
- td.reset();
- }
- if(td.read()>=2.0 && td.read()<4.0){
- Sp=Sp-10;
- wait(0.3);
- }
- else if(td.read()>=4.0){
- Sp=Sp-100;
- wait(0.3);
- }
- //*************************
+ Sp=Sp+0.1*wheel.getPulses();
+ wheel.reset();
- if(Sp>=999){
- Sp=999;
+ if(Sp>=3.3){
+ Sp=3.3;
}
else if (Sp<=0){
Sp=0;
}
lcd.locate(8,1);
- lcd.printf(">Sp=%d ",Sp);
+ lcd.printf(">Sp=%.1f ",Sp);
- if(Boton1.falling()){
+ if(Boton2.falling()){
goto set_Kp;
}
- if(Boton4.falling()){
+ if(Boton1.falling()){
goto PID;
}
}
PID:
+
lcd.cls();
-
- //Sp=2;
- //Kp=(float)12.105;
- //Ki=(float)5.32;
- //Kd=(float)2;
while(1){
yT=vcond.read()*3.3;
- eT=Sp-yT;
- iT=Ki*eT+iT0; //Accion Integral
- dT=Kd*(eT-eT0); //Accion Derivativa
- uT=iT+Kp*eT+dT;
- if (uT>3.3) { //Salida PID si es mayor que el MAX
- uT=3.3;}
- else if (uT<0){ //Salida PID si es menor que el MIN
- uT=0;
- }
- iT0=iT; //Guardar variables
- eT0=eT;
- control=(float)uT/3.3;
+ ek=Sp-yT;
+
+ //Conversion de parametros PID Continuo a parametros de PID Digital
+ //ESTO ES UN PID DIGITAL Y LOS PARAMETROS AGREGADOS SON PARA UN PID CONTINUO!!
+ K1=Kp+(Ki/2)*Ts+(Kd/Ts);
+ K2=-Kp+(Ki/2)*Ts-(2*Kd/Ts);
+ K3=Kd/Ts;
+
+ uk=ukm1+K1*ek+K2*ekm1+K3*ekm2; //Accion de control
+
+ if (uk>3.3) { //Salida PID si es mayor que el MAX
+ uk=3.3;}
+ else if (uk<0){ //Salida PID si es menor que el MIN
+ uk=0;
+ }
+
+ control=(float)uk/3.3; //Accion de control mapeada a PWM; salida de la tarjeta.
+ ekm2=ekm1;
+ ekm1=ek;
+ ukm1=uk;
lcd.locate(0,0);
- lcd.printf("Error=%.2f ",eT);
+ lcd.printf("Error=%.2f ",ek);
lcd.locate(0,1);
lcd.printf("Y_act=%.2f",yT);
- wait(0.2);
+ wait(Ts); // Muestreos
}
- //lcd.locate(3,0);
- //lcd.writeCommand(C1);
-
-
-
+
}
\ No newline at end of file