main.cpp

00001 #include "mbed.h"
00002 #include "Servo.h"
00003 #include "TCS3200.h"
00004 
00005 TCS3200 color(PF_6, PF_7, PF_8, PF_9, PA_0);  //Create a TCS3200 object 
00006  //            S0    S1    S2     S3     OUT
00007  
00008  Servo Servo2(PD_12);
00009  Servo Servo3(PD_13);
00010  Servo Servo4(PD_14);
00011  Servo Servo5(PD_15);
00012  
00013 void move_1();
00014 void move_2();
00015 void move_3();
00016 void move_4();
00017 
00018 DigitalOut myled1(LED1);
00019 DigitalOut myled2(LED2);
00020 DigitalOut myled3(LED3);
00021 
00022 int colour(); 
00023  
00024 int main(int argc, char* argv[])
00025 {  
00026 
00027 //Set the scaling factor to 100%
00028  //color.SetMode(TCS3200::SCALE_100);
00029    myled1 = 0; 
00030    myled2 = 0;
00031    myled3 = 0;
00032  
00033  
00034   Servo5.Enable(1500,20000); //turn
00035   Servo2.Enable(1500,20000); // forward
00036   Servo3.Enable(1500,20000);
00037   Servo4.Enable(1500,20000); // backward
00038   
00039   //
00040  
00041   while(1) 
00042   {
00043       //1
00044        move_1();
00045        move_2();
00046        move_3();
00047        move_4();
00048        if(colour()== 1)
00049        {
00050           printf("RED\n"); 
00051                myled1 = 0; 
00052                myled2 = 0;
00053                myled3 = 1;
00054                
00055                
00056        }
00057        else
00058        {
00059            if(colour() == 2)
00060            {
00061               printf("GREEN\n"); 
00062                myled1 = 1; 
00063                myled2 = 0;
00064                myled3 = 0;
00065            }
00066            else
00067            {
00068               printf("BLUE\n");
00069                myled1 = 0; 
00070                myled2 = 1;
00071                myled3 = 0;
00072            }   
00073        }
00074     
00075       
00076   }
00077   
00078   return 0;
00079   }
00080   
00081 void move_1()
00082 {
00083     for (int pos = 500; pos < 2000; pos += 25) 
00084       {
00085           Servo4.SetPosition(pos);  
00086           wait_ms(20);
00087       }
00088       
00089       for (int pos = 2000; pos > 500; pos -= 25) 
00090       {
00091           Servo4.SetPosition(pos); 
00092           wait_ms(20); 
00093       } 
00094 }
00095 void move_2()
00096 {
00097     for (int pos = 500; pos < 2000; pos += 25) 
00098       {
00099           Servo2.SetPosition(pos);  
00100           wait_ms(20);
00101       }
00102       
00103       for (int pos = 2000; pos > 500; pos -= 25) 
00104       {
00105           Servo2.SetPosition(pos); 
00106           wait_ms(20); 
00107       } 
00108 }
00109 void move_3()
00110 {
00111     for (int pos = 500; pos < 2000; pos += 25) 
00112       {
00113           Servo3.SetPosition(pos);  
00114           wait_ms(20);
00115       }
00116       
00117       for (int pos = 2000; pos > 500; pos -= 25) 
00118       {
00119           Servo3.SetPosition(pos); 
00120           wait_ms(20); 
00121       } 
00122 }
00123 void move_4()
00124 {
00125     for (int pos = 500; pos < 2000; pos += 25) 
00126       {
00127           Servo5.SetPosition(pos);  
00128           wait_ms(20);
00129       }
00130       
00131       for (int pos = 2000; pos > 500; pos -= 25) 
00132       {
00133           Servo5.SetPosition(pos); 
00134           wait_ms(20); 
00135       } 
00136 }
00137 
00138 int colour()
00139 {
00140  color.SetMode(TCS3200::SCALE_100);
00141  long red, green, blue, clear;
00142  int r = 0,g = 0, b =0 ;
00143  for(int c=0 ; c < 10; c++)
00144 {
00145     
00146     red = color.ReadRed();
00147     green = color.ReadGreen();
00148     blue = color.ReadBlue();
00149     clear = color.ReadClear();
00150         
00151     if((red < blue) && (red < green))
00152     {
00153     r++;        // red
00154     }
00155     if((green < blue) && (green < red))
00156     {
00157              g++;//green
00158     }
00159     if(blue < red && blue < green)
00160     {
00161              b++;//blue 
00162     }
00163          
00164 }
00165    
00166    if((r > b) && (r > g))
00167    {
00168      return 1;                  //red
00169    }
00170    else
00171    {
00172   if(g > b && g > r)
00173    {
00174      return 2;                 //green
00175    }
00176    else
00177    {
00178                       //blue
00179       return 3;       
00180    }         
00181 
00182 
00183    }
00184 
00185 }
00186   
00187