ENTREGA

Dependencies:   mbed

Fork of 01-04EntregaPrimerCorte by ferney alberto beltran molina

Embed: (wiki syntax)

« Back to documentation index

Show/hide line numbers color.cpp Source File

color.cpp

00001 #include "mover.h"
00002 #include "main.h"
00003 #include "mbed.h"
00004 #include "math.h"
00005 
00006 DigitalOut s0(PA_3);
00007 DigitalOut s1(PA_2);
00008 DigitalOut s2(PA_10);
00009 DigitalOut s3 (PB_3);
00010 //DigitalOut led (PB_4);
00011 DigitalIn out(PB_5);
00012 Timer tiempo;
00013 Serial pc(USBTX, USBRX);
00014 unsigned int lectura(void);
00015 
00016 void inicio(){
00017         iniciando_();}
00018 void sensor_color(uint8_t motores, uint8_t posicion){
00019    // inicio();
00020 unsigned int rojo=0;
00021     unsigned int verde=0;
00022     unsigned int azul=0;
00023     unsigned int clean=0;
00024     //led=0;
00025     if(motores==0){
00026         
00027         s0=1;
00028         s1=1;
00029     //led=1;
00030         s2=0;s3=0;
00031         rojo=lectura();
00032         s2=0;s3=1;
00033         azul=lectura();
00034         s2=1;s3=1;
00035         verde=lectura();
00036         s2=1;s3=0;
00037         clean=lectura();
00038         
00039        
00040         //wait(1);
00041        if (((verde>=18)&&(verde<=22))&&((azul>=07)&&(azul<=11))&&((rojo>=26)&&(rojo<=30))&&((clean>=4)&&(clean<=6))){
00042             pc.printf("\nazul");
00043             adelante();
00044         
00045             }
00046         else 
00047         if(((verde>=13)&&(verde<=17))&&((azul>=12)&&(azul<=15))&&((rojo>=21)&&(rojo<=24))&&((clean>=4)&&(clean<=6))){
00048            pc.printf("\nverde");
00049            atras();
00050            
00051             }
00052             else 
00053             if(((verde>=19)&&(verde<=22))&&((azul>=12)&&(azul<=15))&&((rojo>=4)&&(rojo<=8))&&((clean>=3)&&(clean<=4))){
00054          pc.printf("\nrojo");
00055          derecha();
00056          
00057          
00058     }
00059         else 
00060         if(((verde>=4)&&(verde<=7))&&((azul>=7)&&(azul<=10))&&((rojo>=4)&&(rojo<=7))&&((clean>=3)&&(clean<=6))){
00061          pc.printf("\namarillo");
00062          izquierda();
00063          
00064     }else
00065     if(((verde>=02)&&(verde<=05))&&((azul>=2)&&(azul<=5))&&((rojo>=2)&&(rojo<=5))&&((clean>=2)&&(clean<=5))){
00066          pc.printf("\nblanco");
00067         inicio();
00068         }
00069     
00070         else 
00071          pc.printf("\n%d %d %d %d \n color no definido\n",verde,azul,rojo,clean); 
00072  
00073 
00074     }
00075     else     
00076         pc.printf("\ncomando erroneo\n");
00077         
00078     }
00079   
00080     
00081     void adelante(){
00082         pc.printf("\n           ******mover a el frente.****** \n\n");
00083         adelante_();
00084         }
00085     void atras(){
00086        pc.printf("\n           ******mover atras.****** \n\n");
00087         atras_();
00088         }   
00089     void derecha(){
00090         pc.printf("\n           ******mover derecha.****** \n\n");
00091         derecha_();
00092         
00093         
00094         }
00095     void izquierda(){
00096         pc.printf("\n           ******mover izquiera.****** \n\n");
00097         izquierda_();
00098         } 
00099 unsigned int lectura (void){
00100 unsigned int inicio=0, final=0, resultado=0;    
00101     tiempo.start();
00102     while(out){}
00103     while(!out){}
00104     while(out){}
00105     inicio=tiempo.read_us();
00106     while(!out){}
00107     final=tiempo.read_us();
00108     resultado=(final-inicio); 
00109     tiempo.reset();
00110     return(resultado); 
00111 }