deiwid ricaurte
controlpid por control remoto
Dependencies: Pulse1 QEI TextLCD mbed
Fork of
irda1
by 
Gustavo Ramirez
Embed:
(wiki syntax)
Show/hide line numbers
main.cpp
00001 #include "mbed.h" 00002 #include <Pulse1.h> 00003 #include "QEI.h" 00004 #include "TextLCD.h" 00005 00006 00007 TextLCD lcd(PTB10, PTB11, PTE2, PTE3, PTE4, PTE5); // rs, e, d4-d7 00008 AnalogIn y(PTB2);//entrada analoga 00009 AnalogOut u(PTE30);//salida analoga OJO solo se le pueden drenar 1.5mA en circuitos use un Buffer 00010 PulseInOut irda(PTD5);// en este puerto se pone el sensor infrarrojo 00011 Serial pc(USBTX, USBRX); 00012 DigitalOut led(LED1); 00013 DigitalOut led2(LED2); 00014 int header =0; //tiempo de cabecera pulso abajo 00015 const int head_H = 9100; //+20% medida con osciloscopio en microsegundos 00016 const int head_L = 8500;//-20% medida con osciloscopio 00017 int i=0; 00018 const int T_alto=1680;//ponga su tiempo de la prueba 00019 const int T_bajo=526;//ponga su tiempo de la prueba 00020 const int num_bits = 32;//ponga su numero de bits 00021 int num[num_bits];//cadena para almacenar todos los tiempos que conforman los bits de datos 00022 int num1[num_bits]; 00023 int arriba[]={1,1,1,0,0,0,0,1,0,1,1,1,1,0,1,0,1,1,0,1,0,0,0,0,0,0,1,0,1,1,1}; 00024 int abajo[]={1,1,1,0,0,0,0,1,0,1,1,1,1,0,1,0,0,0,1,1,0,0,0,0,1,1,0,0,1,1,1}; 00025 int derecha[]={1,1,1,0,0,0,0,1,0,1,1,1,1,0,1,0,0,0,1,1,1,0,0,0,1,1,0,0,0,1,1}; 00026 int ok[]={1,1,1,0,0,0,0,1,0,1,1,1,1,0,1,0,1,0,1,0,1,0,0,0,0,1,0,1,0,1,1}; 00027 int dato; // tiempo de cada dato que se lee 00028 int a=0; 00029 int b=0; 00030 int c=0; 00031 int d=0; 00032 00033 00034 int C2=0x18; // desplaza izquierda 00035 int C3=0x1A; // desplaza derecha 00036 int C4=0x0C; // quito cursor bajo 00037 00038 int C1=0x0F; 00039 int cambio=0, diferencia=0; 00040 float pid,o,ai,ad,ap,med,err; 00041 float err_v; 00042 int sp=0,ki=0,kp=0,kd=0,pos=1; 00043 00044 00045 int main(){ 00046 lcd.locate(0,1); 00047 lcd.printf("**Control PID**"); 00048 wait(2); 00049 lcd.cls(); // Borrar Pantalla 00050 lcd.writeCommand(C1);//escribimos un comando segun el manual del modulo LCD 00051 00052 lcd.locate(8,0); 00053 lcd.printf("Kp=%d",kp); 00054 lcd.locate(0,1); 00055 lcd.printf("Ki=%d",ki); 00056 lcd.locate(8,1); 00057 lcd.printf("Kd=%d",kd); 00058 lcd.locate(0,0); 00059 lcd.printf("Sp=%d",sp); 00060 00061 while(1){ 00062 ini1: header=0; 00063 a=0; 00064 b=0; 00065 c=0; 00066 d=0; 00067 led2=1; 00068 led=1; 00069 header = irda.read_low_us(); //funcion para leer un pulso de caida o bajo 00070 if (header > head_L && header < head_H) goto seguir;//verificar que este en la tolerancia +-20% 00071 else goto ini1; 00072 00073 seguir: 00074 //leo los datos de la trama y se meten a un arreglo 00075 wait_us(5000); 00076 led2=0; 00077 for(i=0;i<(num_bits-1);++i) 00078 { // POR OSCILOSCOPIO se determina que llegan (num_bits),datos 00079 dato = irda.read_high_us(); //leer un bit de datos que es pulso arriba en este control 00080 num[i]=dato; 00081 wait_us(300); 00082 } 00083 wait(0.5); //espero un poquito antes de leer todo el arreglo y ponerlo en pantalla 00084 pc.printf(",%d",header); 00085 for(i=0;i<(num_bits-1);++i) 00086 { 00087 pc.printf(",%d",num[i]); 00088 } 00089 wait(0.1); //espero e imprimo en binario 00090 pc.printf("\n\n"); 00091 for(i=0;i<(num_bits-1);++i) 00092 { 00093 00094 if(num[i] > ((T_alto+T_bajo)/2)) 00095 { 00096 pc.printf("1"); 00097 num1[i]=1; 00098 if(num1[i]==arriba[i]){ 00099 a=a+1; 00100 00101 } 00102 if(num1[i]==abajo[i]){ 00103 b=b+1; 00104 00105 } 00106 if(num1[i]==derecha[i]){ 00107 c=c+1; 00108 00109 } 00110 if(num1[i]==ok[i]){ 00111 d=d+1; 00112 00113 } 00114 } 00115 else 00116 { 00117 pc.printf("0"); 00118 num1[i]=0; 00119 if(num1[i]==arriba[i]){ 00120 a=a+1; 00121 } 00122 if(num1[i]==abajo[i]){ 00123 b=b+1; 00124 } 00125 if(num1[i]==derecha[i]){ 00126 c=c+1; 00127 } 00128 if(num1[i]==ok[i]){ 00129 d=d+1; 00130 } 00131 } 00132 } 00133 if(a==31) 00134 { 00135 a=0; 00136 b=0; 00137 c=0; 00138 d=0; 00139 if(pos==1) 00140 { 00141 00142 if(sp+1>=999) 00143 { 00144 sp=999; 00145 lcd.locate(3,0); 00146 lcd.printf(" "); 00147 lcd.locate(3,0); 00148 lcd.printf("%d", sp); 00149 } 00150 else 00151 { 00152 sp+=1; 00153 lcd.locate(3,0); 00154 lcd.printf("%d", sp); 00155 00156 } 00157 } 00158 else if(pos==2) 00159 { 00160 00161 if(kp+1>=999) 00162 { 00163 kp=999; 00164 lcd.locate(11,0); 00165 lcd.printf(" "); 00166 lcd.locate(11,0); 00167 lcd.printf("%d", kp); 00168 } 00169 else 00170 { 00171 kp+=1; 00172 lcd.locate(11,0); 00173 lcd.printf("%d", kp); 00174 00175 } 00176 } 00177 else if(pos==3) 00178 { 00179 00180 if(ki+1>=999) 00181 { 00182 sp=999; 00183 lcd.locate(3,1); 00184 lcd.printf(" "); 00185 lcd.locate(3,1); 00186 lcd.printf("%d", ki); 00187 } 00188 else 00189 { 00190 ki+=1; 00191 lcd.locate(3,1); 00192 lcd.printf("%d", ki); 00193 00194 } 00195 } 00196 else if(pos==4) 00197 { 00198 00199 if(kd+1>=999) 00200 { 00201 sp=999; 00202 lcd.locate(11,1); 00203 lcd.printf(" "); 00204 lcd.locate(11,1); 00205 lcd.printf("%d", kd); 00206 } 00207 else 00208 { 00209 kd+=1; 00210 lcd.locate(11,1); 00211 lcd.printf("%d", kd); 00212 00213 } 00214 } 00215 } 00216 00217 else if(b==31) 00218 { 00219 a=0; 00220 b=0; 00221 c=0; 00222 d=0; 00223 if(pos==1) 00224 { 00225 00226 if(sp-1<=0) 00227 { 00228 sp=0; 00229 lcd.locate(3,0); 00230 lcd.printf(" "); 00231 lcd.locate(3,0); 00232 lcd.printf("%d", sp); 00233 } 00234 else 00235 { 00236 sp-=1; 00237 lcd.locate(3,0); 00238 lcd.printf("%d", sp); 00239 00240 } 00241 } 00242 else if(pos==2) 00243 { 00244 00245 if(kp-1<=0) 00246 { 00247 kp=0; 00248 lcd.locate(11,0); 00249 lcd.printf(" "); 00250 lcd.locate(11,0); 00251 lcd.printf("%d", kp); 00252 } 00253 else 00254 { 00255 kp-=1; 00256 lcd.locate(11,0); 00257 lcd.printf("%d", kp); 00258 00259 } 00260 } 00261 else if(pos==3) 00262 { 00263 00264 if(ki-1<=0) 00265 { 00266 sp=0; 00267 lcd.locate(3,1); 00268 lcd.printf(" "); 00269 lcd.locate(3,1); 00270 lcd.printf("%d", ki); 00271 } 00272 else 00273 { 00274 ki-=1; 00275 lcd.locate(3,1); 00276 lcd.printf("%d", ki); 00277 00278 } 00279 } 00280 else if(pos==4) 00281 { 00282 00283 if(kd-1<=0) 00284 { 00285 sp=0; 00286 lcd.locate(11,1); 00287 lcd.printf(" "); 00288 lcd.locate(11,1); 00289 lcd.printf("%d", kd); 00290 } 00291 else 00292 { 00293 kd-=1; 00294 lcd.locate(11,1); 00295 lcd.printf("%d", kd); 00296 00297 } 00298 } 00299 } 00300 00301 if(c==31) 00302 { 00303 a=0; 00304 b=0; 00305 c=0; 00306 d=0; 00307 led2 =!led2; 00308 if(pos==4) 00309 { 00310 pos=1; 00311 lcd.locate(3,0); 00312 lcd.printf("%d", sp); 00313 } 00314 else if (pos==1) 00315 { 00316 pos++; 00317 lcd.locate(11,0); 00318 lcd.printf("%d", kp); 00319 } 00320 else if(pos==2) 00321 { 00322 pos++; 00323 lcd.locate(3,1); 00324 lcd.printf("%d", ki); 00325 } 00326 else if(pos==3) 00327 { 00328 pos++; 00329 lcd.locate(11,1); 00330 lcd.printf("%d", kd); 00331 } 00332 wait(0.25); 00333 } 00334 00335 if(d==31) 00336 { 00337 a=0; 00338 b=0; 00339 c=0; 00340 d=0; 00341 break; //sale del bucle si pulsan ok 00342 } 00343 else 00344 { 00345 a=0; 00346 b=0; 00347 c=0; 00348 d=0; 00349 } 00350 goto ini1; 00351 00352 00353 } 00354 //Transicion 00355 lcd.writeCommand(C4);//escribimos un comando segun el manual del modulo LCD para quitar cursor bajo 00356 lcd.cls(); //borra la pantalla 00357 lcd.printf(" GUARDADOS!"); 00358 wait(1); 00359 lcd.cls(); 00360 lcd.printf(" INICIA EL PID"); 00361 wait(1); 00362 // se imprimen los parches del control ***************************************** 00363 lcd.cls(); 00364 lcd.printf("Er=%3.0f",err); 00365 lcd.locate(8,0); 00366 lcd.printf("Me=%3.0f",med); 00367 lcd.locate(0,1); 00368 lcd.printf("Sp=%3.0f",sp); 00369 lcd.locate(8,1); 00370 lcd.printf("Co=%3.0f",pid); 00371 wait(1); 00372 00373 // CICLO PRINCIPAL CONTROLADOR PID 00374 lop1: med = y.read()*999; 00375 err = (sp-med); //se calcula el error 00376 ap = kp*err*0.01f; //se calcula la accion proporcinal 00377 ai =(ki*err*0.01f)+ai; //calculo de la integral del error 00378 ad = kd*(err-err_v)*0.01f; //calculo de la accion derivativa 00379 pid = (ap+ai+ad); 00380 // se verifica que pid sea positivo ************************************** 00381 if(pid<=0) 00382 { 00383 pid=0; 00384 } 00385 00386 // se verifica que pid sea menor o igual la valor maximo ***************** 00387 if (pid > 999) 00388 { 00389 pid=999; 00390 } 00391 00392 00393 //se muestran las variables****************************************** 00394 lcd.locate(3,0); 00395 lcd.printf(" "); 00396 lcd.locate(3,0); 00397 lcd.printf("%3.0f",err); 00398 lcd.locate(11,0); 00399 lcd.printf(" "); 00400 lcd.locate(11,0); 00401 lcd.printf("%3.0f",med); 00402 lcd.locate(3,1); 00403 lcd.printf(" "); 00404 lcd.locate(3,1); 00405 lcd.printf("%d",sp); 00406 lcd.locate(11,1); 00407 lcd.printf(" "); 00408 lcd.locate(11,1); 00409 lcd.printf("%3.0f",pid); 00410 00411 00412 00413 00414 //Normalizacion de la salida 00415 // se actualizan las variables ******************************************* 00416 err_v = err; 00417 00418 o = pid/999; 00419 u.write(o); 00420 00421 wait_ms(300); 00422 goto lop1; 00423 }
Generated on Sun Jul 17 2022 12:34:33 by
1.7.2