2doejemplo.cpp

00001 #include "mbed.h"
00002 #include "Sabertooth2x25.h"
00003 
00004 Sabertooth2x25 m(p9,9600);
00005 //*********************LEDs***************************
00006 DigitalOut led1(p29);       //Linea Derecha(rojo)
00007 DigitalOut led2(p28);       //Derecha
00008 DigitalOut led3(p27);       //Derecha Central
00009 DigitalOut led4(p21);       //Central
00010 DigitalOut led5(p22);       //izquierda Central
00011 DigitalOut led6(p23);       //Izquierda
00012 DigitalOut led7(p24);       //Linea Izquierda
00013 //********************Sensores int*********************
00014 DigitalIn senLinDer(p14);
00015 DigitalIn senDer(p15);
00016 DigitalIn senDerCen(p16);
00017 DigitalIn senCen(p20);
00018 DigitalIn senIzqCen(p19);
00019 DigitalIn senIzq(p18);
00020 DigitalIn senLinIzq(p17);
00021 //*******************SW  Int******************************
00022 DigitalIn sw1(p10);     //1
00023 DigitalIn sw2(p11);     //2
00024 DigitalIn sw3(p12);     //3
00025 DigitalIn sw4(p13);     //4
00026 //*******************variables***********************
00027                 int seleccion_linea_derecha=0;                           ////////
00028                 int seleccion_derecha=0;                                ////////  
00029                 int seleccion_enfrente_derecha=0;                       //////// 
00030                 int seleccion_enfrente=0;                               ////////                                      
00031                 int seleccion_enfrente_izquierda=0;                     ////////
00032                 int seleccion_izquierda=0;                              ////////     
00033                 int seleccion_linea_izquierda=0;                         ////////
00034                 int seleccion_suma=0;                                   ////////
00035                 int selec_programa_sw3=0;
00036                 int selec_programa_sw4=0;
00037                 int selec_programa_suma=0;
00038                
00039 //********************
00040     int Time=.2;
00041     int tiempo=0;
00042 //*****
00043 void Seleccion();
00044 void Inicio();
00045 void Leds();
00046 void Busqueda();
00047 void Casos();
00048 void Frente();
00049 void Derecha();
00050 void Izquierda();
00051 void Atras();
00052 void Paro();
00053 //**********************
00054 int main()
00055 {
00056 Seleccion();
00057 wait(1); 
00058 Leds();//.7seg
00059 Leds();//1.4seg
00060 Leds();//2.1seg
00061 Leds();//2.8seg
00062 Leds();//3.5seg
00063 Inicio();   
00064     while(1)
00065     {
00066     //Linea_derecha
00067         if(seleccion_suma==1)                                       ///-O-//// 
00068              Derecha();
00069         else            
00070 ///////////////////////////////////////////////////////////////////////////////
00071     //Linea izquierda
00072         if(seleccion_suma==1000)                                       ///-O-//// 
00073              Izquierda();
00074         else 
00075                                                                                //linea_derecha y linea izquierda                       //////////                                                                              
00076                                                                                if(seleccion_suma==1001)                                       ///-O-//// 
00077                                                                                {
00078                                                                                Atras();
00079                                                                                wait(.1);//100 ///////////////////////////////--------aqui-------------------------------------------------------
00080                                                                                Derecha();
00081                                                                                wait(Time);  /// ///////////////////////////////--------aqui-------------------------------------------------------
00082                                                                                }
00083                                                                                else                                                  ///-O-////       
00084     Busqueda();    
00085     Casos();
00086     }
00087     
00088 }
00089 
00090 void Leds()
00091 {
00092     led1=1;
00093     wait(.1);
00094     led1=0;
00095     
00096     led2=1;
00097     wait(.1);
00098     led2=0;
00099     
00100     led3=1;
00101     wait(.1);
00102     led3=0;
00103     
00104     led4=1;
00105     wait(.1);
00106     led4=0;
00107     
00108     led5=1;
00109     wait(.1);
00110     led5=0;
00111     
00112     led6=1;
00113     wait(.1);
00114     led6=0;
00115     
00116     led7=1;
00117     wait(.1);
00118     led7=0;      
00119 }
00120 void Seleccion()
00121 {       
00122     selec_programa_suma=0;
00123     
00124     if(sw3==1)
00125     {
00126     selec_programa_sw3=100;
00127     led2=1;
00128     }
00129     else
00130     led2=0;
00131     
00132     if(sw4==1)
00133     {
00134     selec_programa_sw4=1000;
00135     led1=1;
00136     }
00137     else
00138     led1=0;
00139     
00140 //******************   
00141     selec_programa_suma=selec_programa_sw3+selec_programa_sw4;
00142     
00143     selec_programa_sw3=0;
00144     selec_programa_sw4=0;
00145     
00146 }
00147 void Inicio()
00148 {                                                        ///-C-////
00149                                                                      ///-A-////
00150             switch( selec_programa_suma)                             ///-S-////
00151             {                                                        ///-O-////
00152                                                                      ///-S-////
00153 ///////////////////////////////////////////////////////////////////////////////
00154 ///////////////////////////////////////////////////////////////////////////////
00155       //sin oponente                                                 //////////
00156      case 0 : 
00157              selec_programa_suma=0;                                       ///-O-//// 
00158              Frente();
00159              break;    
00160 ///////////////////////////////////////////////////////////////////////////////
00161 ///////////////////////////////////////////////////////////////////////////////
00162      case 100 : 
00163              selec_programa_suma=0;                                       ///-O-//// 
00164              Izquierda();
00165              wait(Time);
00166              break; 
00167 ///////////////////////////////////////////////////////////////////////////////
00168 ///////////////////////////////////////////////////////////////////////////////
00169      case 1000 : 
00170              selec_programa_suma=0;                                       ///-O-//// 
00171              Derecha();
00172              wait(Time);
00173              break;            
00174 ///////////////////////////////////////////////////////////////////////////////
00175 ///////////////////////////////////////////////////////////////////////////////
00176     default:                                                       //////////
00177              selec_programa_suma=0;                                  ///-O-//// 
00178                                                                      //////////
00179                                                                      //////////
00180              break;                                                  //////////
00181                                                                      //////////                             
00182                        
00183             }
00184 }
00185 void Busqueda()
00186 
00187 {
00188     seleccion_suma=0;
00189     
00190     if(senLinDer==0)
00191     {
00192     seleccion_linea_derecha=1; 
00193     led7=1;
00194     }
00195     else
00196     led7=0;
00197     
00198     if(senDer==0)
00199     {
00200     seleccion_derecha=5;
00201     led6=1;
00202     }
00203     else
00204     led6=0;
00205     
00206     if(senDerCen==0)
00207     { 
00208     seleccion_enfrente_derecha=10;  
00209     led5=1;
00210     }
00211     else
00212     led5=0;
00213     
00214     if(senCen==0)
00215     {
00216     seleccion_enfrente=50; 
00217     led4=1;
00218     }
00219     else
00220     led4=0;
00221     
00222     if(senIzqCen==0)
00223     {
00224     seleccion_enfrente_izquierda=100; 
00225     led3=1;
00226     }
00227     else
00228     led3=0;
00229     
00230     if(senIzq==0)
00231     {
00232     seleccion_izquierda=500; 
00233     led2=1;
00234     }
00235     else
00236     led2=0;
00237     
00238     if(senLinIzq==0)
00239     {
00240     seleccion_linea_izquierda=1000; 
00241     led1=1;
00242     }
00243     else
00244     led1=0;
00245     
00246      seleccion_suma=seleccion_derecha + seleccion_enfrente_derecha + seleccion_enfrente + seleccion_enfrente_izquierda + seleccion_izquierda ; 
00247      
00248      //seleccion_linea_derecha=0;                           ////////1
00249      seleccion_derecha=0;                                 ////////5  
00250      seleccion_enfrente_derecha=0;                        ////////10
00251      seleccion_enfrente=0;                                ////////50                                      
00252      seleccion_enfrente_izquierda=0;                      ////////100
00253      seleccion_izquierda=0;                               ////////500     
00254      //seleccion_linea_izquierda=0;                         ////////1000  
00255      
00256      
00257      if(seleccion_suma>0)  
00258              {
00259                tiempo=0;
00260              }
00261 
00262 }
00263 void Casos()                                                       
00264             {                                                        ///-C-////
00265                                                                      ///-A-////
00266             switch( seleccion_suma)                                  ///-S-////
00267             {                                                        ///-O-////
00268                                                                      ///-S-////
00269 ////////////////////////////////////////////////////////////////////////////////
00270 ///////////////////////////////////////////////////////////////////////////////
00271       //sin oponente                                                 //////////
00272      case 0 : 
00273              seleccion_suma=0;                                       ///-O-//// 
00274              tiempo++;
00275              if(tiempo>=30000)
00276                {
00277                  Derecha();
00278                  tiempo=0;
00279                  wait(.280);
00280                }
00281                else{
00282                     Paro();
00283                     }
00284              //frente_inicio();
00285 
00286              break;                                                  ///-O-////          
00287 ///////////////////////////////////////////////////////////////////////////////
00288 ///////////////////////////////////////////////////////////////////////////////
00289      //derecha
00290      case 5 : 
00291              seleccion_suma=0;                                       ///-O-//// 
00292              Derecha();
00293              break;                                                  ///-O-////                      
00294 ///////////////////////////////////////////////////////////////////////////////
00295 ///////////////////////////////////////////////////////////////////////////////
00296      //enfre_derecha
00297      case 10 : 
00298              seleccion_suma=0;                                       ///-O-//// 
00299              Derecha();
00300              break;                                                  ///-O-////            
00301 ///////////////////////////////////////////////////////////////////////////////
00302 ///////////////////////////////////////////////////////////////////////////////
00303      //Frente
00304      case 50 : 
00305              seleccion_suma=0;                                       ///-O-//// 
00306              Frente();
00307              break;                                                  ///-O-////                                   
00308 ///////////////////////////////////////////////////////////////////////////////
00309 ///////////////////////////////////////////////////////////////////////////////
00310       //enfrente y enfrente derecha                                  //////////
00311      case 60 : 
00312              seleccion_suma=0;                                       ///-O-//// 
00313              Frente();
00314              break;                                                  ///-O-////                        
00315 ///////////////////////////////////////////////////////////////////////////////
00316 ///////////////////////////////////////////////////////////////////////////////
00317      //Frente_Izqu
00318      case 100 : 
00319              seleccion_suma=0;                                       ///-O-//// 
00320              Izquierda();
00321              break;                                                  ///-O-//// 
00322 ///////////////////////////////////////////////////////////////////////////////
00323 ///////////////////////////////////////////////////////////////////////////////
00324       //enfrente y enfrente izquierda                                 //////////
00325      case 150 : 
00326              seleccion_suma=0;                                       ///-O-//// 
00327 
00328              Frente();
00329              break;                                                  ///-O-////              
00330 ///////////////////////////////////////////////////////////////////////////////
00331 ///////////////////////////////////////////////////////////////////////////////
00332       //enfrente y enfrente izquierda  y enfrente derecha            //////////
00333      case 160 : 
00334              seleccion_suma=0;                                       ///-O-//// 
00335              Frente();
00336              break;                                                  ///-O-////
00337 ///////////////////////////////////////////////////////////////////////////////
00338 ///////////////////////////////////////////////////////////////////////////////
00339      //Izquierda
00340      case 500 : 
00341              seleccion_suma=0;
00342              Izquierda();                                       ///-O-//// 
00343              break;                                                  ///-O-////            
00344 
00345                                                                    ///-O-//// 
00346 ///////////////////////////////////////////////////////////////////////////////
00347                                                                               
00348                                                                               ///////////////////////////////////////////////////////////////////////////////
00349                                                                               ///////////////////////////////////////////////////////////////////////////////
00350                                                                               //////////// enfrente derecha y derecha
00351                                                                                    case 15 : 
00352                                                                                            seleccion_suma=0;                                       ///-O-//// 
00353                                                                                            Derecha();
00354                                                                                            break;                                                  ///-O-////                          
00355                                                                               ///////////////////////////////////////////////////////////////////////////////
00356                                                                               ///////////////////////////////////////////////////////////////////////////////
00357                                                                               //enfrente derecha y enfrente izquierda
00358                                                                               case  110: 
00359                                                                                            seleccion_suma=0;                                       ///-O-//// 
00360                                                                                            Frente();
00361                                                                                            break;                                                  ///-O-////              
00362                                                                               ///////////////////////////////////////////////////////////////////////////////
00363                                                                               ///////////////////////////////////////////////////////////////////////////////
00364                                                                               //enfrente izquierda e  izquierda
00365                                                                               case 600 : 
00366                                                                                            seleccion_suma=0;                                       ///-O-//// 
00367                                                                                            Izquierda();
00368                                                                                            break;                                                  ///-O-////              
00369                                                                               ///////////////////////////////////////////////////////////////////////////////
00370                                                                               ///////////////////////////////////////////////////////////////////////////////
00371                                                                                                                                                             ///////////////////////////////////////////////
00372                                                                                                                                                             ///////////////////////////////////////////////////////////////////////////////
00373                                                                                                                                                             //enfrente izquierda e izquierda y enfrente
00374                                                                                                                                                             case 650 : 
00375                                                                                                                                                                          seleccion_suma=0;                                       ///-O-//// 
00376                                                                                                                                                                          Frente();
00377                                                                                                                                                                          break;                                                  ///-O-////              
00378                                                                                                                                                            //////////////////////////////////////////////////
00379 ///////////////////////////////////////////////////////////////////////////////
00380       //derecha y enfrente                                                //////////
00381      case 55 : 
00382              seleccion_suma=0;                                       ///-O-//// 
00383              Frente();
00384              break;                                                  ///-O-////            
00385 ///////////////////////////////////////////////////////////////////////////////
00386 ///////////////////////////////////////////////////////////////////////////////
00387 
00388 
00389 ///////////////////////////////////////////////////////////////////////////////
00390       //izquierda  y enfrente                                         //////////
00391      case 550 : 
00392              seleccion_suma=0;                                       ///-O-//// 
00393              Frente();
00394              break;                                                  ///-O-////                
00395 
00396 
00397 
00398 
00399 
00400 
00401 ///////////////////////////////////////////////////////////////////////////////
00402 ///////////////////////////////////////////////////////////////////////////////
00403        //mas de una opcion                                           //////////
00404       default:                                                       //////////
00405              seleccion_suma=0;                                       ///-O-//// 
00406                                                                      //////////
00407            Paro();                                                   //////////
00408                                                                      //////////
00409              break;                                                  //////////
00410                                                                      //////////
00411             }//signo del switch                                      //////////
00412         }//signo del fin de funcion casos  
00413 void Frente()
00414 {
00415  m.drive_motor_1(128, 0);
00416  m.drive_motor_2(128, 0);
00417 }
00418 void Derecha() 
00419 {
00420  m.drive_motor_1(128, 0);
00421  m.drive_motor_2(128, 127);
00422 }
00423 void Izquierda()
00424 { 
00425  m.drive_motor_1(128, 127);
00426  m.drive_motor_2(128, 0);
00427 }
00428 void Atras()
00429 { 
00430  m.drive_motor_1(128, 127);
00431  m.drive_motor_2(128, 127);
00432 }
00433 void Paro()
00434 { 
00435  m.drive_motor_1(128, 64);
00436  m.drive_motor_2(128, 64);
00437 }