Laura Corrales
This program makes a PID control and the values por control action are entered by quadrature encoder.
Dependencies: DebouncedIn QEI TextLCD mbed
Diff: main.cpp
- Revision:
- 0:8e9ddfd2cbe2
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp Sun Dec 15 16:18:35 2013 +0000
@@ -0,0 +1,247 @@
+#include "mbed.h"
+#include "DebouncedIn.h"
+#include "TextLCD.h"
+#include "QEI.h"
+
+
+TextLCD lcd(PTB10, PTB11, PTE2, PTE3, PTE4, PTE5); // rs, e, d4-d7
+QEI wheel (PTA1, PTA2, NC, 50);
+
+AnalogIn y(PTC2);
+AnalogOut u(PTE30);
+
+DigitalOut led1(LED1);
+DigitalOut led2(LED2);
+DigitalOut led3(LED3);
+DebouncedIn button1(PTC7);
+DebouncedIn button2(PTC13);
+DebouncedIn button3(PTC0);
+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 C1=0x0F;
+
+int i; // indice de la variable
+int j; //variable controla cambio 4 posiciones
+int kp, ki, kd, sp, med2, yr, cycle, c;
+float pid, med, ap, ai, ad, err, err_v;
+
+int main() {
+ lcd.cls();
+ lcd.writeCommand(C1);//escribimos un comando segun el manual del modulo LCD
+ lcd.locate(0,0);
+ 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);
+
+
+ while(1) {
+
+ led3 =1;
+ if (button3.falling()) { //INCREMENTA POSICION DEL MENU CON BOTON 3 (Switche encoder)
+ led3 =!led3;
+ ++j;
+ }
+
+ if (j==0){
+ sp=sp+wheel.getPulses();
+ wheel.reset();
+ if (sp>999){
+ sp=999;
+ }
+ if (sp<0){
+ sp=0;
+ }
+ lcd.locate(2,0);
+ lcd.printf(" ",sp);
+ lcd.locate(2,0);
+ lcd.printf("%d",sp);
+ wait(0.2);
+
+ if(button3.falling()){
+ j=1;
+ led3=0;
+ wait(0.3);
+ wheel.reset();
+ }
+
+ }
+
+ if (j==1) {
+ kp=kp+wheel.getPulses();
+ wheel.reset();
+ if (kp>999){
+ kp=999;
+ }
+ if (kp<0){
+ kp=0;
+ }
+ lcd.locate(10,0);
+ lcd.printf(" ");
+ lcd.locate(10,0);
+ lcd.printf("%d",kp);
+ wait(0.2);
+
+ if(button3.falling()){
+ j=2;
+ led3=0;
+ wait(0.3);
+ wheel.reset();
+ }
+
+ }
+
+ if (j==2) {
+ ki=ki+wheel.getPulses();
+ wheel.reset();
+ if (ki>999){
+ ki=999;
+ }
+ if (ki<0){
+ ki=0;
+ }
+ lcd.locate(2,1);
+ lcd.printf(" ");
+ lcd.locate(2,1);
+ lcd.printf("%d",ki);
+ wait(0.2);
+
+ if(button3.falling()){
+ j=3;
+ led3=0;
+ wait(0.3);
+ wheel.reset();
+ }
+
+ }
+
+ if (j==3) {
+ kd=kd+wheel.getPulses();
+ wheel.reset();
+ if (kd>999){
+ kd=999;
+ }
+ if (kd<0){
+ kd=0;
+ }
+ lcd.locate(10,1);
+ lcd.printf(" ");
+ lcd.locate(10,1);
+ lcd.printf("%d",kd);
+ wait(0.2);
+
+ if(button3.falling()){
+ j=0;
+ led3=0;
+ wait(0.3);
+ wheel.reset();
+ }
+
+ }
+
+ if (j==4) {
+ j=0;
+ }
+
+ if (!button4){
+ break; //sale del bucle si pisan suiche4
+ }
+ } //cierro while(1)
+//%---------------------------------------------------------------------
+
+
+ lcd.writeCommand(C1);//escribimos un comando segun el manual del modulo LCD para quitar cursor bajo
+ lcd.cls(); //borra la pantalla
+ lcd.printf("GUARDAMOS \nVALORES |m|");
+ wait(2);
+ lcd.cls();
+ lcd.printf(" INICIA EL PID");
+ wait(2);
+ lcd.cls();
+
+ // se imprimen los parches del control *****************************************
+ lcd.locate(0,0);
+ 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("Con%d",pid);
+ //wait(3);
+
+
+ // CICLO PRINCIPAL CONTROLADOR PID
+
+ while(1) {
+ //leer puerto analogo y asignar a med
+ med=y.read();
+ med2=med*100;
+ err=(sp-med2);
+
+ float kpf;
+ kpf=kp*0.001;
+ ap = kpf*err;
+ float kif;
+ kif=ki*0.001;
+ ai =(kif*err)+ai;
+ float kdf;
+ kdf=kd*0.0001;
+ ad = kdf*(err-err_v);
+ err_v=err;
+ 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;
+ }
+ // se verifica que la accion integral no sea muy grande
+ if(ai>0.5){
+ ai =0.5;
+ }
+
+ u=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("Con%4.3f",pid);
+ c=0;
+ }
+ else
+ c++;
+
+ }
+
+
+
+}
\ No newline at end of file