Nicolás Villegas Echavarría
a
Dependencies: QEI TextLCD mbed
Fork of
PID_ENCODER_OK
by 
Gustavo Ramirez
Diff: main.cpp
- Revision:
- 2:00db719ebd81
- Parent:
- 1:058b8f5c135d
--- a/main.cpp Sun Apr 24 18:32:52 2016 +0000
+++ b/main.cpp Wed May 10 22:36:03 2017 +0000
@@ -1,306 +1,251 @@
+/*
+ * Código por: Laura Álvila
+ * Daniela López
+ * Nicolás Villegas
+ */
+
#include "mbed.h"
#include "QEI.h"
#include "TextLCD.h"
+Serial GSM(PTE0,PTE1); // Módulo Bluetooh
TextLCD lcd(PTB10, PTB11, PTE2, PTE3, PTE4, PTE5); // rs, e, d4-d7
-QEI encoder (PTA13, PTD5, NC, 624);
-AnalogIn y(PTB3);//entrada analoga
-AnalogOut u(PTE30);//salida analoga OJO solo se le pueden drenar 1.5mA en circuitos use un Buffer
-//si se ignora esto se arruina la FRDMKL25Z
+QEI encoder (PTA13, PTD5, NC, 1000);
+
+
+AnalogIn y(PTC2); // Entrada análoga: Salida del sistema
+AnalogOut u(PTE30); // Salida análoga: Señal de control
+
DigitalOut led1(LED1);
DigitalOut led2(LED2);
DigitalOut led3(LED3);
-DigitalIn button3(PTC16);//cambia ingreso de los 4 parametros
-DigitalIn button4(PTC17);//termina y consolida valores de 4 parametros y sale del loop
-
+DigitalIn button3(PTC16); // Botón del encoder
+DigitalIn button4(PTC17); // Pulsador
-//codigos movimiento del curzor
-
-//int C1=0x0E; // solo muestra el curzor
+// Códigos movimiento del cursor:
+int C1=0x0F;
int C2=0x18; // desplaza izquierda
int C3=0x1A; // desplaza derecha
int C4=0x0C; // quito cursor bajo
-int C1=0x0F;
-int cambio=0, diferencia=0;
-float pid,o,ai,ad,ap,med,err;
+int cambio = 0, diferencia = 0;
+float pid, o, ai, ad, ap, med, err;
float err_v;
-int spnum=0,kinum=0,kpnum=0,kdnum=0,pos=1;
+int spnum = 0, kinum = 0, kpnum = 0 ,kdnum = 0, pos = 1;
+int j,k;
-int main()
-{
+int main() {
+ // Se asigna baudrate y se configura el puerto serie de la USART
+ GSM.baud(9600);
+ GSM.format(8,Serial::None,1);
+
lcd.locate(0,1);
lcd.printf("**Control PID**");
- wait(2);
- lcd.cls(); // Borrar Pantalla
- lcd.writeCommand(C1);//escribimos un comando segun el manual del modulo LCD
+ wait(3);
+
+ lcd.cls(); // Borrar Pantalla
+ lcd.writeCommand(C1); // Escribimos un comando segun el manual del modulo LCD
lcd.locate(8,0);
- lcd.printf("Kp=%d",kpnum);
+ lcd.printf("Kp=%d", kpnum);
lcd.locate(0,1);
- lcd.printf("Ki=%d",kinum);
+ lcd.printf("Ki=%d", kinum);
lcd.locate(8,1);
- lcd.printf("Kd=%d",kdnum);
+ lcd.printf("Kd=%d", kdnum);
lcd.locate(0,0);
- lcd.printf("Sp=%d",spnum);
+ lcd.printf("Sp=%d", spnum);
- while(1)
- {
- //lcd.locate(8,0);
- //lcd.printf("Kp=%d",encoder.getPulses());
- //wait(.5);
+ while(true) {
- diferencia=encoder.getPulses()-cambio;
+ diferencia=encoder.getPulses() - cambio;
cambio=encoder.getPulses();
-
- if (diferencia==0)
- {
- //nada
- }
- else if(diferencia>0)
- {
- if(pos==1)
- {
- if(spnum+diferencia>=999)
- {
- spnum=999;
+
+ if (diferencia != 0) {
+ switch (pos) {
+ case 1:
+ spnum += diferencia;
+
+ if (spnum >= 999)
+ spnum = 999;
+ else if (spnum < 0)
+ spnum = 0;
+
lcd.locate(3,0);
lcd.printf(" ");
lcd.locate(3,0);
lcd.printf("%d", spnum);
- }
- else
- {
- spnum+=diferencia;
- lcd.locate(3,0);
- lcd.printf("%d", spnum);
- }
- }
- else if(pos==2)
- {
- if(kpnum+diferencia>=999)
- {
- kpnum=999;
+ break;
+ case 2:
+ kpnum += diferencia;
+
+ if (kpnum >= 999)
+ kpnum = 999;
+ else if (kpnum < 0)
+ kpnum = 0;
lcd.locate(11,0);
lcd.printf(" ");
lcd.locate(11,0);
lcd.printf("%d", kpnum);
- }
- else
- {
- kpnum+=diferencia;
- lcd.locate(11,0);
- lcd.printf("%d", kpnum);
- }
- }
- else if(pos==3)
- {
- if(kinum+diferencia>=999)
- {
- kinum=999;
+ break;
+ case 3:
+ kinum += diferencia;
+
+ if (kinum >= 999)
+ kinum = 999;
+ else if (kinum < 0)
+ kinum = 0;
+
lcd.locate(3,1);
lcd.printf(" ");
lcd.locate(3,1);
lcd.printf("%d", kinum);
- }
- else
- {
- kinum+=diferencia;
- lcd.locate(3,1);
- lcd.printf("%d", kinum);
- }
- }
- else if(pos==4)
- {
- if(kdnum+diferencia>=999)
- {
- kdnum=999;
+ break;
+ case 4:
+ kdnum += diferencia;
+
+ if (kdnum >= 999)
+ kdnum = 999;
+ else if (kdnum < 0)
+ kdnum = 0;
+
lcd.locate(11,1);
lcd.printf(" ");
lcd.locate(11,1);
lcd.printf("%d", kdnum);
- }
- else
- {
- kdnum+=diferencia;
- lcd.locate(11,1);
- lcd.printf("%d", kdnum);
- }
- }
- }
+ break;
+ default:
+ break;
+ } // Fin switch
+ } // Fin if
+
+ if (!button3) { // Cambia la posición de ingreso de parámetros
- else if(diferencia<0)
- {
- if(pos==1)
- {
- if(spnum+diferencia<0)
- {
- //No ocurre nada
- }
- else
- {
- spnum+=diferencia;
- lcd.locate(3,0);
- lcd.printf(" ");
+ led3 = !led3;
+
+ switch(pos++) {
+ case 4:
+ pos = 1;
lcd.locate(3,0);
lcd.printf("%d", spnum);
- }
- }
- else if(pos==2)
- {
- if(kpnum+diferencia<0)
- {
- //No ocurre nada
- }
- else
- {
- kpnum+=diferencia;
- lcd.locate(11,0);
- lcd.printf(" ");
+ break;
+ case 1:
lcd.locate(11,0);
lcd.printf("%d", kpnum);
- }
- }
- else if(pos==3)
- {
- if(kinum+diferencia<0)
- {
- //No ocurre nada
- }
- else
- {
- kinum+=diferencia;
- lcd.locate(3,1);
- lcd.printf(" ");
+ break;
+ case 2:
lcd.locate(3,1);
lcd.printf("%d", kinum);
- }
- }
- else if(pos==4)
- {
- if(kdnum+diferencia<0)
- {
- //No ocurre nada
- }
- else
- {
- kdnum+=diferencia;
- lcd.locate(11,1);
- lcd.printf(" ");
+ break;
+ case 3:
lcd.locate(11,1);
lcd.printf("%d", kdnum);
- }
- }
- }
+ break;
+ default:
+ break;
+ } // Fin switch
+ wait(0.25);
+ } // Fin if
+
- if (!button3) //cambia la posicion de ingreso de parametros
- {
- led3 =!led3;
- if(pos==4)
- {
- pos=1;
- lcd.locate(3,0);
- lcd.printf("%d", spnum);
- }
- else if (pos==1)
- {
- pos++;
- lcd.locate(11,0);
- lcd.printf("%d", kpnum);
- }
- else if(pos==2)
- {
- pos++;
- lcd.locate(3,1);
- lcd.printf("%d", kinum);
- }
- else if(pos==3)
- {
- pos++;
- lcd.locate(11,1);
- lcd.printf("%d", kdnum);
- }
- wait(0.25);
-
- }
-
- if (!button4)
- {
+ if (!button4) {
break; //sale del bucle si pisan suiche4
}
wait(0.1);
}
-//Transicion
- lcd.writeCommand(C4);//escribimos un comando segun el manual del modulo LCD para quitar cursor bajo
- lcd.cls(); //borra la pantalla
+// Transición
+ lcd.writeCommand(C4); // Escribimos un comando segun el manual del módulo LCD para quitar cursor bajo
+ lcd.cls(); // Borra la pantalla
lcd.printf(" GUARDADOS!");
wait(1);
lcd.cls();
- lcd.printf(" INICIA EL PID");
+ lcd.printf(" *** PID ***");
wait(1);
-// se imprimen los parches del control *****************************************
+
lcd.cls();
- lcd.printf("Er=%3.0f",err);
+ lcd.printf("e=%3.0f",err);
lcd.locate(8,0);
- lcd.printf("Me=%3.0f",med);
+ lcd.printf("y=%3.0f",med);
lcd.locate(0,1);
- lcd.printf("Sp=%3.0f",spnum);
+ lcd.printf("r=%3.0f",spnum);
lcd.locate(8,1);
- lcd.printf("Co=%3.0f",pid);
+ lcd.printf("u=%3.0f",pid);
wait(1);
+int referencia;
+int salida;
+
// CICLO PRINCIPAL CONTROLADOR PID
- lop1: med = y.read()*999;
- err = (spnum-med); //se calcula el error
- ap = kpnum*err*0.01f; //se calcula la accion proporcinal
- ai =(kinum*err*0.01f)+ai; //calculo de la integral del error
- ad = kdnum*(err-err_v)*0.01f; //calculo de la accion derivativa
- pid = (ap+ai+ad);
- // se verifica que pid sea positivo **************************************
- if(pid<=0)
- {
- pid=0;
+loop: med = y.read()*999;
+ err = spnum - med; // Se calcula el error
+ ap = kpnum*err*0.01f; // Se calcula la acción proporcional
+ ai += kinum*err*0.01f; // Cálculo de la integral del error
+ ad = kdnum*(err - err_v)*0.01f; // Cálculo de la acción derivativa
+ pid = ap + ai + ad;
+
+ referencia = (spnum / 999) * 255;
+ salida = med / 999 * 743 + 256;
+
+ if (referencia < 256) { // Debo generar dos casos a APP inventor solo me recibe hex asi: 0xhhhh (4 cifras)
+ GSM.putc(0); // si el numero es hasta 255 se le ponen dos ceros adelante a la secuencia de bits
+ GSM.putc(referencia); // luego la cifra menos significativa
}
-
- // se verifica que pid sea menor o igual la valor maximo *****************
- if (pid > 999)
- {
- pid=999;
+ /*
+
+ if (salida > 255){ // pero si es mayor a 255 las cifras deben ser convertidas a un hex de dos bytes de la siguiente forma
+ j = salida/256; // calculo la cifra mas significativa
+ k = salida - j*256; // calculo la cifra menos significativa
+ GSM.putc(j); // las envio a la usart para que se las ponga al modulo bluetooth y la lleve al android
+ GSM.putc(k); // mas significativa primero, menos despues si no no funciona!!! y con la orden PUTC solo asi le envia binarios
+ }
+
+
+ GSM.putc('r'); // Se envía el set-point al módulo Bluetooth.
+ if (spnum < 256) {
+ GSM.putc(0);
+ GSM.putc(spnum);
}
+ if (spnum > 255){
+ j = spnum/256;
+ k = spnum - j*256;
+ GSM.putc(j);
+ GSM.putc(k);
+ }
+ */
+
+ // Se verifica que pid sea positivo:
+ if (pid <= 0)
+ pid = 0;
+ else if (pid > 999)
+ pid = 999;
- //se muestran las variables******************************************
- lcd.locate(3,0);
- lcd.printf(" ");
- lcd.locate(3,0);
- lcd.printf("%3.0f",err);
- lcd.locate(11,0);
- lcd.printf(" ");
- lcd.locate(11,0);
- lcd.printf("%3.0f",med);
- lcd.locate(3,1);
- lcd.printf(" ");
- lcd.locate(3,1);
- lcd.printf("%d",spnum);
- lcd.locate(11,1);
- lcd.printf(" ");
- lcd.locate(11,1);
- lcd.printf("%3.0f",pid);
+ // Se muestran las variables en LCD:
+ lcd.locate(3,0);
+ lcd.printf(" ");
+ lcd.locate(3,0);
+ lcd.printf("%3.0f",err);
+ lcd.locate(11,0);
+ lcd.printf(" ");
+ lcd.locate(11,0);
+ lcd.printf("%3.0f",med);
+ lcd.locate(3,1);
+ lcd.printf(" ");
+ lcd.locate(3,1);
+ lcd.printf("%d",spnum);
+ lcd.locate(11,1);
+ lcd.printf(" ");
+ lcd.locate(11,1);
+ lcd.printf("%3.0f",pid);
-
-
-
- //Normalizacion de la salida
- // se actualizan las variables *******************************************
+ // Normalización de la salida y actualización de variables:
err_v = err;
o = pid/999;
u.write(o);
- // se envia el valor pid a puerto analogico de salida (D/A) **************
- // se repite el ciclo
+ // Se repite el ciclo:
wait_ms(300);
- goto lop1;
+ goto loop;
}