PID
Dependencies: Debounced FPointer QEI TextLCD keypad mbed
main.cpp
- Committer:
- Jesse
- Date:
- 2014-06-13
- Revision:
- 0:413ccc1d18e1
- Child:
- 1:7394ade42a1d
File content as of revision 0:413ccc1d18e1:
#include "mbed.h" #include "TextLCD.h" #include "QEI.h" #include "DebouncedIn.h" PwmOut Pwm1(PTA5);//Definicion de las Salidas. PwmOut Pwm2(PTD4);//Definicion de las Salidas.D DigitalOut led(LED1); DigitalOut led2(LED2); DigitalOut led3(LED3); DebouncedIn button3(PTC16); DebouncedIn button4(PTC17); Serial pc(USBTX, USBRX, "pc"); QEI wheel (PTD5, PTD0, NC, 24); AnalogIn y(PTB1); AnalogOut u(PTE30); TextLCD lcd(PTB10, PTB11, PTE2, PTE3, PTE4, PTE5); // rs, e, d4-d7 int C4=0x0C; // quito cursor bajo float PI,PIant; float medd; float sal; int i; // indice de la variable int j; //variable controla cambio 4 posiciones float kp=60, ki=0.26, kd=0, Sp,sp,Set=28,T; float Delta1=0,Delta2=0,Delta3=0,Delta22,Delta33; float yr, ap, ai, ad, err, med, err_v, cycle, pid,pid2; int b=0; float pidn; float f=100; //Frecuencia del PWM float pp=1/f; //Perioo del PWM Timer t; int Fs=10; float Ts=0.1; float inte; float intek; int PWMmodule1(float pp,float Dd) { Pwm1.period(pp); Pwm1.write(Dd); wait(0.01); } int PWMmodule2(float pp,float Dd) { Pwm2.period(pp); Pwm2.write(Dd); wait(0.01); } int main() { led=1; led2=0; medd=3.705503686*100*y.read(); sp=medd; Sp=sp; inte=Set-medd; PIant=0; lcd.cls(); //Valores Constantes, que se deben inicializar. lcd.locate(0,0); lcd.printf("Sp:%.1f",Set); lcd.locate(8,0); lcd.printf("T1:%.1f",Delta1); lcd.locate(0,1); lcd.printf("T2:%.1f",Delta2); lcd.locate(8,1); lcd.printf("T3:%.1f",Delta3); while(1) { led3 =1; if (button3.falling()) { //INCREMENTA POSICION DEL MENU CON BOTON 3 (Switche encoder) led3 =!led3; ++j; } if(j==0) { Set=Set+wheel.getPulses(); wheel.reset(); if (Set>999) { Set=999; } if (Set<0) { Set=0; } lcd.locate(3,0); lcd.printf(" ",Set); lcd.locate(3,0); lcd.printf("%.1f",Set); wait(0.2); if(button3.falling()) { j=1; led3=0; wait(0.3); wheel.reset(); } } if(j==1) { Delta1=Delta1+wheel.getPulses(); wheel.reset(); if (Delta1>12000) { Delta1=12000; } if (Delta1<0) { Delta1=0; } lcd.locate(11,0); lcd.printf(" "); lcd.locate(11,0); lcd.printf("%.1f",Delta1); wait(0.2); if(button3.falling()) { j=2; led3=0; wait(0.3); wheel.reset(); } } if(j==2) { Delta2=Delta2+wheel.getPulses(); wheel.reset(); if (Delta2>12000) { Delta2=12000; } if (Delta2<0) { Delta2=0; } lcd.locate(3,1); lcd.printf(" "); lcd.locate(3,1); lcd.printf("%.1f",Delta2); wait(0.2); if(button3.falling()) { j=3; led3=0; wait(0.3); wheel.reset(); } } if(j==3) { Delta3=Delta3+wheel.getPulses(); wheel.reset(); if (Delta3>12000) { Delta3=12000; } if (Delta3<0) { Delta3=0; } lcd.locate(11,1); lcd.printf(" "); lcd.locate(11,1); lcd.printf("%.1f",Delta3); wait(0.2); if(button3.falling()) { j=0; led3=0; wait(0.3); wheel.reset(); } } if(j==4) { j=0; } if(!button4) { break; } } //%%Cierro While-------------------------------------------- lcd.writeCommand(C4); lcd.cls(); //borra la pantalla lcd.printf(" GUARDAMOS \nVALORES |m|"); wait(2); Delta22=Delta2+Delta1; Delta33=Delta3+Delta2+Delta1; medd=3.4503686*100*y.read(); lcd.cls(); lcd.locate(0,0); lcd.printf("Sp:%.1f",Sp); lcd.locate(0,1); lcd.printf("Ts=%.1f",T); //lcd.locate(8,0); //lcd.printf("Er:%.1f",err); lcd.locate(8,0); lcd.printf("T%.1f",medd); lcd.locate(8,1); lcd.printf("u:%.1f",PI); wait(2); while(1) { medd=3.45503686*100*y.read(); err = (Sp-medd); inte=inte+err*Ts; PI=(kp*err+ ki*inte); pid=PI/100; pid2=1-pid; if(PI>100) { PI=100; } if(PI<0) { PI=0; } //Funcion Rampa if(T<=Delta1) { lcd.cls(); lcd.locate(0,0); Sp=Sp+(((Set-sp)/Delta1))*Ts; lcd.printf("Sp=%.1f",Sp); lcd.locate(0,1); lcd.printf("Ts=%.1f",T); lcd.locate(8,0); lcd.printf("Er:%.1f",medd); lcd.locate(8,1); lcd.printf("u:%.1f",PI); PWMmodule1(pp,pid); PWMmodule2(pp,pid2); } if((T>Delta1)&&(T<=Delta22)) { Sp=Sp; lcd.cls(); lcd.printf("Sp=%.1f",Sp); lcd.locate(0,1); lcd.printf("Ts=%.1f",T); lcd.locate(8,0); lcd.printf("Er:%.1f",medd); lcd.locate(8,1); lcd.printf("u:%.1f",PI); PWMmodule1(pp,pid); PWMmodule2(pp,pid2); } if((T>Delta22)&&(T<=Delta33)) { lcd.cls(); lcd.printf("Sp=%.1f",Sp); lcd.locate(0,1); Sp=Sp-(((Set-sp)/Delta3))*Ts; lcd.printf("Ts=%.1f",T); lcd.locate(8,0); lcd.printf("Er:%.1f",medd); lcd.locate(8,1); lcd.printf("u:%.1f",PI); PWMmodule1(pp,pid); PWMmodule2(pp,pid2); //pid=1-(PI/100); led=1; led2=0; /*PWMmodule2(pp,pid); PWMmodule1(pp,0);*/ } //Fin de funcion rampa. //Inicio del PID /* if (PI>=PIant) { pid=PI/100; led=0; led2=1; PWMmodule1(pp,pid); PWMmodule2(pp,0); } if (PI<PIant) { pid2=1-(PI/100); led=1; led2=0; PWMmodule2(pp,pid2); PWMmodule1(pp,0); } */ PIant=PI; lcd.locate(8,0); lcd.printf("Er:%.1f",medd); lcd.locate(8,1); lcd.printf("u:%.1f",PI); //Fin del PID /*lcd.cls(); lcd.locate(0,0); lcd.printf("T:%d",medd); lcd.locate(0,1); lcd.printf("u:%.2f",PI); lcd.locate(5,0); lcd.printf(":%.2f",inte); */ T=T+Ts; } }