pid control android
Dependencies: QEI TextLCD mbed
main.cpp
00001 #include "mbed.h" 00002 #include <stdio.h> 00003 #include "QEI.h" 00004 #include "TextLCD.h" 00005 #include "string.h" 00006 00007 00008 TextLCD lcd(PTB10, PTB11, PTE2, PTE3, PTE4, PTE5); // rs, e, d4-d7 00009 QEI encoder (PTA13, PTD5, NC, 624); 00010 AnalogIn y(PTB3);//entrada analoga 00011 AnalogOut u(PTE30);//salida analoga OJO solo se le pueden drenar 1.5mA en circuitos use un Buffer 00012 00013 Serial usart(PTE0,PTE1); //puertos del FRDM para el modem 00014 Serial pc(USBTX,USBRX); //puertos del PC 00015 00016 DigitalOut led1(LED1); 00017 DigitalOut led2(LED2); 00018 DigitalOut led3(LED3); 00019 00020 DigitalIn button3(PTC16);//cambia ingreso de los 4 parametros 00021 DigitalIn button4(PTC17);//termina y consolida valores de 4 parametros y sale del loop 00022 00023 //codigos movimiento del curzor 00024 00025 //int C1=0x0E; // solo muestra el curzor 00026 int C2=0x18; // desplaza izquierda 00027 int C3=0x1A; // desplaza derecha 00028 int C4=0x0C; // quito cursor bajo 00029 00030 int C1=0x0F; 00031 float pid,o,ai,ad,ap,med,err; 00032 float err_v,med_v; 00033 int sp=0,ki=0,kp=0,kd=0; 00034 int j,k; 00035 char buffer[4];// TAMAÑO DEL BUFER 00036 Timer t; //VALOR DEL TIEMPO 00037 int count; 00038 int contador; 00039 int i = 0; 00040 int c=0; 00041 char cc; 00042 char caux[]="0000"; 00043 int readBuffer(char *buffer,int count) //esta funcion lee un bufer de datos 00044 { 00045 int ij=0; 00046 t.start(); //CUENTA EL TIEMPO DE CONEXION E INICIA 00047 while(1) { 00048 while (usart.readable()) { 00049 //char 00050 cc = usart.getc(); 00051 if (cc == '\r' || cc == '\n') cc = '$';//si se envia fin de linea o de caracxter inserta $ 00052 buffer[ij++] = cc;//mete al bufer el caracter leido 00053 if(ij > count)break;//sale del loop si ya detecto terminacion 00054 } 00055 if(ij > count)break; 00056 if(t.read() > 1) { //MAS DE UN SEGUNDO DE ESPERA SE SALE Y REINICA EL RELOJ Y SE SALE 00057 t.stop(); 00058 t.reset(); 00059 break; 00060 } 00061 } 00062 return 0; 00063 } 00064 00065 void cleanBuffer(char *buffer, int count) //esta funcion limpia el bufer 00066 { 00067 for(int i=0; i < count; i++) { 00068 buffer[i] = '\0'; 00069 } 00070 } 00071 void encoderdatos() 00072 { 00073 00074 int pos=1; 00075 int cambio=0, diferencia=0; 00076 00077 while(1) 00078 { 00079 00080 diferencia=encoder.getPulses()-cambio; 00081 cambio=encoder.getPulses(); 00082 if (diferencia==0) 00083 { 00084 //nada 00085 } 00086 else if(diferencia>0) 00087 { 00088 if(pos==1) 00089 { 00090 00091 if(sp+diferencia>=999) 00092 { 00093 sp=999; 00094 lcd.locate(3,0); 00095 lcd.printf(" "); 00096 lcd.locate(3,0); 00097 lcd.printf("%d", sp); 00098 } 00099 else 00100 { 00101 sp+=diferencia; 00102 lcd.locate(3,0); 00103 lcd.printf("%d", sp); 00104 00105 00106 } 00107 } 00108 else if(pos==2) 00109 { 00110 if(kp+diferencia>=999) 00111 { 00112 kp=999; 00113 lcd.locate(11,0); 00114 lcd.printf(" "); 00115 lcd.locate(11,0); 00116 lcd.printf("%d", kp); 00117 } 00118 else 00119 { 00120 kp+=diferencia; 00121 lcd.locate(11,0); 00122 lcd.printf("%d", kp); 00123 } 00124 } 00125 else if(pos==3) 00126 { 00127 if(ki+diferencia>=999) 00128 { 00129 ki=999; 00130 lcd.locate(3,1); 00131 lcd.printf(" "); 00132 lcd.locate(3,1); 00133 lcd.printf("%d", ki); 00134 } 00135 else 00136 { 00137 ki+=diferencia; 00138 lcd.locate(3,1); 00139 lcd.printf("%d", ki); 00140 } 00141 } 00142 else if(pos==4) 00143 { 00144 if(kd+diferencia>=999) 00145 { 00146 kd=999; 00147 lcd.locate(11,1); 00148 lcd.printf(" "); 00149 lcd.locate(11,1); 00150 lcd.printf("%d", kd); 00151 } 00152 else 00153 { 00154 kd+=diferencia; 00155 lcd.locate(11,1); 00156 lcd.printf("%d", kd); 00157 } 00158 } 00159 } 00160 else if(diferencia<0) 00161 { 00162 if(pos==1) 00163 { 00164 if(sp+diferencia<0) 00165 { 00166 //No ocurre nada 00167 } 00168 else 00169 { 00170 sp+=diferencia; 00171 lcd.locate(3,0); 00172 lcd.printf(" "); 00173 lcd.locate(3,0); 00174 lcd.printf("%d", sp); 00175 } 00176 } 00177 else if(pos==2) 00178 { 00179 if(kp+diferencia<0) 00180 { 00181 //No ocurre nada 00182 } 00183 else 00184 { 00185 kp+=diferencia; 00186 lcd.locate(11,0); 00187 lcd.printf(" "); 00188 lcd.locate(11,0); 00189 lcd.printf("%d", kp); 00190 } 00191 } 00192 else if(pos==3) 00193 { 00194 if(ki+diferencia<0) 00195 { 00196 //No ocurre nada 00197 } 00198 else 00199 { 00200 ki+=diferencia; 00201 lcd.locate(3,1); 00202 lcd.printf(" "); 00203 lcd.locate(3,1); 00204 lcd.printf("%d", ki); 00205 } 00206 } 00207 else if(pos==4) 00208 { 00209 if(kd+diferencia<0) 00210 { 00211 //No ocurre nada 00212 } 00213 else 00214 { 00215 kd+=diferencia; 00216 lcd.locate(11,1); 00217 lcd.printf(" "); 00218 lcd.locate(11,1); 00219 lcd.printf("%d", kd); 00220 } 00221 } 00222 } 00223 if (!button3) //cambia la posicion de ingreso de parametros 00224 { 00225 led3 =!led3; 00226 if(pos==4) 00227 { 00228 pos=1; 00229 lcd.locate(3,0); 00230 lcd.printf("%d", sp); 00231 } 00232 else if (pos==1) 00233 { 00234 pos++; 00235 lcd.locate(11,0); 00236 lcd.printf("%d", kp); 00237 } 00238 else if(pos==2) 00239 { 00240 pos++; 00241 lcd.locate(3,1); 00242 lcd.printf("%d", ki); 00243 } 00244 else if(pos==3) 00245 { 00246 pos++; 00247 lcd.locate(11,1); 00248 lcd.printf("%d", kd); 00249 } 00250 wait(0.25); 00251 00252 } 00253 00254 00255 if (!button4) 00256 { 00257 break; //sale del bucle si pisan suiche4 00258 } 00259 00260 wait(0.01); 00261 } 00262 //return spnum,kinum,kpnum,kdnum; 00263 00264 } 00265 00266 int main() 00267 { 00268 00269 00270 lcd.locate(0,1); 00271 lcd.printf("**Control PID**"); 00272 wait(2); 00273 lcd.cls(); // Borrar Pantalla 00274 lcd.writeCommand(C1);//escribimos un comando segun el manual del modulo LCD 00275 00276 lcd.locate(8,0); 00277 lcd.printf("Kp=%d",kp); 00278 lcd.locate(0,1); 00279 lcd.printf("Ki=%d",ki); 00280 lcd.locate(8,1); 00281 lcd.printf("Kd=%d",kd); 00282 lcd.locate(0,0); 00283 lcd.printf("Sp=%d",sp); 00284 wait(0.2); 00285 encoderdatos(); 00286 wait(0.1); 00287 00288 usart.baud(9600); // asigno baudios y configuro puerto serie de la usart 00289 usart.format(8,Serial::None,1); 00290 // 00291 00292 00293 // 00294 //sp=valores[0]; 00295 00296 wait(0.001); 00297 if(sp<256) 00298 { 00299 usart.putc(sp); 00300 usart.putc(0); 00301 00302 } 00303 if(sp>255) 00304 { 00305 j=sp/256; 00306 k=sp-j*256; 00307 usart.putc(k); 00308 usart.putc(j); 00309 00310 } 00311 wait(0.001); 00312 if(kp<256) 00313 { 00314 usart.putc(kp); 00315 usart.putc(0); 00316 00317 } 00318 if(kp>255) 00319 { 00320 j=kp/256; 00321 k=kp-j*256; 00322 usart.putc(k); 00323 usart.putc(j); 00324 00325 } 00326 wait(0.001); 00327 if(ki<256) 00328 { 00329 00330 usart.putc(ki); 00331 usart.putc(0); 00332 00333 } 00334 if(ki>255) 00335 { 00336 j=ki/256; 00337 k=ki-j*256; 00338 usart.putc(k); 00339 usart.putc(j); 00340 00341 } 00342 wait(0.001); 00343 if(kd<256) 00344 { 00345 usart.putc(kd); 00346 usart.putc(0); 00347 00348 } 00349 if(sp>255) 00350 { 00351 j=kd/256; 00352 k=kd-j*256; 00353 usart.putc(k); 00354 usart.putc(j); 00355 00356 } 00357 00358 //Transicion 00359 lcd.writeCommand(C4);//escribimos un comando segun el manual del modulo LCD para quitar cursor bajo 00360 lcd.cls(); //borra la pantalla 00361 lcd.printf(" GUARDADOS!"); 00362 wait(1); 00363 lcd.cls(); 00364 lcd.printf(" INICIA EL PID"); 00365 wait(1); 00366 // se imprimen los parches del control ***************************************** 00367 lcd.cls(); 00368 lcd.printf("Er=%3.0f",err); 00369 lcd.locate(8,0); 00370 lcd.printf("Me=%3.0f",med); 00371 lcd.locate(0,1); 00372 lcd.printf("Sp=%3.0f",sp); 00373 lcd.locate(8,1); 00374 lcd.printf("Co=%3.0f",pid); 00375 wait(1); 00376 00377 //ciclo control 00378 00379 while(1) 00380 { 00381 // 00382 if (usart.readable()) 00383 { 00384 cleanBuffer(buffer,4); 00385 readBuffer(buffer,4); 00386 pc.printf("buffer= %s\n\r ",buffer); //imprime el bufer 00387 00388 if ('h' == buffer[0]) 00389 { 00390 lcd.cls(); 00391 lcd.locate(3,1); 00392 lcd.printf("hola: %s",buffer); 00393 wait(1); 00394 lcd.cls(); 00395 //cleanBuffer(buffer,4); 00396 //sp=88; 00397 for(int ii=1; ii < 4; ii++){ 00398 caux[ii-1]=buffer[ii]; 00399 00400 } 00401 //sp=caux[2]; 00402 00403 } 00404 //sp=strtol(caux,null,2); 00405 00406 } 00407 00408 00409 // 00410 med = y.read()*999; 00411 err = (sp-med); //se calcula el error 00412 ap = kp*err*0.01f; //se calcula la accion proporcinal 00413 ai =(ki*err*0.01f)+ai; //calculo de la integral del error 00414 ad = kd*(err-err_v)*0.01f; //calculo de la accion derivativa 00415 pid = (ap+ai+ad); 00416 // se verifica que pid sea positivo ************************************** 00417 if(pid<=0) 00418 { 00419 pid=0; 00420 } 00421 00422 // se verifica que pid sea menor o igual la valor maximo ***************** 00423 if (pid > 999) 00424 { 00425 pid=999; 00426 } 00427 00428 00429 //se muestran las variables****************************************** 00430 lcd.locate(3,0); 00431 lcd.printf(" "); 00432 lcd.locate(3,0); 00433 lcd.printf("%3.0f",err); 00434 lcd.locate(11,0); 00435 lcd.printf(" "); 00436 lcd.locate(11,0); 00437 lcd.printf("%3.0f",med); 00438 lcd.locate(3,1); 00439 lcd.printf(" "); 00440 lcd.locate(3,1); 00441 lcd.printf("%d",sp); 00442 lcd.locate(11,1); 00443 lcd.printf(" "); 00444 lcd.locate(11,1); 00445 lcd.printf("%3.0f",pid); 00446 00447 00448 00449 00450 //Normalizacion de la salida 00451 // se actualizan las variables ******************************************* 00452 err_v = err; 00453 if(err_v<0) 00454 { 00455 err_v=err_v*(-1); 00456 } 00457 00458 if(err_v<256) 00459 { 00460 usart.putc(err_v); 00461 usart.putc(0); 00462 00463 } 00464 00465 if(err_v>255) 00466 { 00467 j=err_v/256; 00468 k=err_v-j*256; 00469 usart.putc(k); 00470 usart.putc(j); 00471 00472 } 00473 //wait(0.1); 00474 00475 med_v = med; 00476 //wait(0.1); 00477 if(med_v<256) 00478 { 00479 usart.putc(med_v); 00480 usart.putc(0); 00481 00482 } 00483 00484 if(med_v>255) 00485 { 00486 j=med_v/256; 00487 k=med_v-j*256; 00488 usart.putc(k); 00489 usart.putc(j); 00490 00491 } 00492 //GSM.putc(err_v); 00493 o = pid/999; 00494 u.write(o); 00495 // se envia el valor pid a puerto analogico de salida (D/A) ************** 00496 00497 // se repite el ciclo 00498 wait_ms(300); 00499 } 00500 00501 00502 }
Generated on Thu Jul 14 2022 20:48:48 by 1.7.2