main.cpp

00001 #include "mbed.h"
00002 #include "Motor.h"
00003 #include "scolor_TCS3200.h"
00004 
00005 // Puerto de comunicacion Serial
00006 Serial CoolTerm(USBTX, USBRX);
00007 
00008 // Motores        m1step
00009 TraccionD Motores(PB_5, PB_3, PB_10, PB_4, 200, 3.75, 15.5) ;
00010 
00011 /// PWM OUTPUTS
00012 PwmOut Buzzer(D10); // LED1
00013 
00014 //  Temporizadores 
00015 Ticker MuestrearCOLOR;
00016 
00017 // SENSOR DE COLOR
00018 scolor_TCS3200 SENSOR_COLOR (PA_9, PC_7, PB_6, PA_7, PA_8); 
00019 
00020 
00021 // Lecturas Analogas de Joystick 
00022 AnalogIn JEjeX(A0);
00023 AnalogIn JEjeY(A1);
00024 
00025  
00026 // Salidas digitales
00027 DigitalOut LED(PA_5);
00028 
00029 
00030 //******************************************************************************
00031 ///         Declarar Variables Globales 
00032 
00033 
00034 /// Variables sensor de color
00035 long red;                        //Almacenan el Tiempo que dura el CicloUtil de la frecuencia
00036 long blue;                       //Generada por el Sensor TSC3200, al leer el componente
00037 long green;                      //R, G, B o Clear del color que tenga en Frente
00038 long clear;
00039 
00040 
00041 /// Constantes Sensor de Color
00042 #define CMD_rojo   0x01
00043 #define CMD_azul   0x02
00044 #define CMD_verde  0x03
00045 #define CMD_clear  0x04
00046 #define ColorNoIdentificado 0x00
00047 
00048 
00049 uint8_t color_identificado = ColorNoIdentificado;
00050 
00051 
00052 
00053 /// Varialbles Buzzer
00054 uint8_t duracion_tono = 1;       // Variable que almacena el Tiempo que es ESCUCHARÁ el Tono
00055 uint8_t tipo_tono = 1;           // Variable que almacena el Tono que se desee escuchar
00056 
00057 /// Constantes Buzzer
00058 #define TONO_DO 0x01 /// si tipo_tono == 0x01, se escuchará un DO
00059 #define TONO_RE 0x02 /// si tipo_tono == 0x02, se escuchará un RE
00060 #define TONO_MI 0x03 /// si tipo_tono == 0x03, se escuchará un MI
00061 #define TONO_SI 0x04 /// si tipo_tono == 0x04, se escuchará un SI
00062 
00063 #define DO 3.78 /// Duración del periodo en ms, que se pondrá en el Buzzer.period_ms() PARA ESCUCHAR UN DO
00064 #define RE 3.36 /// Duración del periodo en ms, que se pondrá en el Buzzer.period_ms() PARA ESCUCHAR UN RE
00065 #define MI 3.03 /// Duración del periodo en ms, que se pondrá en el Buzzer.period_ms() PARA ESCUCHAR UN MI
00066 #define SI 2.82 /// Duración del periodo en ms, que se pondrá en el Buzzer.period_ms() PARA ESCUCHAR UN SI 
00067 
00068 
00069 
00070 
00071 #define limite 0.727
00072 
00073 // variables de control y flujo de programa
00074 
00075 uint8_t   programa_ejecutar = 0; // Variable que almacena la ORDEN (Telemetria ó Telecomando)
00076                                  // enviada desde el CoolTerm
00077 uint8_t   coolterm_data;         // Se almacenan Todos los Caracteres recividos por el CoolTerm.
00078 
00079 
00080 // Constantes de velocidad
00081 
00082 #define VelAlta  300
00083 #define VelMedia 200
00084 #define VelBaja 100
00085 
00086 
00087 //******************************************************************************
00088 // COMANDOS
00089 
00090 #define iniciar_telemetria      0xFE
00091 #define iniciar_telecomando     0xFF
00092 
00093 #define telemetria_1            0x01  //
00094 #define telemcomando_1          0x01
00095 
00096 #define C_LeerColor  0x00
00097 #define C_Sonido1    0x01
00098 #define C_Sonido2    0x02
00099 #define C_Sonido3    0x03
00100 #define C_Sonido4    0x04
00101 #define C_Adelante   0x05
00102 #define C_Atras      0x06
00103 #define C_Izquierda  0x07
00104 #define C_Derecha    0x08
00105 #define C_Velocidad  0x09
00106 #define C_Joistck    0x0A
00107 int comando_joystick = 0;
00108 
00109 
00110 // variables y constantes del Joystick
00111 
00112 uint8_t estado_x; 
00113 uint8_t estado_y;
00114 #define E_Derecha 1
00115 #define E_Izquier 0
00116 #define E_Adelante 1
00117 #define E_Atras 0
00118 #define E_Centro 3
00119 #define Lim_JKIn 150
00120 #define Lim_JKSup 180
00121 
00122 //****************************************************************************
00123 //                      Prototipo de funciones
00124 
00125 void ReadPort(void);                // Lee el puerto Serial
00126 void MainConfig(void);              // Configuracion Inicial de los Perifericos del uC
00127 void Buzzer_Tone(uint8_t tipo_tono, uint8_t duracion_tono); // configura el tono y la duracion escuchada a travez del Buzzer
00128 void leer_color(void);              // funcion que retorna los componentes
00129                                     // RGB y Clear del color leido
00130 void leer_Joystick();               // Ejerce control sobre el Movimiento del carro desde el Joistick  
00131 
00132 //****************************************************************************
00133 //     
00134 #define valorInicial 0xAA
00135 uint8_t n_interrupcion = 0;
00136 uint8_t ComandoRecivido = valorInicial, Parametro = valorInicial;
00137 
00138 void ReadPort()
00139 {
00140    
00141     //if(CoolTerm.writable()) CoolTerm.abort_write(); // ELIMINA LO QUE ESTEMOS ESCRIBIENDO AL COOLTERM
00142     
00143     if (n_interrupcion == 0) { coolterm_data = CoolTerm.getc(); }
00144     if (n_interrupcion == 1) { ComandoRecivido = CoolTerm.getc(); n_interrupcion++; }
00145     if (n_interrupcion == 2) { Parametro       = CoolTerm.getc(); n_interrupcion = 0;}
00146     if (coolterm_data == iniciar_telecomando) { programa_ejecutar = iniciar_telecomando; n_interrupcion++; }
00147     if (ComandoRecivido == 0x0A) comando_joystick = 0; // Da valores diferentes al inicial de cada variable
00148     
00149 }
00150 
00151 ///******************************************+
00152 
00153 
00154 void leer_Joystick()
00155 {   
00156 
00157 /// Variables Joystick
00158 float EjeX;
00159 float EjeY;
00160 float Vx;
00161 float Vy;
00162 
00163  while (comando_joystick == 1) // 
00164  {
00165     
00166     EjeX = JEjeX.read();
00167     Vx = EjeX * 3300; 
00168     wait (0.1);
00169     EjeY = JEjeY.read();
00170     Vy = EjeY * 3300;
00171     
00172 //    CoolTerm.printf ("ejex: %f    ejey: %f  Vx: %f  Vy: %f \n   ", EjeX, EjeY, Vx, Vy);
00173 
00174     if(int(Vx/10) > Lim_JKIn && int(Vx/10) < Lim_JKSup) {estado_x = E_Centro; }//(CoolTerm.printf ("Estado X Centro \n"); }          
00175     if(int(Vy/10) > Lim_JKIn && int(Vy/10) < Lim_JKSup) {estado_y = E_Centro; }//CoolTerm.printf ("Estado Y Centro \n"); }
00176     
00177     if(int(Vx/10) > Lim_JKSup && estado_y == E_Centro){ estado_x = E_Izquier; }// CoolTerm.printf ("Estado X Izquierda\n"); }
00178     if(int(Vy/10) > Lim_JKSup && estado_x == E_Centro){ estado_y = E_Atras; }//  CoolTerm.printf ("Estado Y Adelante\n"); }
00179     
00180     if(int(Vx/10) < Lim_JKIn && estado_y == E_Centro){ estado_x = E_Derecha; } //CoolTerm.printf ("Estado X Derecha\n"); }
00181     if(int(Vy/10) < Lim_JKIn && estado_x == E_Centro){ estado_y = E_Adelante; } //CoolTerm.printf ("Estado Y Atras\n"); }
00182  
00183  
00184        // CoolTerm.printf ("\n\n X = %d         Y = %d \n",estado_x , estado_y); 
00185       // wait(2);      
00186     // Combinacion de estados para STOP
00187     if( estado_x == E_Centro && estado_y == E_Centro){ Motores.Stop(); CoolTerm.printf ("MOTORES STOP\n"); }
00188     
00189     // Combinacion de estados para ADELANTE
00190     if(estado_x == E_Centro && estado_y == E_Adelante) { Motores.Back(); Motores.Run(1);CoolTerm.printf ("MOTORES BACK\n"); } 
00191     
00192     // Combinacion de estados para ATRAS
00193     if(estado_x == E_Centro && estado_y == E_Atras) { Motores.Forward(); Motores.Run(1);CoolTerm.printf ("MOTORES FORWARD\n"); } 
00194     
00195     
00196     // Combinacion de estados para DERECHA
00197     if(estado_y == E_Centro && estado_x == E_Derecha) {  Motores.Giro(15, false); Motores.Run(1); CoolTerm.printf ("MOTORES DERECHA\n"); } 
00198     
00199     // Combinacion de estados para IZQUIERDA
00200     if(estado_y == E_Centro && estado_x == E_Izquier) { Motores.Giro(15, true); Motores.Run(1); CoolTerm.printf ("MOTORES IZQUIERDA\n"); } 
00201     //wait(1.5);
00202 }
00203     comando_joystick = 0;
00204 
00205 }
00206 
00207 
00208 int main() {
00209 
00210     Buzzer.write(0);        ///configura el ciclo util
00211     Motores.StepFreq(VelMedia);
00212     CoolTerm.attach(&ReadPort, Serial::RxIrq);    ////  se Habilita la interrupcion serial o recepcion de datos
00213     //MuestrearCOLOR.attach(&leer_color, 0.6);
00214     
00215     //CoolTerm.printf("Hello World, System Run !!\n"); // mensaje inicial
00216     //leer_Joystick ();
00217       
00218     while(1) 
00219     {
00220         
00221       
00222        /// Espera hasta recivir OxFF
00223         while(programa_ejecutar != iniciar_telecomando)wait(0.01);
00224          
00225        /// Espera hasta que reciva algo diferente de 0xAA 
00226         while(ComandoRecivido == valorInicial)wait(0.01);
00227         
00228        /// Espera hasta que reciva algo diferente de 0xAA 
00229         while(Parametro == valorInicial)wait(0.01);
00230         
00231         
00232         ////  Desactivamos la interrupcion serial o recepcion de datos PORQUE NO NECESITAMOS recibir mas datos por AHORA
00233          CoolTerm.attach(NULL, Serial::RxIrq);
00234             
00235         switch(ComandoRecivido)
00236         {
00237             
00238             //case C_LeerColor:   // Ejecutamos la Funcion LeerColor();
00239               //                  leer_color();
00240                     //break;
00241             case C_Sonido1:     //CoolTerm.printf("SONIDO 1\n");
00242                                 duracion_tono = Parametro;  // lo almacenamos en:  duracion_tono
00243                                 tipo_tono     = TONO_DO;
00244                                 Buzzer_Tone(tipo_tono, duracion_tono);         
00245                     break;
00246             case C_Sonido2:     //CoolTerm.printf("SONIDO 2\n");
00247                                 duracion_tono = Parametro;  // lo almacenamos en:  duracion_tono
00248                                 tipo_tono     = TONO_RE;
00249                                 Buzzer_Tone(tipo_tono, duracion_tono);         
00250                     break;
00251             case C_Sonido3:     //CoolTerm.printf("SONIDO 3\n");
00252                                 duracion_tono = Parametro;  // lo almacenamos en:  duracion_tono
00253                                 tipo_tono     = TONO_MI;
00254                                 Buzzer_Tone(tipo_tono, duracion_tono);         
00255                     break;
00256             case C_Sonido4:     //CoolTerm.printf("SONIDO 4\n");
00257                                 duracion_tono = Parametro;  // lo almacenamos en:  duracion_tono
00258                                 tipo_tono     = TONO_SI;
00259                                 Buzzer_Tone(tipo_tono, duracion_tono);         
00260                     break;
00261             case C_Adelante:    Motores.Forward(); Motores.RunRound(Parametro);
00262                     break;
00263             case C_Atras:       Motores.Back(); Motores.RunRound(Parametro);
00264                     break;
00265             case C_Izquierda:   Motores.Giro(65, true);
00266                     break;
00267             case C_Derecha:     Motores.Giro(65, false);
00268                     break;
00269             case C_Velocidad:   if(Parametro == 0x01)Motores.StepFreq(VelBaja);
00270                                 if(Parametro == 0x02)Motores.StepFreq(VelMedia);
00271                                 if(Parametro == 0x03)Motores.StepFreq(VelAlta);                                
00272                     break;
00273             case C_Joistck:     comando_joystick = 1; leer_Joystick();     
00274                     break;
00275             default: break;    
00276                     
00277             
00278         }
00279             
00280             //CoolTerm.printf("ProgramaFinalizado!!\n"); 
00281             
00282          // Re inicializamos nuestras variables de control a sus valores iniciales
00283             // Para no seguir entrando a las sentencias IF
00284             programa_ejecutar = 0; coolterm_data = 0; 
00285             ComandoRecivido = valorInicial; Parametro  = valorInicial;
00286             ////  HABILITAMOS NUEVAMENTE la interrupcion serial o recepcion de datos
00287             CoolTerm.attach(&ReadPort, Serial::RxIrq);  
00288         
00289     }
00290     
00291 }
00292 
00293 
00294 
00295 void Buzzer_Tone(uint8_t tipo_tono, uint8_t duracion_tono)
00296     {
00297         
00298             
00299                 switch (tipo_tono)
00300             {
00301                     
00302                     case TONO_DO:       Buzzer.period_ms(DO);
00303                                        //CoolTerm.printf("Tono Seleccionado DO!!\n");                                        
00304                                         
00305                                         break; // salimos del SWITCH
00306                                         
00307                     case TONO_RE:       Buzzer.period_ms(RE);
00308                                         //CoolTerm.printf("Tono Seleccionado RE!!\n");
00309                                         
00310                                         break; // salimos del SWITCH
00311                                         
00312                     case TONO_MI:       Buzzer.period_ms(MI);
00313                                         //CoolTerm.printf("Tono Seleccionado MI!!\n");
00314                                         
00315                                         break; // salimos del SWITCH
00316                                         
00317                     case TONO_SI:       Buzzer.period_ms(SI);
00318                                         //CoolTerm.printf("Tono Seleccionado SI!!\n");
00319                                         
00320                                         break; // salimos del SWITCH
00321                                                             
00322                     // si no fue ninguno de los valores anteriores entonces:                    
00323                     default:            //CoolTerm.printf("teleComando desconocido, inicie nuevamente !!\n");
00324                                         
00325                                         break; // salimos del SWITCH
00326 
00327             }
00328                 // COMO EL CICLO UTIL DEL BUZZER ESTABA EN 0, POR LO CUAL NO SONABA
00329                 // SE PONE AL 50% DEL PERIODO
00330                 Buzzer.write(0.5);
00331                 // SE ESPERA DURANTE EN TIEMPO INGRESADO (EN SEGUNDOS )
00332                 wait(duracion_tono);
00333                 
00334                 // Se Reinicializa el Periodo y el Ciclo útil de la señal PWM 
00335                 // que va al Buzzer
00336                 Buzzer.period_ms(1);
00337                 Buzzer.write(0);
00338               
00339     
00340     
00341     }
00342 
00343 
00344 
00345 void leer_color()
00346     {
00347     
00348         red    = SENSOR_COLOR.ReadRed(); // OBTENEMOS EL TIEMPO DEL CICLO UTIL DE LA FRECUENCIA DE SALIDA 
00349         green  = SENSOR_COLOR.ReadGreen();
00350         blue   = SENSOR_COLOR.ReadBlue();
00351         clear  = SENSOR_COLOR.ReadClear();
00352         
00353         //printf("RED: %5d     GREEN: %5d     BLUE: %5d     CLEAR: %5d    \n ", red, green, blue, clear);
00354          
00355         red     *= 2;   // Calculamos EL PERIODO de la frecuencia generada por la lectura del fotodiodo rojo         
00356         blue    *= 2;   // Calculamos EL PERIODO de la frecuencia generada por la lectura del fotodiodo rojo
00357         green   *= 2;   // Calculamos EL PERIODO  de la frecuencia generada por la lectura del fotodiodo rojo
00358         clear   *= 2;   // Calculamos EL PERIODO  de la frecuencia generada por la lectura del fotodiodo rojo
00359         
00360         //printf("RED: %5d     GREEN: %5d     BLUE: %5d     CLEAR: %5d    \n ", red, green, blue, clear);
00361        
00362         
00363        //////////////////////////////////////////////////////////////     
00364        ////         identificar azul
00365        
00366        
00367        if(red <=42 && red >=24)
00368         {
00369             if(green >= 20 && green <= 28 )
00370             {
00371                 if(blue >= 10 && blue <= 16)
00372                 {
00373                         color_identificado = CMD_azul;
00374                         CoolTerm.putc( iniciar_telemetria);
00375                         CoolTerm.putc( CMD_azul ); 
00376                         //Buzzer.period_ms(DO);
00377                         //Buzzer.write(0.5);
00378                         //wait(4);
00379                         //Buzzer.write(0);
00380                        
00381                 }
00382             }
00383          }   
00384                  
00385         
00386         
00387         
00388         /////////////////////////////////////////////////////////////
00389         ///         identificar rojo
00390         if(red <= 12 )
00391         {
00392             if(green >= 10 && green <= 28 ) 
00393                 {
00394                     if(blue >= 18 && blue <= 24)
00395                     {
00396                             color_identificado = CMD_rojo;
00397                             CoolTerm.putc( iniciar_telemetria);
00398                             CoolTerm.putc( CMD_rojo ); 
00399                             //Buzzer.period_ms(RE);
00400                             //Buzzer.write(0.5);  //PERIODO UTIL
00401                             //wait(4);            //TIEMPO ACTIVO DEL BUZZER
00402                             //Buzzer.write(0.0);
00403                     }
00404                 }
00405                 
00406             if(green < 10 && green >= 6 )
00407                 {
00408                     if(blue <= 12  )
00409                     {
00410                             color_identificado = CMD_clear;
00411                             CoolTerm.putc( iniciar_telemetria);
00412                             CoolTerm.putc( CMD_clear );
00413                             //Buzzer.period_ms(MI);
00414                             //Buzzer.write(0.5);
00415                             //wait(4);
00416                             //Buzzer.write(0);
00417                     }
00418                     
00419                 }
00420             
00421          }   
00422          
00423          
00424              if(green >= 36 && green <= 44 )
00425             {
00426                 if(red >= 40 && red <= 50 )
00427             
00428                 {
00429                         color_identificado = CMD_verde;
00430                         CoolTerm.putc( iniciar_telemetria);
00431                         CoolTerm.putc( CMD_verde );
00432                             //Buzzer.period_ms(SI);
00433                             //Buzzer.write(0.5);
00434                             //wait(4);
00435                             //Buzzer.write(0); 
00436                         
00437                     
00438                 }
00439             } 
00440             
00441             if  (color_identificado == ColorNoIdentificado)
00442             {
00443                 
00444                  
00445                         CoolTerm.putc( iniciar_telemetria);
00446                         CoolTerm.putc( ColorNoIdentificado);
00447                             //Buzzer.period_ms(10);
00448                             //Buzzer.write(0.5);
00449                             //wait(4);
00450                             //Buzzer.write(0); 
00451                         
00452                 
00453             }
00454                 
00455             color_identificado = ColorNoIdentificado;
00456         }
00457 
00458