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   }}}