yahir yahir
YAAA
Embed:
(wiki syntax)
Show/hide line numbers
main.cpp
00001 #include "mbed.h" 00002 00003 float velocidad = 10; // dejar siempre en 10 00004 00005 float Kpn = 1.45; //.1//SIGA LA LÍNEA SIN VIBRAR, Si vibra mucho bajar valor, si no sigue line y no vibra subir valor 00006 long Kdn = 100; // 8//Mucho KDN hace cabecear y se sale en curvas//Poco KDn no cabezea pero se pierde en las curvas 00007 long Kde = 180; // 25 00008 long xu = 190 ; // repeticiones 00009 00010 int reversa = 255; // velocidad ala que ira el motor hacia atras 00011 int frente = 255; // velovidad ala que ira el motor hacia enfrente 00012 int turbina = 98; // velocidad de turbina 00013 int line_data = 2; // elegir el color de la linea, 2 negro o 1 blanco 00014 int modulo_on = 2; // 0 = solo usar boton. 1 = usar modulo solo para iniciar. 2 = usar modulo para iniciar y apagar 00015 char allow = 1; // permitir o no el uso dela EDF manualmente sin interfaz 0 y 1 00016 00017 00018 00019 00020 00021 int memoria_on = 0; 00022 00023 00024 00025 00026 Timeout interrupt_edf; 00027 Timeout interrupt_pid; 00028 00029 Serial pc ( USBTX , USBRX); 00030 00031 DigitalOut uno(PF_1); 00032 DigitalOut dos(PF_0); 00033 DigitalOut tres(PB_1); 00034 DigitalOut cuatro(PB_6); 00035 00036 AnalogIn sensores(PB_0); 00037 DigitalOut led(PA_1); //Cambiar LED1 a PA_1 para usar led de la pcb, dejar en LED1 para usar led del mismo núcleo 00038 DigitalOut led1(PA_1); 00039 DigitalOut led2(LED1); 00040 DigitalOut led3(PA_3); 00041 AnalogIn boton ( PA_4); 00042 //AnalogIn encoder ( PA_3); 00043 00044 00045 00046 PwmOut mdf(PA_8); 00047 PwmOut mda(PA_11); 00048 00049 PwmOut mia(PB_4); 00050 PwmOut mif(PB_5); 00051 00052 00053 00054 DigitalOut brush( PA_9); 00055 00056 Timer timerb; // brush 00057 Timer timerx; // principal para los calculos 00058 Timer Tvista; // para retardar el paro tipo brasil 00059 Timer Tmargen; 00060 Timer Tled; 00061 Timer Tesc; 00062 long esc = 0; 00063 long cont_margen = 0; 00064 long t_margen = 0; 00065 00066 float Kp ; 00067 int Kd ; 00068 float reversaPD = 1; 00069 long TiempoMuestreo = 1; 00070 unsigned long pasado=0; 00071 unsigned long ahora; 00072 double Y; 00073 double errorpd; 00074 double errorPass=0; 00075 double errorAnt=0; 00076 double U; 00077 int errorx; 00078 int tb_permitir = 0; 00079 long contg; 00080 long conts; 00081 int memo; 00082 long contv; 00083 long tt; 00084 char tu ; 00085 int vc= 0; 00086 int z; 00087 int z1; 00088 int z2 = 0; 00089 float z3; 00090 uint16_t ebrush; 00091 uint16_t estado1; 00092 uint16_t estado2; 00093 uint16_t estado3; 00094 uint16_t estado4; 00095 uint16_t estado5; 00096 uint16_t estado6; 00097 uint16_t estado7; 00098 uint16_t estado8; 00099 uint16_t estado9; 00100 uint16_t estado10; 00101 uint16_t estado11; 00102 uint16_t estado12; 00103 uint16_t estado13; 00104 uint16_t estado14; 00105 uint16_t estado15; 00106 uint16_t estado16; 00107 00108 00109 int16_t maxs1 = 0; 00110 uint16_t maxs2 = 0; 00111 uint16_t maxs3 = 0; 00112 uint16_t maxs4 = 0; 00113 uint16_t maxs5 = 0; 00114 uint16_t maxs6 = 0; 00115 uint16_t maxs7 = 0; 00116 uint16_t maxs8 = 0; 00117 uint16_t maxs9 = 0; 00118 uint16_t maxs10 = 0; 00119 uint16_t maxs11 = 0; 00120 uint16_t maxs12 = 0; 00121 uint16_t maxs13 = 0; 00122 uint16_t maxs14 = 0; 00123 uint16_t maxs15 = 0; 00124 uint16_t maxs16 = 0; 00125 00126 int16_t mins1 = 0; 00127 uint16_t mins2 = 0; 00128 uint16_t mins3 = 0; 00129 uint16_t mins4 = 0; 00130 uint16_t mins5 = 0; 00131 uint16_t mins6 = 0; 00132 uint16_t mins7 = 0; 00133 uint16_t mins8 = 0; 00134 uint16_t mins9 = 0; 00135 uint16_t mins10 = 0; 00136 uint16_t mins11 = 0; 00137 uint16_t mins12 = 0; 00138 uint16_t mins13 = 0; 00139 uint16_t mins14 = 0; 00140 uint16_t mins15 = 0; 00141 uint16_t mins16 = 0; 00142 00143 int posicion ; 00144 int in_min = 0; 00145 long in_max = 65536; 00146 long out_min = 0; 00147 long out_max = 255; 00148 long v; 00149 long k; 00150 long k1; 00151 long k2 ; 00152 float time_edfon; 00153 float time_edfclb; 00154 int counter_edfon = 0; 00155 int counter_edfclb = 0; 00156 int calibrate_esc=0; 00157 uint16_t eboton; 00158 uint16_t epot; 00159 int esc_fin = 0; 00160 int m_esc= 0; 00161 int memoria_inter = 0; 00162 int memoria_tb=0; 00163 int contn =0; 00164 long j=0; 00165 int pp = 0; 00166 int cont_tbpermitir=0; 00167 int cont_mtpermitir = 0; 00168 float k3; 00169 int memox = 0; 00170 int data_start = 0; 00171 int cont_ndatos = 0; 00172 int edf_tx = 0; 00173 00174 int valor_sens = 100; 00175 int vista = 0; 00176 long before_vista = 0; 00177 int x0 = 0; 00178 uint16_t e_encoder = 0; 00179 long cont_encoder = 0; 00180 long cont_encoder2 = 0; 00181 long cont_marcas = 0; 00182 int cc = 0; 00183 int cc2 = 0; 00184 int memoria_encoder = 0; 00185 int yuk = 0; 00186 float time_muestreo = 0; 00187 int yh = 0; 00188 int kt = 0; 00189 int estado1x = 0; 00190 int estado2x = 0; 00191 int estado3x = 0; 00192 int estado4x = 0; 00193 int estado5x = 0; 00194 int estado6x = 0; 00195 int estado7x = 0; 00196 int estado8x = 0; 00197 int estado9x = 0; 00198 int estado10x = 0; 00199 int estado11x = 0; 00200 int estado12x = 0; 00201 int estado13x = 0; 00202 int estado14x = 0; 00203 int estado15x = 0; 00204 int estado16x = 0; 00205 long TL = 0; // Guardar tiempo para led 00206 int calibrar_inter = 0; 00207 long count_deg = 0; 00208 int memoria_deg = 0; 00209 int class_memory1 = 0; 00210 int class_memory2 = 0; 00211 int class_memory3 = 0; 00212 int xc = 1; 00213 float lineal = 0; 00214 00215 int m = 0; 00216 00217 00218 00219 00220 00221 00222 00223 void lectura() 00224 { 00225 00226 00227 uno = 0; dos = 0; tres = 0; cuatro = 0; estado1 = sensores.read_u16()/257; 00228 uno = 0; dos = 0; tres = 0; cuatro = 1; estado5 = sensores.read_u16()/257; 00229 uno = 0; dos = 0; tres = 1; cuatro = 0; estado9 = sensores.read_u16()/257; 00230 uno = 0; dos = 0; tres = 1; cuatro = 1; estado13 = sensores.read_u16()/257; 00231 uno = 0; dos = 1; tres = 0; cuatro = 0; estado3 = sensores.read_u16()/257; 00232 uno = 0; dos = 1; tres = 0; cuatro = 1; estado7 = sensores.read_u16()/257; 00233 uno = 0; dos = 1; tres = 1; cuatro = 0; estado11 = sensores.read_u16()/257; 00234 uno = 0; dos = 1; tres = 1; cuatro = 1; estado15 = sensores.read_u16()/257; 00235 uno = 1; dos = 0; tres = 0; cuatro = 0; estado2 = sensores.read_u16()/257; 00236 uno = 1; dos = 0; tres = 0; cuatro = 1; estado6 = sensores.read_u16()/257; 00237 uno = 1; dos = 0; tres = 1; cuatro = 0; estado10 = sensores.read_u16()/257; 00238 uno = 1; dos = 0; tres = 1; cuatro = 1; estado14 = sensores.read_u16()/257; 00239 uno = 1; dos = 1; tres = 0; cuatro = 0; estado4 = sensores.read_u16()/257; 00240 uno = 1; dos = 1; tres = 0; cuatro = 1; estado8 = sensores.read_u16()/257; 00241 uno = 1; dos = 1; tres = 1; cuatro = 0; estado12 = sensores.read_u16()/257; 00242 uno = 1; dos = 1; tres = 1; cuatro = 1; estado16 = sensores.read_u16()/257; 00243 00244 00245 00246 // 2 para linea negre, el robot normalmente da maximo en blanco minimo en negro 00247 if ( line_data == 2 ) { 00248 00249 estado1 = abs (255-estado1); 00250 estado2 = abs (255-estado2); 00251 estado3 = abs (255-estado3); 00252 estado4 = abs (255-estado4); 00253 estado5 = abs (255-estado5); 00254 estado6 = abs (255-estado6); 00255 estado7 = abs (255-estado7); 00256 estado8 = abs (255-estado8); 00257 estado9 = abs (255-estado9); 00258 estado10 = abs (255-estado10); 00259 estado11 = abs (255-estado11); 00260 estado12 = abs (255-estado12); 00261 estado13 = abs (255-estado13); 00262 estado14 = abs (255-estado14); 00263 estado15 = abs (255-estado15); 00264 estado16 = abs (255-estado16); } 00265 00266 } 00267 00268 00269 00270 00271 00272 00273 void timer_interrupt_pid() 00274 { 00275 00276 time_muestreo = 0.001; 00277 00278 00279 double errorD =(errorx-errorAnt); 00280 float P=Kp*errorx; 00281 float D=Kd*errorD*.1; 00282 U=P+D; 00283 00284 errorAnt=errorx; 00285 interrupt_pid.attach(&timer_interrupt_pid, time_muestreo); 00286 00287 00288 } 00289 00290 00291 00292 00293 00294 00295 void timer_interrupt_edf() 00296 { 00297 00298 counter_edfon=counter_edfon+1; 00299 if ( counter_edfon == 1 ) { time_edfon = k3; } 00300 if ( counter_edfon == 2 ) { time_edfon = 0.019; } 00301 if ( cont_tbpermitir == 0 ) { time_edfon= 0.001; } 00302 if ( allow == 0 ) { time_edfon= 0.001; } 00303 brush = !brush; 00304 00305 interrupt_edf.attach(&timer_interrupt_edf, time_edfon); 00306 if ( counter_edfon == 2 ) { counter_edfon = 0; } 00307 00308 } 00309 00310 00311 00312 00313 00314 00315 00316 int main() { 00317 pc.baud(9600); 00318 pc.format(8,Serial::None,1); 00319 mif.period(0.002); 00320 mia.period(0.002); 00321 mdf.period(0.002); 00322 mda.period(0.002); 00323 00324 //boton.mode(PullDown); 00325 00326 // Datas***************************************************************************************************************************************************** 00327 00328 00329 00330 00331 brushs1: //****************************************************************************** 00332 eboton = boton.read_u16(); 00333 if ( eboton > 30000 ) { allow = 0; led = 0; } 00334 if ( eboton < 1000 ) { led = 1; } 00335 00336 timerb.start(); 00337 j = timerb.read_ms(); 00338 if (j > 3000) { esc_fin= 1; timerb.stop(); goto data_before; } 00339 brush = 1; 00340 wait_us(1200); 00341 brush = 0; 00342 wait_us(18000); 00343 goto brushs1; 00344 00345 data_before: //**************************************************************** 00346 led3 = 0; 00347 eboton = boton.read_u16(); 00348 if ( eboton > 30000 ) { wait ( .5); goto calibrate; } 00349 goto data_before; 00350 00351 00352 00353 00354 00355 00356 00357 00358 00359 off: 00360 mif.write(0); mdf.write(0); mia.write(0); mda.write(0); 00361 cont_tbpermitir = 0; 00362 eboton = boton.read_u16(); 00363 if ( eboton > 30000 && modulo_on >= 1 ) { goto inicio_bef; } 00364 goto off; 00365 00366 00367 00368 00369 00370 00371 00372 calibrate: //*********************************************************************************** 00373 00374 Tesc.start(); 00375 esc = Tesc.read_us(); 00376 if ( esc <= 1200 && allow == 1 ) { brush = 1; } // 1170 blanco - 1370 azul 00377 if ( esc > 1200 ) { brush = 0; } 00378 if ( esc >= 18000 ) { Tesc.reset(); } 00379 00380 Tled.start(); 00381 TL = Tled.read_ms(); 00382 if ( TL >= 200 ) { Tled.reset(); } 00383 if ( TL <= 100 ) { led3 = 1; } 00384 if ( TL > 100 ) { led3 = 0; } 00385 00386 00387 lectura(); 00388 00389 eboton = boton.read_u16(); 00390 if ( eboton > 30000 && calibrar_inter == 0) { Tled.stop(); led3 = 0; wait(.3); goto start1; } 00391 00392 00393 //if (estado1 > maxs1) { maxs1 = estado1; } 00394 if (estado1 > mins1) { mins1 = estado1; } 00395 00396 //if (estado2 > maxs2) { maxs2 = estado2; } 00397 if (estado2 > mins2) { mins2 = estado2; } 00398 00399 // if (estado3 > maxs3) { maxs3 = estado3; } 00400 if (estado3 > mins3) { mins3 = estado3; } 00401 00402 //if (estado4 > maxs4) { maxs4 = estado4; } 00403 if (estado4 > mins4) { mins4 = estado4; } 00404 00405 // if (estado5 > maxs5) { maxs5 = estado5; } 00406 if (estado5 > mins5) { mins5 = estado5; } 00407 00408 // if (estado6 > maxs6) { maxs6 = estado6; } 00409 if (estado6 > mins6) { mins6 = estado6; } 00410 00411 // if (estado7 > maxs7) { maxs7 = estado7; } 00412 if (estado7 > mins7) { mins7 = estado7; } 00413 00414 //if (estado8 > maxs8) { maxs8 = estado8; } 00415 if (estado8 > mins8) { mins8 = estado8; } 00416 00417 00418 00419 //if (estado9 > maxs9) { maxs9 = estado9; } 00420 if (estado9 > mins9) { mins9 = estado9; } 00421 00422 // if (estado10 > maxs10) { maxs10 = estado10; } 00423 if (estado10 > mins10) { mins10 = estado10; } 00424 00425 //if (estado11 > maxs11) { maxs11 = estado11; } 00426 if (estado11 > mins11) { mins11 = estado11; } 00427 00428 //if (estado12 > maxs12) { maxs12 = estado12; } 00429 if (estado12 > mins12) { mins12 = estado12; } 00430 00431 // if (estado13 > maxs13) { maxs13 = estado13; } 00432 if (estado13 > mins13) { mins13 = estado13; } 00433 00434 //if (estado14 > maxs14) { maxs14 = estado14; } 00435 if (estado14 > mins14) { mins14 = estado14; } 00436 00437 //if (estado15 > maxs15) { maxs15 = estado15; } 00438 if (estado15 > mins15) { mins15 = estado15; } 00439 00440 //if (estado16 > maxs16) { maxs16 = estado16; } 00441 if (estado16 > mins16) { mins16 = estado16; } 00442 00443 maxs1 = 255+mins1; 00444 maxs2 = 255+mins2; 00445 maxs3 = 255+mins3; 00446 maxs4 = 255+mins4; 00447 maxs5 = 255+mins5; 00448 maxs6 = 255+mins6; 00449 maxs7 = 255+mins7; 00450 maxs8 = 255+mins8; 00451 maxs9 = 255+mins9; 00452 maxs10 = 255+mins10; 00453 maxs11 = 255+mins11; 00454 maxs12 = 255+mins12; 00455 maxs13 = 255+mins13; 00456 maxs14 = 255+mins14; 00457 maxs15 = 255+mins15; 00458 maxs16 = 255+mins16; 00459 00460 goto calibrate; 00461 00462 00463 00464 //****************************************************************************************************************************************** 00465 00466 start1: 00467 00468 Tesc.start(); 00469 esc = Tesc.read_us(); 00470 if ( esc <= 1200 && allow == 1 ) { brush = 1; } 00471 if ( esc > 1200 ) { brush = 0; } 00472 if ( esc >= 18000 ) { Tesc.reset(); } 00473 00474 eboton = boton.read_u16(); 00475 if ( eboton > 30000 && class_memory2 == 0 ) { class_memory3 = 0; class_memory1 = class_memory1+1; class_memory2 = 1; wait(.1); } 00476 if ( eboton < 1000 && class_memory2 == 1 ) { wait(.050); class_memory2 = 0; } 00477 if ( class_memory1 >= 1 ) { wait(.001); class_memory3 = class_memory3+1; } 00478 if ( class_memory3 >= 800) { goto showx; } 00479 00480 goto start1; 00481 00482 showx: 00483 if ( class_memory1 == 1 ) { lineal = 1; m = 1; goto conturbina; } 00484 00485 while ( xc < class_memory1 ) { 00486 led = 1; 00487 wait(.2); 00488 led = 0; 00489 wait(.3); 00490 xc = xc+1; } 00491 00492 if ( class_memory1 == 2 ) { lineal = .1; goto sinturbina; } 00493 if ( class_memory1 == 3 ) { lineal = 0.05; goto sinturbina; } 00494 if ( class_memory1 == 4 ) { lineal = 0.04; goto sinturbina; } 00495 if ( class_memory1 == 5 ) { lineal = 0.03; goto sinturbina; } 00496 if ( class_memory1 == 6 ) { lineal = 0.02; goto sinturbina; } 00497 if ( class_memory1 == 7 ) { lineal = 0.01; goto sinturbina; } 00498 00499 goto showx; 00500 00501 conturbina: 00502 00503 00504 led= 1; 00505 00506 Tesc.start(); 00507 esc = Tesc.read_us(); 00508 if ( esc <= 1400 && allow == 1 ) { brush = 1; } // 1170 blanco - 1370 azul 00509 if ( esc > 1400 ) { brush = 0; } 00510 if ( esc >= 18000 ) { Tesc.reset(); } 00511 00512 00513 eboton = boton.read_u16(); 00514 if ( eboton > 30000 && modulo_on >= 1) { led = 0; Tesc.stop(); brush = 0; goto inicio_bef; } 00515 if ( eboton > 30000 && x0 == 0) { wait (.2); x0 = 1; } 00516 if ( eboton < 500 && x0 == 1 ) {led = 0; Tesc.stop(); brush = 0; wait (.06); goto inicio_bef; } 00517 goto conturbina; 00518 00519 00520 sinturbina: 00521 Tesc.start(); 00522 esc = Tesc.read_us(); 00523 if ( esc <= 2450 ) { brush = 1; } // 1170 blanco - 1370 azul 00524 if ( esc > 2450 ) { brush = 0; } 00525 if ( esc >= 17000 ) { Tesc.reset(); } 00526 00527 led= 1; 00528 eboton = boton.read_u16(); 00529 if ( eboton > 30000 && modulo_on >= 1) { led = 0; Tesc.stop(); brush = 0; goto inicio_bef; } 00530 if ( eboton > 30000 && x0 == 0) { wait (.2); x0 = 1; } 00531 if ( eboton < 500 && x0 == 1 ) {led = 0; Tesc.stop(); brush = 0; goto inicio_bef; } 00532 goto sinturbina; 00533 00534 inicio_bef: //************************************************************************************************************************ 00535 00536 // wait ( 3); // 3 SEGUNDOS ANTES DE TURBINA 00537 00538 interrupt_edf.attach(&timer_interrupt_edf, time_edfon); 00539 interrupt_pid.attach(&timer_interrupt_pid, time_muestreo); 00540 00541 00542 00543 00544 k3 = turbina*0.00002; // tiempo turbina 00545 00546 00547 00548 cont_tbpermitir = 1; // permitir uso de edf 00549 00550 AnalogIn pot ( PA_0); // declaro mi pot despues del Serial 00551 00552 // wait ( 2); // 2 SEGUNDOS DESPUES DE ENCENDER TURBINA**** 00553 00554 while(1) { 00555 00556 00557 00558 eboton = boton.read_u16(); 00559 if ( eboton < 500 && modulo_on == 2 ) { goto off; } 00560 00561 epot = pot.read_u16(); 00562 epot =(epot - 0) * (255 - 0) / (65536 - 0) + 0; //mapear 00563 //if ( vista == 0) { velocidad =epot; } 00564 //velocidad =epot 00565 00566 if ( velocidad < epot ) { velocidad = velocidad+lineal; } // Aceleracion Lineal .5 solo con turbina 00567 00568 00569 lectura(); 00570 00571 estado1x =(estado1 - mins1 ) * maxs1 /(255-mins1) ; 00572 if ( estado1x <= 0 ) { estado1x = 0; } 00573 if ( estado1x >= 255 ) { estado1x = 255; } 00574 00575 00576 estado2x =(estado2 - mins2 )* maxs2 /(255-mins2) ; 00577 if ( estado2x <= 0 ) { estado2x = 0; } 00578 if ( estado2x >= 255 ) { estado2x = 255; } 00579 00580 estado3x =(estado3 - mins3 )* maxs3/(255-mins3); 00581 if ( estado3x <= 0 ) { estado3x = 0; } 00582 if ( estado3x >= 255 ) { estado3x = 255; } 00583 00584 estado4x =(estado4 - mins4 )* maxs4/(255-mins4); 00585 if ( estado4x <= 0 ) { estado4x = 0; } 00586 if ( estado4x >= 255 ) { estado4x = 255; } 00587 00588 estado5x =(estado5 - mins5 )* maxs5/(255-mins5); 00589 if ( estado5x <= 0 ) { estado5x = 0; } 00590 if ( estado5x >= 255 ) { estado5x = 255; } 00591 00592 estado6x =(estado6 - mins6 )* maxs6/(255-mins6); 00593 if ( estado6x <= 2 ) { estado6x = 0; } 00594 if ( estado6x >= 255 ) { estado6x = 255; } 00595 00596 estado7x =(estado7 - mins7 )* maxs7/(255-mins7); 00597 if ( estado7x <= 0 ) { estado7x = 0; } 00598 if ( estado7x >= 255 ) { estado7x = 255; } 00599 00600 estado8x =(estado8 - mins8 )* maxs8/(255-mins8); 00601 if ( estado8x <= 0 ) { estado8x = 0; } 00602 if ( estado8x >= 255 ) { estado8x = 255; } 00603 00604 estado9x =(estado9 - mins9 )* maxs9/(255-mins9); 00605 if ( estado9x <= 0 ) { estado9x = 0; } 00606 if ( estado9x >= 255 ) { estado9x = 255; } 00607 00608 estado10x =(estado10 - mins10 )* maxs10/(255-mins10); 00609 if ( estado10x <= 0 ) { estado10x = 0; } 00610 if ( estado10x >= 255 ) { estado10x = 255; } 00611 00612 estado11x =(estado11 - mins11 )* maxs11/(255-mins11); 00613 if ( estado11x <= 0 ) { estado11x = 0; } 00614 if ( estado11x >= 255 ) { estado11x = 255; } 00615 00616 estado12x =(estado12 - mins12 )* maxs12/(255-mins12); 00617 if ( estado12x <= 0 ) { estado12x = 0; } 00618 if ( estado12x >= 255 ) { estado12x = 255; } 00619 00620 estado13x =(estado13 - mins13 )* maxs13/(255-mins13); 00621 if ( estado13x <= 0 ) { estado13x = 0; } 00622 if ( estado13x >= 255 ) { estado13x = 255; } 00623 00624 estado14x =(estado14 - mins14 )* maxs14/(255-mins14); 00625 if ( estado14x <= 0 ) { estado14x = 0; } 00626 if ( estado14x >= 255 ) { estado14x = 255; } 00627 00628 estado15x =(estado15 - mins15 )* maxs15/(255-mins15); 00629 if ( estado15x <= 0 ) { estado15x = 0; } 00630 if ( estado15x >= 255 ) { estado15x = 255; } 00631 00632 estado16x =(estado16 - mins16 )* maxs16/(255-mins16); 00633 if ( estado16x <= 0 ) { estado16x = 0; } 00634 if ( estado16x >= 255 ) { estado16x = 255; } 00635 00636 00637 00638 // pc.printf (" e1: %i.\r\n", estado8); // imprime potenciometro 00639 00640 00641 if ( estado5x > valor_sens ) { Kp = Kpn; Kd = Kdn; } 00642 if ( estado12x > valor_sens ) { Kp = Kpn; Kd = Kdn; } 00643 00644 if ( estado8x > valor_sens ) { Kp = Kpn; Kd = Kde; } 00645 if ( estado9x > valor_sens ) { Kp = Kpn; Kd = Kde; } 00646 00647 if ( estado8x > valor_sens && estado9x > valor_sens ) { memo = 1; } 00648 00649 if ( estado8x > valor_sens && estado9x < valor_sens ) { memo = 2; } 00650 if ( estado9x > valor_sens && estado8x < valor_sens ) { memo = 3; } 00651 00652 00653 00654 // si hay intersecciones o no tiene que leer marcas a los lados 00655 00656 // Normal if ( memo == 1 ) { Kp = Kpn; Kd = Kde; errorx = estado6x*(-.4)+estado7x*(-.2)+estado8x*(-.1) + estado9x*(.1) +estado10x*(.2)+estado11x*(.4);} 00657 // Normal if ( memo == 2 ) { Kp = Kpn; Kd = Kdn; errorx = estado1x*(-1) + estado2x*(-2)+ estado3x*(-1)+estado4x*(-.8)+ estado5x*(-.6)+ estado6x*(-.4)+estado7x*(-.2)+estado8x*(-.1) + estado9x*(.1) +estado10x*(.2)+estado11x*(.4)+estado12x*(.6)+ estado13x*(.8)+ estado14x*(1)+estado15x*(2)+estado16x*(1);} 00658 00659 if ( memo == 1 ) { errorx = estado7x*(-.2)+estado8x*(-.1) + estado9x*(.1) +estado10x*(.2);} 00660 00661 if ( memo == 2 ) { 00662 errorx = estado1x*(-1) + estado2x*(-2)+ estado3x*(-1)+estado4x*(-.8)+ estado5x*(-.6)+ estado6x*(-.4)+estado7x*(-.2)+estado8x*(-.1) ;} 00663 00664 if ( memo == 3 ) { 00665 errorx = estado9x*(.1) +estado10x*(.2)+estado11x*(.4)+estado12x*(.6)+ estado13x*(.8)+ estado14x*(1)+estado15x*(2)+estado16x*(1);} 00666 00667 /* 00668 errorx = estado1x*(-1) + estado2x*(-2)+ estado3x*(-1)+estado4x*(-.8)+ estado5x*(-.6)+ estado6x*(-.4)+estado7x*(-.2)+estado8x*(-.1) + estado9x*(.1) +estado10x*(.2)+estado11x*(.4)+estado12x*(.6)+ estado13x*(.8)+ estado14x*(1)+estado15x*(2)+estado16x*(1); 00669 if ( memo == 1 ) { Kp = Kpn; Kd = Kde; } 00670 if ( memo == 2 ) { Kp = Kpn; Kd = Kdn; } 00671 */ 00672 00673 00674 if ( estado2x > valor_sens ) { posicion = 0; contg = 0;} 00675 if ( estado15x > valor_sens ) { posicion = 0; contg = 0;} 00676 00677 //if ( estado1x > valor_sens ) { posicion = 1;} // que vea extremo, 8 y 9 no vean 00678 //if ( estado16x > valor_sens ) { posicion = 16;} 00679 00680 if ( estado1x > valor_sens && estado8x < valor_sens && estado9x < valor_sens ) { posicion = 1;} // que vea extremo, 8 y 9 no vean 00681 if ( estado16x > valor_sens && estado8x < valor_sens && estado9x < valor_sens ) { posicion = 16;} 00682 00683 00684 // parte para poder leer angulos 00685 if ( estado16x > valor_sens) { count_deg = 0; memoria_deg = 16;} 00686 if ( memoria_deg == 16 && estado16x < valor_sens && estado7x < valor_sens && estado8x < valor_sens && estado9x < valor_sens && estado10x < valor_sens ) { memoria_deg = 0; posicion = 16;} 00687 00688 if ( estado1x > valor_sens ) { count_deg = 0; memoria_deg = 1;} 00689 if ( memoria_deg == 1 && estado1x < valor_sens && estado7x < valor_sens && estado8x < valor_sens && estado9x < valor_sens && estado10x < valor_sens ) { memoria_deg = 0; posicion = 1;} 00690 00691 if ( estado1 < valor_sens && memoria_deg > 0 ) { count_deg = count_deg+1; led = 1;} 00692 if ( estado16 < valor_sens && memoria_deg > 0 ) { count_deg = count_deg+1; led = 1;} 00693 if ( count_deg >= 1000) { memoria_deg = 0; led = 0; } 00694 //// parte para poder leer angulos 00695 00696 00697 if ( U >= 255 ) { U = 255; } 00698 if ( U <= -255 ) { U = -255; } 00699 00700 int pwm1 = velocidad - (U); 00701 int pwm2 = velocidad + (U); 00702 00703 if ( pwm1 >= 255 ) { pwm1 = 255; } 00704 if ( pwm2 >= 255 ) { pwm2 = 255; } 00705 00706 float pwm11 = pwm1*.004; 00707 float pwm22 = pwm2*.004; 00708 00709 if ( pwm11 >= 1 ) { pwm11 = 1; } 00710 if ( pwm22 >= 1 ) { pwm22 = 1; } 00711 00712 00713 00714 float r1 = abs(pwm11); 00715 float r2 = abs (pwm22); 00716 00717 if ( r1 >= 1 ) { r1 = 1; } 00718 if ( r2 >= 1 ) { r2 = 1; } 00719 00720 float reversa1 = reversa*.004; 00721 float frente1 = frente*.004; 00722 if ( reversa1 >= 1 ) { reversa1 = 1; } 00723 if ( frente1 >= 1 ) { frente1 = 1; } 00724 00725 00726 if ( posicion == 16 ) { 00727 00728 contg = contg+1; 00729 if ( contg < xu) { 00730 mda.write(reversa1); mdf.write(0); 00731 mif.write (frente1); mia.write (0); } 00732 00733 if ( contg > xu) { 00734 mda.write(0); mdf.write(0); 00735 mif.write(frente1); mia.write(0); } } 00736 00737 00738 00739 if ( posicion == 1 ) { 00740 00741 contg = contg+1; 00742 if ( contg < xu) { 00743 mdf.write(frente1); mda.write(0); 00744 mia.write(reversa1); mif.write(0); } 00745 00746 if ( contg > xu) { 00747 mdf.write(frente1); mda.write(0); 00748 mif.write(0); mia.write(0); } } 00749 00750 00751 if ( posicion != 16 && posicion != 1 ) { 00752 00753 00754 if ( pwm11 < 0 ) { mda.write(r1); mdf.write(0); } 00755 00756 if ( pwm22 < 0 ) { mia.write(r2); mif.write(0); } 00757 00758 if ( pwm11 == 0 ) { mda.write(0); mdf.write(0); } 00759 00760 if ( pwm22 == 0 ) { mia.write(0); mif.write(0); } 00761 00762 if ( pwm11 > 0 ) { mdf.write(pwm11); mda.write(0); } 00763 00764 if ( pwm22 > 0 ) { mif.write(pwm22); mia.write(0); } 00765 00766 }}}
Generated on Mon Nov 7 2022 20:05:12 by
1.7.2