Daniel Múnera
CONTROLADOR PID
Dependencies: DebounceIn DebouncedIn QEI TextLCD mbed
Diff: main.cpp
- Revision:
- 0:efe7bab86201
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp Thu Nov 30 01:04:42 2017 +0000
@@ -0,0 +1,410 @@
+#include "mbed.h"
+#include "TextLCD.h"
+#include "DebouncedIn.h"
+#include "QEI.h"
+
+//Declaracion de entradas y salidas
+DebouncedIn Boton1(PTE20); //Boton para confirmar
+DebouncedIn Boton2(PTE21); //Boton para confirmar
+AnalogOut accion_u(PTE30);
+AnalogIn vcond(PTB0);
+
+//Configuracion encoder
+QEI wheel (PTD5, PTD0, NC, 100);
+
+//Configuracion pantalla LCD
+TextLCD lcd(PTB8,PTB9,PTB10,PTB11,PTE2,PTE3); // rs, e, d4-d7
+
+int Kp=0,Ki=0,Kd=0,read=0,var,Kc=0,Tc,readc=0,cont_mayor=0;
+float Sp=0;
+float K1,K2,K3,ek=0,ekm1=0,ekm2=0,Ts=0.01; //Ts Tiempo Muestreo
+float yT,uk=0,ukm1=0,uk_inicial,uk_mayor,delta_uk,delta_uk_mayor,uc_mayor;
+Timer timer;
+Timer tc;
+
+
+int main()
+{
+
+
+ lcd.cls();
+ lcd.locate(1,0);
+ lcd.printf("Kp=%d",Kp);
+ lcd.locate(9,0);
+ lcd.printf("Ki%d=",Ki);
+ lcd.locate(1,1);
+ lcd.printf("Kd=%d",Kd);
+ lcd.locate(9,1);
+ lcd.printf("Sp=%.1f",Sp);
+
+
+set_Kp:
+ lcd.cls();
+ lcd.locate(0,0);
+ lcd.printf(">Kp= ");
+
+ lcd.locate(9,0);
+ lcd.printf("Ki=%d",Ki);
+ lcd.locate(1,1);
+ lcd.printf("Kd=%d",Kd);
+ lcd.locate(9,1);
+ lcd.printf("Sp=%.1f",Sp);
+
+
+ while(1) {
+
+ Kp=Kp+wheel.getPulses();
+ wheel.reset();
+
+ if(Kp>=999) {
+ Kp=999;
+ } else if (Kp<=0) {
+ Kp=0;
+ }
+
+ lcd.locate(0,0);
+ lcd.printf(">Kp=%d ",Kp);
+
+ if(Boton2.falling()) {
+ goto set_Ki;
+ }
+
+
+ if(Boton1.falling()) {
+ goto PID;
+ }
+
+ }
+
+set_Ki:
+
+ lcd.cls();
+ lcd.locate(8,0);
+ lcd.printf(">Ki= ");
+
+ lcd.locate(1,0);
+ lcd.printf("Kp=%d",Kp);
+ lcd.locate(1,1);
+ lcd.printf("Kd=%d",Kd);
+ lcd.locate(9,1);
+ lcd.printf("Sp=%.1f",Sp);
+
+ //Ki=0;
+ while(1) {
+
+ Ki=Ki+wheel.getPulses();
+ wheel.reset();
+
+ if(Ki>=999) {
+ Ki=999;
+ } else if (Ki<=0) {
+ Ki=0;
+ }
+
+ lcd.locate(8,0);
+ lcd.printf(">Ki=%d ",Ki);
+
+ if(Boton2.falling()) {
+ goto set_Kd;
+ }
+
+ if(Boton1.falling()) {
+ goto PID;
+ }
+
+ }
+
+set_Kd:
+
+ lcd.cls();
+ lcd.locate(0,1);
+ lcd.printf(">Kd= ");
+
+ lcd.locate(1,0);
+ lcd.printf("Kp=%d",Kp);
+ lcd.locate(9,0);
+ lcd.printf("Ki=%d",Ki);
+ lcd.locate(9,1);
+ lcd.printf("Sp=%.1f",Sp);
+
+
+
+ while(1) {
+
+ Kd=Kd+wheel.getPulses();
+ wheel.reset();
+
+ if(Kd>=999) {
+ Kd=999;
+ } else if (Kd<=0) {
+ Kd=0;
+ }
+
+ lcd.locate(0,1);
+ lcd.printf(">Kd=%d ",Kd);
+
+ if(Boton2.falling()) {
+ goto set_Sp;
+ }
+
+ if(Boton1.falling()) {
+ goto PID;
+ }
+
+ }
+
+set_Sp:
+
+ lcd.cls();
+ lcd.locate(8,1);
+ lcd.printf(">Sp= ");
+
+ lcd.locate(1,0);
+ lcd.printf("Kp=%d",Kp);
+ lcd.locate(9,0);
+ lcd.printf("Ki=%d",Ki);
+ lcd.locate(1,1);
+ lcd.printf("Kd=%d",Kd);
+
+ //Sp=0;
+ while(1) {
+
+ Sp=Sp+0.1*wheel.getPulses();
+ wheel.reset();
+
+ if(Sp>=3.3) {
+ Sp=3.3;
+ } else if (Sp<=0) {
+ Sp=0;
+ }
+
+ lcd.locate(8,1);
+ lcd.printf(">Sp=%.1f ",Sp);
+
+ if(Boton2.falling()) {
+ goto PID2;
+ }
+
+ if(Boton1.falling()) {
+ goto PID;
+ }
+ }
+
+
+
+PID2:
+ lcd.cls();
+ Kp = 1;
+
+while (Kp<100) {
+
+ read = timer.read();
+
+ if (read==0) {
+ timer.start();
+ uk_mayor=0;
+ }
+
+ if (read==1) {
+ uk_inicial=uk;
+ }
+
+ if (read==5) {
+ if (Kp<100) {
+ Kp = Kp+10;
+ }
+ timer.reset();
+ }
+
+ if (uk>uk_mayor) {
+ uk_mayor=uk;
+ }
+
+ if (read==4) {
+ delta_uk=uk_mayor-uk_inicial;
+ if (delta_uk>delta_uk_mayor) {
+ delta_uk_mayor=delta_uk;
+ Kc=Kp;
+ //Tc=2;
+ uc_mayor=uk_mayor;
+
+ }
+ }
+
+ if (Kp>100) {
+ goto CALCULO_TC;
+ }
+
+
+
+ yT=vcond.read()*3.3;
+ ek=Sp-yT;
+
+
+ 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.
+ accion_u = uk;
+
+ if (uk>uk_mayor) {
+ uk_mayor=uk;
+ }
+
+ ekm2=ekm1;
+ ekm1=ek;
+ ukm1=uk;
+ lcd.locate(0,0);
+ //lcd.printf("Error=%.2f ",ek);
+ lcd.printf("a_tunning");
+
+ }
+
+
+
+PID:
+ lcd.cls();
+ while(1){
+
+ yT=vcond.read()*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.
+ accion_u = uk;
+
+ ekm2=ekm1;
+ ekm1=ek;
+ ukm1=uk;
+ lcd.locate(0,0);
+ lcd.printf("Error=%.2f ",ek);
+ lcd.locate(0,1);
+ lcd.printf("Y_act=%.2f",yT);
+ wait(Ts); // Muestreos
+ }
+
+
+
+CALCULO_TC:
+ while(readc<21) {
+
+ Kp=Kc;
+ tc.start();
+ readc=tc.read();
+
+ if (uk==uc_mayor) {
+ cont_mayor= cont_mayor+1;
+ }
+
+ if (readc==20) {
+ Tc=20/cont_mayor;
+ goto CONTROL_AT;
+ }
+
+ yT=vcond.read()*3.3;
+ ek=Sp-yT;
+
+
+ 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.
+ accion_u = uk;
+
+ if (uk>uk_mayor) {
+ uk_mayor=uk;
+ }
+
+ ekm2=ekm1;
+ ekm1=ek;
+ ukm1=uk;
+ lcd.locate(0,0);
+
+ lcd.printf("a_tunnig");
+
+
+ }
+
+CONTROL_AT:
+
+ while(1) {
+ Kp=0.45*Kc;
+ Ki=0.54*Kc/Tc;
+ Kd=0;
+
+ yT=vcond.read()*3.3;
+ ek=Sp-yT;
+
+
+ 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.
+ accion_u = uk;
+
+ if (uk>uk_mayor) {
+ uk_mayor=uk;
+ }
+
+ ekm2=ekm1;
+ ekm1=ek;
+ ukm1=uk;
+ //lcd.locate(0,0);
+ //lcd.printf("Error=%.2f ",ek);
+ //lcd.printf("delta_mayor=%.2f",delta_uk_mayor);
+ //lcd.locate(0,1);
+ //lcd.printf("Kc=%I",Kc);
+ lcd.locate(0,0);
+ lcd.printf("Error=%.2f ",ek);
+ lcd.locate(0,1);
+ lcd.printf("Y_act=%.2f",yT);
+ wait(Ts); // Muestreos
+ lcd.locate(13,1);
+ lcd.printf("%.I",Kp);
+ wait(Ts); // Muestreos
+ }
+
+}
+
+
+