Aldaiir Navarro G
/
T1_PID_AppInv_Encoder
Important changes to repositories hosted on mbed.com
Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.
To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.
It is also possible to export all your personal repositories from the account settings page.
Dependencies: QEI TextLCD mbed
Fork of
PID
by 
Team La Verga
Diff: main.cpp
- Revision:
- 1:e1360b8db5e0
- Parent:
- 0:8ce5f8fa8f9b
--- a/main.cpp Thu Oct 27 02:58:30 2016 +0000
+++ b/main.cpp Wed Nov 30 04:07:18 2016 +0000
@@ -3,16 +3,17 @@
#include "TextLCD.h"
#include "stdio.h"
#include "string.h"
+#include "stdlib.h"
+#include "math.h"
-Serial pc(USBTX,USBRX); //puertos del PC
+// Serial pc(USBTX,USBRX); //puertos del PC
+Serial pc(PTE0,PTE1);
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
-DigitalOut led1(LED1);
-DigitalOut led2(LED2);
-DigitalOut led3(LED3);
+//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
@@ -24,7 +25,7 @@
int C1=0x0F;
char buffer[6];// TAMAÑO DEL BUFER
-char cadena[3];
+char cadena[3], mm[6];
Timer t; //VALOR DEL TIEMPO
int count;
int i = 0;
@@ -33,8 +34,12 @@
float pid,o,ai,ad,ap,med,err;
float err_v;
int spnum=0,kinum=0,kpnum=0,kdnum=0,num=0,pos=1;
+char bloqueo='d';
-int readBuffer(char *buffer,int count) //esta funcion lee un bufer de datos
+
+//============================================================================ Function para limpiar Buffer =================
+
+int readBuffer(char *buffer,int count)
{
int i=0;
t.start(); //CUENTA EL TIEMPO DE CONEXION E INICIA
@@ -47,400 +52,307 @@
}
if(i > count)break;
if(t.read() > 1) { //MAS DE UN SEGUNDO DE ESPERA SE SALE Y REINICA EL RELOJ Y SE SALE
+
+// ======== Analizar aca, tiempo ========================== //
t.stop();
t.reset();
break;
}
}
- return 0;
-}
+ return 0; }
-void cleanBuffer(char *buffer, int count) //esta funcion limpia el bufer
-{
- for(int i=0; i < count; i++) {
- buffer[i] = '\0';
- }
-}
+//============================================================================ Function para limpiar Buffer ==============================
+void cleanBuffer(char *buffer, int count)
+ {
+ for(int i=0; i < count; i++) {
+ buffer[i] = '\0'; } }
+
-void LecturaSerial(void){
- /***********
- lcd.locate(8,0);
- lcd.printf("Kp=%d",kpnum);
- lcd.locate(0,1);
- lcd.printf("Ki=%d",kinum);
- lcd.locate(8,1);
- lcd.printf("Kd=%d",kdnum);
- lcd.locate(0,0);
- lcd.printf("Sp=%d",spnum);
- *************/
-
- while(1){
- if (pc.readable()) {
- readBuffer(buffer,6);
- wait(0.2);
- pc.printf("%s\n",buffer); //imprime el bufer
+//============================================================================ Function Leer Parámetros del Celular ===========================
+void LecturaSerial(void){
+ while(bloqueo=='b'){
+
+ cleanBuffer(buffer,6);
+ readBuffer(buffer,6);
+
+ if (buffer[0]=='G'||buffer[0]=='d') { break;}
+
cadena[0]=buffer[2];
cadena[1]=buffer[3];
cadena[2]=buffer[4];
num=strtod(cadena,NULL);
- if(num>999){
- num=999;
- }
- if(num<0){
- num=0;
- }
- if (buffer[0]=='S' && buffer[1]=='P'){
- spnum=num;
- lcd.locate(3,0);
- lcd.printf(" ");
- lcd.locate(3,0);
- lcd.printf("%d", spnum);
-
- }
+ if(num>999) {num=999; } if(num<0) {num=0; }
+
+ if (buffer[0]=='S' && buffer[1]=='P')
+ { spnum=num;
+ lcd.locate(3,0); lcd.printf(" ");
+ lcd.locate(3,0); lcd.printf("%d", spnum);
+ pos=1; diferencia=0 ; break; }
- if (buffer[0]=='K' && buffer[1]=='P'){
- kpnum=num;
- lcd.locate(11,0);
- lcd.printf(" ");
- lcd.locate(11,0);
- lcd.printf("%d", kpnum);
- }
+ if (buffer[0]=='K' && buffer[1]=='P')
+ { kpnum=num;
+ lcd.locate(11,0); lcd.printf(" ");
+ lcd.locate(11,0); lcd.printf("%d", kpnum);
+ pos=2; diferencia=0 ; break;}
- if (buffer[0]=='K' && buffer[1]=='I'){
- kinum=num;
- lcd.locate(3,1);
- lcd.printf(" ");
- lcd.locate(3,1);
- lcd.printf("%d", kinum);
- }
+ if (buffer[0]=='K' && buffer[1]=='I')
+ { kinum=num;
+ lcd.locate(3,1); lcd.printf(" ");
+ lcd.locate(3,1); lcd.printf("%d", kinum);
+ pos=3; diferencia=0 ; break ;}
- if (buffer[0]=='K' && buffer[1]=='D'){
- kdnum=num;
- lcd.locate(11,1);
- lcd.printf(" ");
- lcd.locate(11,1);
- lcd.printf("%d", kdnum);
- }
+ if (buffer[0]=='K' && buffer[1]=='D')
+ { kdnum=num;
+ lcd.locate(11,1); lcd.printf(" ");
+ lcd.locate(11,1); lcd.printf("%d", kdnum);
+ pos=4; diferencia=0 ; break;}
+
- if(buffer[0]=='X'){ //Vuelve a darle dominio al encoder.
- break;
- }
-
- cleanBuffer(buffer,6);
-
- } //Cierra el pc.readable()
-
-
-}//Cierre while(1)
+ }//Cierre while(1)
+ } // cierra function LecturaSerial
+
+//==================================== itoa function ==========================================
-/*********
-// 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(1);
- lcd.cls();
- lcd.printf(" INICIA EL PID");
- wait(1);
-// se imprimen los parches del control *****************************************
- lcd.cls();
- lcd.printf("Er=%3.0f",err);
- lcd.locate(8,0);
- lcd.printf("Me=%3.0f",med);
- lcd.locate(0,1);
- lcd.printf("Sp=%3.0f",spnum);
- lcd.locate(8,1);
- lcd.printf("Co=%3.0f",pid);
- wait(1);
-
-// CICLO PRINCIPAL CONTROLADOR PID
- lop2: 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;
- }
-
- // se verifica que pid sea menor o igual la valor maximo *****************
- if (pid > 999)
- {
- pid=999;
- }
+/*** C++ version 0.4 char* style "itoa": * Written by Lukás Chmela * Released under GPLv3. */
+
+char* itoa(int value, char* result, int base)
+{
+ // check that the base if valid
+ if ( base < 2 || base > 36 ) {
+ *result = '\0';
+ return result; }
+
+ char* ptr = result, *ptr1 = result, tmp_char;
+ int tmp_value;
+
+ do {
+ tmp_value = value;
+ value /= base;
+ *ptr++ = "zyxwvutsrqponmlkjihgfedcba9876543210123456789abcdefghijklmnopqrstuvwxyz"[35 + (tmp_value - value * base)];
+ } while ( value );
+
+// Apply negative sign
+ if ( tmp_value < 0 )
+ *ptr++ = '-';
+ *ptr-- = '\0';
+
+ while ( ptr1 < ptr ) {
+ tmp_char = *ptr;
+ *ptr-- = *ptr1;
+ *ptr1++ = tmp_char; }
+
+ return result;
+}
+//convertir float a hex
+//=========================================================Float to hex (string)
+char* ftoa(float in,char* out)
+
+{
+ int a, b;
+ char inte[6];
+ char flot[6];
+
+ a=in*1;
+ b=(in-a)*10;
+
+ itoa(a, inte,16);
+ itoa(b, flot,16);
+
+ strcpy (out,inte);
+ strcat (out,"2e"); // . ---> 2e ASCII
+ strcat (out,flot);
+
+ return out;
+ }
+
+
+//============================================================================ MAIN ===========================
+int main(void) {
- //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);
-
-
-
-
- //Normalizacion de la salida
- // se actualizan las 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
- wait_ms(300);
- goto lop2;
- ********/
- }
-
-int main(void) //**********************************MAIN***************************************************
- {
pc.baud(9600);
pc.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
-
- lcd.locate(8,0);
- lcd.printf("Kp=%d",kpnum);
+
lcd.locate(0,1);
- lcd.printf("Ki=%d",kinum);
- lcd.locate(8,1);
- lcd.printf("Kd=%d",kdnum);
- lcd.locate(0,0);
- lcd.printf("Sp=%d",spnum);
+ lcd.printf("**Control PID**"); wait(2);
+ lcd.cls(); lcd.writeCommand(C1);
+
+ lcd.locate(8,0); lcd.printf("Kp=%d",kpnum);
+ lcd.locate(0,1); lcd.printf("Ki=%d",kinum);
+ lcd.locate(8,1); lcd.printf("Kd=%d",kdnum);
+ lcd.locate(0,0); lcd.printf("Sp=%d",spnum);
- while(1)
- {
+//============================================================================= Lectura del Celular
+ while(1){
+
+ ftoa(509.7,mm) ;
+ pc.printf("SP%s", mm);
+
+
+ if(pc.readable())
+ { bloqueo='b'; LecturaSerial();
+ if (buffer[0]=='G') { break;}}
+
+ else if(buffer[0]=='S'||buffer[0]=='K'){
+ LecturaSerial();}
+
+ else {bloqueo='d';}
+
+
+ if(!button4){break; } //boton del encoder // wait(0.1);
diferencia=encoder.getPulses()-cambio;
cambio=encoder.getPulses();
- if (diferencia==0)
- {
- //nada
- }
+ if (diferencia==0) { } //nada
+
else if(diferencia>0)
- {
- if(pos==1)
- {
- if(spnum+diferencia>=999)
- {
- spnum=999;
+ { if(pos==1)
+ { if(spnum+diferencia>=999)
+ { spnum=999;
lcd.locate(3,0);
lcd.printf(" ");
lcd.locate(3,0);
- lcd.printf("%d", spnum);
- }
- else
- {
- spnum+=diferencia;
+ lcd.printf("%d", spnum); }
+ else {
+ spnum+=diferencia;
lcd.locate(3,0);
- lcd.printf("%d", spnum);
- }
- }
+ lcd.printf("%d", spnum); }
+ //itoa(spnum,mm,16) ;
+ //pc.printf("SP%s", mm);
+ wait(0.3); } // Adicionado, envio de los parametros al celular
+ // convierto numero a caracter y adicional el comodin inicial
+
else if(pos==2)
- {
- if(kpnum+diferencia>=999)
- {
- kpnum=999;
- 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);
- }
- }
+ { if(kpnum+diferencia>=999)
+ { kpnum=999;
+ 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); }
+ // itoa(kpnum,mm,16) ;
+ // pc.printf("KP%s", mm);
+ wait(0.3); } // https://www.tutorialspoint.com//perl/perl_sprintf.htm
+ // http://www.cplusplus.com/reference/cstdlib/itoa/
+
else if(pos==3)
- {
- if(kinum+diferencia>=999)
- {
- kinum=999;
- lcd.locate(3,1);
- lcd.printf(" ");
- lcd.locate(3,1);
- lcd.printf("%d", kinum);
- }
+ { if(kinum+diferencia>=999)
+ { kinum=999;
+ 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);
- }
- }
+ { kinum+=diferencia;
+ lcd.locate(3,1);
+ lcd.printf("%d", kinum); }
+ //itoa(kinum,mm,16) ;
+ //pc.printf("KI%s", mm);
+ wait(0.3); }
+
else if(pos==4)
- {
- if(kdnum+diferencia>=999)
- {
- kdnum=999;
- lcd.locate(11,1);
- lcd.printf(" ");
- lcd.locate(11,1);
- lcd.printf("%d", kdnum);
- }
+ { if(kdnum+diferencia>=999)
+ { kdnum=999;
+ 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);
- }
- }
- }
+ { kdnum+=diferencia;
+ lcd.locate(11,1); lcd.printf("%d", kdnum); }
+ //itoa(kdnum,mm,16) ;
+ //pc.printf("KD%s", mm);
+ wait(0.3); }
+ }
else if(diferencia<0)
{
if(pos==1)
- {
- if(spnum+diferencia<0)
- {
- //No ocurre nada
- }
- else
- {
- spnum+=diferencia;
- lcd.locate(3,0);
- lcd.printf(" ");
- lcd.locate(3,0);
- lcd.printf("%d", spnum);
- }
- }
- else if(pos==2)
- {
- if(kpnum+diferencia<0)
- {
- //No ocurre nada
+ { if(spnum+diferencia<0) { //No ocurre nada
}
else
- {
- kpnum+=diferencia;
- lcd.locate(11,0);
- lcd.printf(" ");
- lcd.locate(11,0);
- lcd.printf("%d", kpnum);
- }
- }
- else if(pos==3)
- {
- if(kinum+diferencia<0)
- {
- //No ocurre nada
+ { spnum+=diferencia;
+ lcd.locate(3,0); lcd.printf(" ");
+ lcd.locate(3,0); lcd.printf("%d", spnum); }
+ itoa(spnum,mm,16) ;
+ pc.printf("SP%s", mm);
+ wait(0.3); }
+
+ else if(pos==2)
+ { if(kpnum+diferencia<0) { //No ocurre nada
}
else
- {
- kinum+=diferencia;
- lcd.locate(3,1);
- lcd.printf(" ");
- lcd.locate(3,1);
- lcd.printf("%d", kinum);
- }
- }
- else if(pos==4)
- {
- if(kdnum+diferencia<0)
- {
- //No ocurre nada
+ { kpnum+=diferencia;
+ lcd.locate(11,0); lcd.printf(" ");
+ lcd.locate(11,0); lcd.printf("%d", kpnum); }
+ //itoa(kpnum,mm,16) ;
+ //pc.printf("KP%s", mm);
+ wait(0.3); }
+
+ else if(pos==3)
+ { if(kinum+diferencia<0) { //No ocurre nada
}
else
- {
- kdnum+=diferencia;
- lcd.locate(11,1);
- lcd.printf(" ");
- lcd.locate(11,1);
- lcd.printf("%d", kdnum);
+ { kinum+=diferencia;
+ lcd.locate(3,1); lcd.printf(" ");
+ lcd.locate(3,1); lcd.printf("%d", kinum); }
+ //itoa(kinum,mm,16) ;
+ //pc.printf("KI%s", mm);
+ wait(0.3); }
+
+ else if(pos==4)
+ { if(kdnum+diferencia<0)
+ { //No ocurre nada
}
- }
- }
+
+ else
+ { kdnum+=diferencia;
+ lcd.locate(11,1); lcd.printf(" ");
+ lcd.locate(11,1); lcd.printf("%d", kdnum); }
+ //itoa(kdnum,mm,16) ;
+ //pc.printf("KD%s", mm);
+ wait(0.3); }
+ } // cierra el if de cambio en elencoder
- 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 (!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);
+ }
+ } // cierra el while 1
+
- if (!button4)
- {
- break; //sale del bucle si pisan suiche4
- }
- wait(0.1);
-
- if(pc.readable()){ //Si hay algo para leer entra al bucle de lectura serial y da todo el dominio al serial.
- readBuffer(buffer,6);
- if(buffer[0]=='G'){
- break; //sale del bucle si le dan al botón guardar del celular.
- }
- else{
- LecturaSerial();
- readBuffer(buffer,6);
- wait(0.2);
- }
- }
-
- }
-
-//Transicion
- 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(1);
- lcd.cls();
- lcd.printf(" INICIA EL PID");
- wait(1);
+//==============================================================================Transición hacia el PID
+ lcd.writeCommand(C4); //quitar cursor
+ lcd.cls(); lcd.printf(" GUARDADOS!"); wait(1);
+ lcd.cls(); lcd.printf(" INICIA EL PID"); wait(1);
+
// se imprimen los parches del control *****************************************
- lcd.cls();
- lcd.printf("Er=%3.0f",err);
- lcd.locate(8,0);
- lcd.printf("Me=%3.0f",med);
- lcd.locate(0,1);
- lcd.printf("Sp=%3.0f",spnum);
- lcd.locate(8,1);
- lcd.printf("Co=%3.0f",pid);
+ lcd.cls(); lcd.printf("Er=%3.0f",err);
+ lcd.locate(8,0); lcd.printf("Me=%3.0f",med);
+ lcd.locate(0,1); lcd.printf("Sp=%3.0f",spnum);
+ lcd.locate(8,1); lcd.printf("Co=%3.0f",pid);
wait(1);
// CICLO PRINCIPAL CONTROLADOR PID
@@ -450,54 +362,41 @@
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;
- }
+
+ if(pid<=0) { // se verifica que pid sea positivo **************************************
+ pid=0; }
- // se verifica que pid sea menor o igual la valor maximo *****************
- if (pid > 999)
- {
- pid=999;
- }
+
+ if (pid > 999) { // se verifica que pid sea menor o igual la valor maximo *****************
+ 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 Y ENVIAN las variables******************************************
+ lcd.locate(3,0); lcd.printf(" ");
+ lcd.locate(3,0); lcd.printf("%3.0f",err);
+ ftoa(err,mm) ; pc.printf("ER%s", mm);
+ wait(0.3);
-
-
+ lcd.locate(11,0); lcd.printf(" ");
+ lcd.locate(11,0); lcd.printf("%3.0f",med);
+ ftoa(med,mm) ; pc.printf("ME%s", mm);
+ wait(0.3);
+
+ 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);
+ ftoa(pid,mm) ; pc.printf("ID%s", mm);
+ wait(0.3);
+
//Normalizacion de la salida
// se actualizan las variables *******************************************
err_v = err;
o = pid/999;
u.write(o);
// se envia el valor pid a puerto analogico de salida (D/A) **************
+ wait_ms(300);
- pc.printf("%3.0f\t",err);
- pc.printf("%3.0f\t",med);
- pc.printf("%3.0f\t",spnum);
- pc.printf("%3.0f\n",pid);
-
- // se repite el ciclo
- wait_ms(300);
- goto lop1;
+ goto lop1; // se repite el ciclo
}//Cierre main
\ No newline at end of file