File content as of revision 4:707923a648dc:

#include "mbed.h"
#include "Servo.h"
#include "TCS3200.h"

TCS3200 color(PF_6, PF_7, PF_8, PF_9, PA_0);  //Create a TCS3200 object 
 //            S0    S1    S2     S3     OUT
 
 Servo Servo2(PD_12);
 Servo Servo3(PD_13);
 Servo Servo4(PD_14);
 Servo Servo5(PD_15);
 
void move_1();
void move_2();
void move_3();
void move_4();

DigitalOut myled1(LED1);
DigitalOut myled2(LED2);
DigitalOut myled3(LED3);

int colour(); 
 
int main(int argc, char* argv[])
{  

//Set the scaling factor to 100%
 //color.SetMode(TCS3200::SCALE_100);
   myled1 = 0; 
   myled2 = 0;
   myled3 = 0;
 
 
  Servo5.Enable(1500,20000); //turn
  Servo2.Enable(1500,20000); // forward
  Servo3.Enable(1500,20000);
  Servo4.Enable(1500,20000); // backward
  
  //
 
  while(1) 
  {
      //1
       move_1();
       move_2();
       move_3();
       move_4();
       if(colour()== 1)
       {
          printf("RED\n"); 
               myled1 = 0; 
               myled2 = 0;
               myled3 = 1;
               
               
       }
       else
       {
           if(colour() == 2)
           {
              printf("GREEN\n"); 
               myled1 = 1; 
               myled2 = 0;
               myled3 = 0;
           }
           else
           {
              printf("BLUE\n");
               myled1 = 0; 
               myled2 = 1;
               myled3 = 0;
           }   
       }
    
      
  }
  
  return 0;
  }
  
void move_1()
{
    for (int pos = 500; pos < 2000; pos += 25) 
      {
          Servo4.SetPosition(pos);  
          wait_ms(20);
      }
      
      for (int pos = 2000; pos > 500; pos -= 25) 
      {
          Servo4.SetPosition(pos); 
          wait_ms(20); 
      } 
}
void move_2()
{
    for (int pos = 500; pos < 2000; pos += 25) 
      {
          Servo2.SetPosition(pos);  
          wait_ms(20);
      }
      
      for (int pos = 2000; pos > 500; pos -= 25) 
      {
          Servo2.SetPosition(pos); 
          wait_ms(20); 
      } 
}
void move_3()
{
    for (int pos = 500; pos < 2000; pos += 25) 
      {
          Servo3.SetPosition(pos);  
          wait_ms(20);
      }
      
      for (int pos = 2000; pos > 500; pos -= 25) 
      {
          Servo3.SetPosition(pos); 
          wait_ms(20); 
      } 
}
void move_4()
{
    for (int pos = 500; pos < 2000; pos += 25) 
      {
          Servo5.SetPosition(pos);  
          wait_ms(20);
      }
      
      for (int pos = 2000; pos > 500; pos -= 25) 
      {
          Servo5.SetPosition(pos); 
          wait_ms(20); 
      } 
}

int colour()
{
 color.SetMode(TCS3200::SCALE_100);
 long red, green, blue, clear;
 int r = 0,g = 0, b =0 ;
 for(int c=0 ; c < 10; c++)
{
    
    red = color.ReadRed();
    green = color.ReadGreen();
    blue = color.ReadBlue();
    clear = color.ReadClear();
        
    if((red < blue) && (red < green))
    {
    r++;        // red
    }
    if((green < blue) && (green < red))
    {
             g++;//green
    }
    if(blue < red && blue < green)
    {
             b++;//blue 
    }
         
}
   
   if((r > b) && (r > g))
   {
     return 1;                  //red
   }
   else
   {
  if(g > b && g > r)
   {
     return 2;                 //green
   }
   else
   {
                      //blue
      return 3;       
   }         


   }

}