Aldaiir Navarro G
Tarea-1-Procesadores-2016-2
Dependencies: QEI TextLCD mbed
Fork of
PID
by 
Team La Verga
Embed:
(wiki syntax)
Show/hide line numbers
main.cpp
00001 #include "mbed.h" 00002 #include "QEI.h" 00003 #include "TextLCD.h" 00004 #include "stdio.h" 00005 #include "string.h" 00006 #include "stdlib.h" 00007 #include "math.h" 00008 00009 // Serial pc(USBTX,USBRX); //puertos del PC 00010 Serial pc(PTE0,PTE1); 00011 TextLCD lcd(PTB10, PTB11, PTE2, PTE3, PTE4, PTE5); // rs, e, d4-d7 00012 QEI encoder (PTA13, PTD5, NC, 624); 00013 AnalogIn y(PTB3);//entrada analoga 00014 AnalogOut u(PTE30);//salida analoga OJO solo se le pueden drenar 1.5mA en circuitos use un Buffer 00015 //si se ignora esto se arruina la FRDMKL25Z 00016 //DigitalOut led1(LED1);DigitalOut led2(LED2); DigitalOut led3(LED3); 00017 00018 DigitalIn button3(PTC16);//cambia ingreso de los 4 parametros 00019 DigitalIn button4(PTC17);//termina y consolida valores de 4 parametros y sale del loop 00020 00021 //int C1=0x0E; // solo muestra el curzor 00022 int C2=0x18; // desplaza izquierda 00023 int C3=0x1A; // desplaza derecha 00024 int C4=0x0C; // quito cursor bajo 00025 00026 int C1=0x0F; 00027 char buffer[6];// TAMAÑO DEL BUFER 00028 char cadena[3], mm[6]; 00029 Timer t; //VALOR DEL TIEMPO 00030 int count; 00031 int i = 0; 00032 int c=0; 00033 int cambio=0, diferencia=0; 00034 float pid,o,ai,ad,ap,med,err; 00035 float err_v; 00036 int spnum=0,kinum=0,kpnum=0,kdnum=0,num=0,pos=1; 00037 char bloqueo='d'; 00038 00039 00040 //============================================================================ Function para limpiar Buffer ================= 00041 00042 int readBuffer(char *buffer,int count) 00043 { 00044 int i=0; 00045 t.start(); //CUENTA EL TIEMPO DE CONEXION E INICIA 00046 while(1) { 00047 while (pc.readable()) { 00048 char c = pc.getc(); 00049 //if (c == '\r' || c == '\n') c = '$';//si se envia fin de linea o de caracxter inserta $ 00050 buffer[i++] = c;//mete al bufer el caracter leido 00051 if(i > count)break;//sale del loop si ya detecto terminacion 00052 } 00053 if(i > count)break; 00054 if(t.read() > 1) { //MAS DE UN SEGUNDO DE ESPERA SE SALE Y REINICA EL RELOJ Y SE SALE 00055 00056 // ======== Analizar aca, tiempo ========================== // 00057 t.stop(); 00058 t.reset(); 00059 break; 00060 } 00061 } 00062 return 0; } 00063 00064 //============================================================================ Function para limpiar Buffer ============================== 00065 void cleanBuffer(char *buffer, int count) 00066 { 00067 for(int i=0; i < count; i++) { 00068 buffer[i] = '\0'; } } 00069 00070 00071 //============================================================================ Function Leer Parámetros del Celular =========================== 00072 void LecturaSerial(void){ 00073 while(bloqueo=='b'){ 00074 00075 cleanBuffer(buffer,6); 00076 readBuffer(buffer,6); 00077 00078 00079 if (buffer[0]=='G'||buffer[0]=='d') { break;} 00080 00081 cadena[0]=buffer[2]; 00082 cadena[1]=buffer[3]; 00083 cadena[2]=buffer[4]; 00084 num=strtod(cadena,NULL); 00085 00086 if(num>999) {num=999; } if(num<0) {num=0; } 00087 00088 if (buffer[0]=='S' && buffer[1]=='P') 00089 { spnum=num; 00090 lcd.locate(3,0); lcd.printf(" "); 00091 lcd.locate(3,0); lcd.printf("%d", spnum); 00092 pos=1; diferencia=0 ; break; } 00093 00094 if (buffer[0]=='K' && buffer[1]=='P') 00095 { kpnum=num; 00096 lcd.locate(11,0); lcd.printf(" "); 00097 lcd.locate(11,0); lcd.printf("%d", kpnum); 00098 pos=2; diferencia=0 ; break;} 00099 00100 if (buffer[0]=='K' && buffer[1]=='I') 00101 { kinum=num; 00102 lcd.locate(3,1); lcd.printf(" "); 00103 lcd.locate(3,1); lcd.printf("%d", kinum); 00104 pos=3; diferencia=0 ; break ;} 00105 00106 if (buffer[0]=='K' && buffer[1]=='D') 00107 { kdnum=num; 00108 lcd.locate(11,1); lcd.printf(" "); 00109 lcd.locate(11,1); lcd.printf("%d", kdnum); 00110 pos=4; diferencia=0 ; break;} 00111 00112 00113 }//Cierre while(1) 00114 } // cierra function LecturaSerial 00115 00116 //==================================== itoa function ========================================== 00117 00118 /*** C++ version 0.4 char* style "itoa": * Written by Lukás Chmela * Released under GPLv3. */ 00119 00120 char* itoa(int value, char* result, int base) 00121 { 00122 // check that the base if valid 00123 if ( base < 2 || base > 36 ) { 00124 *result = '\0'; 00125 return result; } 00126 00127 char* ptr = result, *ptr1 = result, tmp_char; 00128 int tmp_value; 00129 00130 do { 00131 tmp_value = value; 00132 value /= base; 00133 *ptr++ = "zyxwvutsrqponmlkjihgfedcba9876543210123456789abcdefghijklmnopqrstuvwxyz"[35 + (tmp_value - value * base)]; 00134 } while ( value ); 00135 00136 // Apply negative sign 00137 if ( tmp_value < 0 ) 00138 *ptr++ = '-'; 00139 *ptr-- = '\0'; 00140 00141 while ( ptr1 < ptr ) { 00142 tmp_char = *ptr; 00143 *ptr-- = *ptr1; 00144 *ptr1++ = tmp_char; } 00145 00146 return result; 00147 } 00148 00149 //convertir float a hex 00150 //=========================================================Float to hex (string) 00151 char* ftoa(float in,char* out) 00152 00153 { 00154 int a, b; 00155 char inte[6]; 00156 char flot[6]; 00157 00158 a=in*1; 00159 b=(in-a)*10; 00160 00161 itoa(a, inte,16); 00162 itoa(b, flot,16); 00163 00164 strcpy (out,inte); 00165 strcat (out,"2e"); // . ---> 2e ASCII 00166 strcat (out,flot); 00167 00168 return out; 00169 } 00170 00171 00172 //============================================================================ MAIN =========================== 00173 int main(void) { 00174 00175 pc.baud(9600); 00176 pc.format(8,Serial::None,1); 00177 00178 lcd.locate(0,1); 00179 lcd.printf("**Control PID**"); wait(2); 00180 lcd.cls(); lcd.writeCommand(C1); 00181 00182 lcd.locate(8,0); lcd.printf("Kp=%d",kpnum); 00183 lcd.locate(0,1); lcd.printf("Ki=%d",kinum); 00184 lcd.locate(8,1); lcd.printf("Kd=%d",kdnum); 00185 lcd.locate(0,0); lcd.printf("Sp=%d",spnum); 00186 00187 //============================================================================= Lectura del Celular 00188 while(1){ 00189 00190 ftoa(509.7,mm) ; 00191 pc.printf("SP%s", mm); 00192 00193 00194 if(pc.readable()) 00195 { bloqueo='b'; LecturaSerial(); 00196 if (buffer[0]=='G') { break;}} 00197 00198 else if(buffer[0]=='S'||buffer[0]=='K'){ 00199 LecturaSerial();} 00200 00201 else {bloqueo='d';} 00202 00203 00204 if(!button4){break; } //boton del encoder // wait(0.1); 00205 00206 diferencia=encoder.getPulses()-cambio; 00207 cambio=encoder.getPulses(); 00208 00209 if (diferencia==0) { } //nada 00210 00211 else if(diferencia>0) 00212 { if(pos==1) 00213 { if(spnum+diferencia>=999) 00214 { spnum=999; 00215 lcd.locate(3,0); 00216 lcd.printf(" "); 00217 lcd.locate(3,0); 00218 lcd.printf("%d", spnum); } 00219 else { 00220 spnum+=diferencia; 00221 lcd.locate(3,0); 00222 lcd.printf("%d", spnum); } 00223 //itoa(spnum,mm,16) ; 00224 //pc.printf("SP%s", mm); 00225 wait(0.3); } // Adicionado, envio de los parametros al celular 00226 // convierto numero a caracter y adicional el comodin inicial 00227 00228 else if(pos==2) 00229 { if(kpnum+diferencia>=999) 00230 { kpnum=999; 00231 lcd.locate(11,0); 00232 lcd.printf(" "); 00233 lcd.locate(11,0); 00234 lcd.printf("%d", kpnum); } 00235 else { 00236 kpnum+=diferencia; 00237 lcd.locate(11,0); 00238 lcd.printf("%d", kpnum); } 00239 // itoa(kpnum,mm,16) ; 00240 // pc.printf("KP%s", mm); 00241 wait(0.3); } // https://www.tutorialspoint.com//perl/perl_sprintf.htm 00242 // http://www.cplusplus.com/reference/cstdlib/itoa/ 00243 00244 else if(pos==3) 00245 { if(kinum+diferencia>=999) 00246 { kinum=999; 00247 lcd.locate(3,1); 00248 lcd.printf(" "); 00249 lcd.locate(3,1); 00250 lcd.printf("%d", kinum); } 00251 else 00252 { kinum+=diferencia; 00253 lcd.locate(3,1); 00254 lcd.printf("%d", kinum); } 00255 //itoa(kinum,mm,16) ; 00256 //pc.printf("KI%s", mm); 00257 wait(0.3); } 00258 00259 else if(pos==4) 00260 { if(kdnum+diferencia>=999) 00261 { kdnum=999; 00262 lcd.locate(11,1); lcd.printf(" "); 00263 lcd.locate(11,1); lcd.printf("%d", kdnum); } 00264 else 00265 { kdnum+=diferencia; 00266 lcd.locate(11,1); lcd.printf("%d", kdnum); } 00267 //itoa(kdnum,mm,16) ; 00268 //pc.printf("KD%s", mm); 00269 wait(0.3); } 00270 } 00271 00272 else if(diferencia<0) 00273 { 00274 if(pos==1) 00275 { if(spnum+diferencia<0) { //No ocurre nada 00276 } 00277 else 00278 { spnum+=diferencia; 00279 lcd.locate(3,0); lcd.printf(" "); 00280 lcd.locate(3,0); lcd.printf("%d", spnum); } 00281 itoa(spnum,mm,16) ; 00282 pc.printf("SP%s", mm); 00283 wait(0.3); } 00284 00285 else if(pos==2) 00286 { if(kpnum+diferencia<0) { //No ocurre nada 00287 } 00288 else 00289 { kpnum+=diferencia; 00290 lcd.locate(11,0); lcd.printf(" "); 00291 lcd.locate(11,0); lcd.printf("%d", kpnum); } 00292 //itoa(kpnum,mm,16) ; 00293 //pc.printf("KP%s", mm); 00294 wait(0.3); } 00295 00296 else if(pos==3) 00297 { if(kinum+diferencia<0) { //No ocurre nada 00298 } 00299 else 00300 { kinum+=diferencia; 00301 lcd.locate(3,1); lcd.printf(" "); 00302 lcd.locate(3,1); lcd.printf("%d", kinum); } 00303 //itoa(kinum,mm,16) ; 00304 //pc.printf("KI%s", mm); 00305 wait(0.3); } 00306 00307 else if(pos==4) 00308 { if(kdnum+diferencia<0) 00309 { //No ocurre nada 00310 } 00311 00312 else 00313 { kdnum+=diferencia; 00314 lcd.locate(11,1); lcd.printf(" "); 00315 lcd.locate(11,1); lcd.printf("%d", kdnum); } 00316 //itoa(kdnum,mm,16) ; 00317 //pc.printf("KD%s", mm); 00318 wait(0.3); } 00319 } // cierra el if de cambio en elencoder 00320 00321 00322 //=============================================================================== 00323 if (!button3) //cambia la posicion de ingreso de parametros 00324 { //led3 =!led3; 00325 if(pos==4) 00326 { pos=1; 00327 lcd.locate(3,0); lcd.printf("%d", spnum); } 00328 00329 else if (pos==1) 00330 { pos++; 00331 lcd.locate(11,0); lcd.printf("%d", kpnum); } 00332 00333 else if(pos==2) 00334 { pos++; 00335 lcd.locate(3,1); lcd.printf("%d", kinum); } 00336 00337 else if(pos==3) 00338 { pos++; 00339 lcd.locate(11,1); lcd.printf("%d", kdnum); } 00340 00341 wait(0.25); 00342 } 00343 } // cierra el while 1 00344 00345 00346 //==============================================================================Transición hacia el PID 00347 lcd.writeCommand(C4); //quitar cursor 00348 lcd.cls(); lcd.printf(" GUARDADOS!"); wait(1); 00349 lcd.cls(); lcd.printf(" INICIA EL PID"); wait(1); 00350 00351 // se imprimen los parches del control ***************************************** 00352 lcd.cls(); lcd.printf("Er=%3.0f",err); 00353 lcd.locate(8,0); lcd.printf("Me=%3.0f",med); 00354 lcd.locate(0,1); lcd.printf("Sp=%3.0f",spnum); 00355 lcd.locate(8,1); lcd.printf("Co=%3.0f",pid); 00356 wait(1); 00357 00358 // CICLO PRINCIPAL CONTROLADOR PID 00359 lop1: med = y.read()*999; 00360 err = (spnum-med); //se calcula el error 00361 ap = kpnum*err*0.01f; //se calcula la accion proporcinal 00362 ai =(kinum*err*0.01f)+ai; //calculo de la integral del error 00363 ad = kdnum*(err-err_v)*0.01f; //calculo de la accion derivativa 00364 pid = (ap+ai+ad); 00365 00366 if(pid<=0) { // se verifica que pid sea positivo ************************************** 00367 pid=0; } 00368 00369 00370 if (pid > 999) { // se verifica que pid sea menor o igual la valor maximo ***************** 00371 pid=999; } 00372 00373 //se muestran Y ENVIAN las variables****************************************** 00374 lcd.locate(3,0); lcd.printf(" "); 00375 lcd.locate(3,0); lcd.printf("%3.0f",err); 00376 ftoa(err,mm) ; pc.printf("ER%s", mm); 00377 wait(0.3); 00378 00379 lcd.locate(11,0); lcd.printf(" "); 00380 lcd.locate(11,0); lcd.printf("%3.0f",med); 00381 ftoa(med,mm) ; pc.printf("ME%s", mm); 00382 wait(0.3); 00383 00384 lcd.locate(3,1); lcd.printf(" "); 00385 lcd.locate(3,1); lcd.printf("%d",spnum); 00386 00387 lcd.locate(11,1); lcd.printf(" "); 00388 lcd.locate(11,1); lcd.printf("%3.0f",pid); 00389 ftoa(pid,mm) ; pc.printf("ID%s", mm); 00390 wait(0.3); 00391 00392 //Normalizacion de la salida 00393 // se actualizan las variables ******************************************* 00394 err_v = err; 00395 o = pid/999; 00396 u.write(o); 00397 // se envia el valor pid a puerto analogico de salida (D/A) ************** 00398 wait_ms(300); 00399 00400 goto lop1; // se repite el ciclo 00401 00402 }//Cierre main
Generated on Thu Jul 14 2022 13:22:37 by
1.7.2