Mauricio Peña
PID, donde las variaciones de SP, KP, KI y KD se producen por un encoder.
Dependencies: Debounced QEI TextLCD11 mbed
Diff: main.cpp
- Revision:
- 0:b83ad526e653
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp Fri Nov 15 17:00:26 2013 +0000
@@ -0,0 +1,270 @@
+#include "mbed.h"
+#include "TextLCD.h"
+#include "DebouncedIn.h"
+#include "QEI.h"
+
+TextLCD lcd(PTC10, PTC11, PTC12, PTC13, PTC16, PTC17);
+DebouncedIn next(PTE5);
+DebouncedIn menu(PTE4);
+int C1=0x0E;
+int C4=0x0C;
+AnalogOut Aout(PTE30);
+AnalogIn Ain(PTC2);
+QEI leftQei(PTD6, PTD7, NC, 624);
+
+int sp=0;
+int kp=0;
+int ki=0;
+int kd=0;
+
+int b;
+
+int c=0;
+int y=0;
+float i; // set point
+float j=0; // salida planta
+float ji=0; //variable proceso interno
+float spi=0; //variable proceso interno
+float h=0; // entrada planta
+float m=0; // Error
+float mv=0;
+float g=0;
+float x=0;
+float ap,ai,ad;
+
+//NECESITO COMPARACIÓN Y YA
+int pulsos (int x){
+
+
+ if (leftQei.getPulses()>b){
+ x++;
+ b=leftQei.getPulses();
+ }
+ if (leftQei.getPulses()<b){
+ x--;
+ b=leftQei.getPulses();
+ if (x<0){
+ x=0;
+ }
+ }
+ return x;
+} // Fin Int main función pulsos
+
+ int main() {
+ lcd.cls();
+ 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);
+ lcd.writeCommand(C1);//escribimos un comando segun el manual del modulo LCD
+ lcd.locate(0,0);
+ lcd.printf("Sp: %d",sp);
+
+
+
+ while(1) {
+ if (next.falling()) {
+ ++c;
+ } //CAMBIO DE POSICIÓN ENCODER
+ if (c==0){
+ lcd.locate(3,0);
+ lcd.putc(0xFE);
+ lcd.locate(4,0);
+ lcd.printf("%d",sp);
+
+ sp=pulsos(sp);//revisar si puedo dejar sp
+
+ //
+
+ if(sp>999){
+ sp=999;
+ }
+
+ //NEW !! (not zeros in -- process)
+ if(sp<10){
+ lcd.locate(5,0);
+ lcd.putc(0xFE);
+ }
+
+ if(sp<100){
+ lcd.locate(6,0);
+ lcd.putc(0xFE);
+ }
+
+ lcd.locate(4,0);
+ lcd.printf("%d", sp);
+ //wait(0.1);
+
+ //
+
+ }
+ if (c==1) {
+ lcd.locate(11,0);
+ lcd.putc(0xFE);
+ lcd.locate(12,0);
+ lcd.printf("%d",kp);
+ kp=pulsos(kp); //revisar si puedo dejar kp
+
+ //
+
+ if(kp>999){
+ kp=999;
+ }
+
+ //NEW !! (not zeros in -- process)
+ if(kp<10){
+ lcd.locate(13,0);
+ lcd.putc(0xFE);
+ }
+
+ if(kp<100){
+ lcd.locate(14,0);
+ lcd.putc(0xFE);
+ }
+
+ lcd.locate(12,0);
+ lcd.printf("%d", kp);
+ //wait(0.2);
+
+ //
+
+ }
+ if (c==2) {
+ lcd.locate(3,1);
+ lcd.putc(0xFE);
+ lcd.locate(4,1);
+ lcd.printf("%d",ki);
+ ki=pulsos(ki); //revisar si puedo dejar ki
+
+ //
+
+ if(ki>999){
+ ki=999;
+ }
+
+ //NEW !! (not zeros in -- process)
+ if(ki<10){
+ lcd.locate(5,1);
+ lcd.putc(0xFE);
+ }
+
+ if(ki<100){
+ lcd.locate(6,1);
+ lcd.putc(0xFE);
+ }
+
+ lcd.locate(4,1);
+ lcd.printf("%d", ki);
+ //wait(0.2);
+
+ //
+
+ }
+ if (c==3) {
+ lcd.locate(11,1);
+ lcd.putc(0xFE);
+ lcd.locate(12,1);
+ lcd.printf("%d",kd);
+ kd=pulsos(kd); //revisar si puedo dejar kd
+
+ //
+
+ if(kd>999){
+ kd=999;
+ }
+
+ //NEW !! (not zeros in -- process)
+ if(kd<10){
+ lcd.locate(13,1);
+ lcd.putc(0xFE);
+ }
+
+ if(kd<100){
+ lcd.locate(14,1);
+ lcd.putc(0xFE);
+ }
+
+ lcd.locate(12,1);
+ lcd.printf("%d", kd);
+ //wait(0.2);
+ }
+ if (c==4) {
+ c=0;
+ }
+
+ if (menu.falling()){
+ break;
+ }
+ } //While
+
+ lcd.writeCommand(C4);//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 *****************************************
+
+ i=sp/999;
+ lcd.cls();
+ lcd.printf("Er:%.2f",m);
+ lcd.locate(8,0);
+ lcd.printf("Me:%.2f",j);
+ lcd.locate(0,1);
+ lcd.printf("Sp:%d",sp);
+ lcd.locate(8,1);
+ lcd.printf("Co:%.1f",h);
+ wait(2);
+
+ //
+ while(1) {
+
+ spi=sp*(3.3/999);
+
+ //wait(0.3);
+ i=sp/999;
+ if(i<=1){
+
+ j=Ain;
+ ji=j*3.3;
+ m=(spi-ji);
+ //n=m*100;
+ ap=kp*m;
+ ai=(ki*m)+ai;
+
+ ad=kd*(m-mv);
+ h=ap+ai+ad;
+
+ if (h>999){
+ h=999;
+ }
+ if (h<0){
+ h=0;
+ }
+
+ g=(h/999);
+
+ Aout=g;
+ wait(0.22);
+
+
+ lcd.cls();
+ lcd.printf("Er:%.2f",m);
+ lcd.locate(8,0);
+ lcd.printf("Me:%.2f",j);
+ lcd.locate(0,1);
+ lcd.printf("Sp:%d",sp);
+ lcd.locate(8,1);
+ lcd.printf("Co:%.1f",h);
+ }
+
+
+ mv=m;
+ } //While
+ //
+
+ }//int main
\ No newline at end of file