Diff: main.cpp

Revision:

4:707923a648dc

Parent:

3:9640475cdfbe

--- a/main.cpp	Thu Dec 07 15:01:47 2017 +0000
+++ b/main.cpp	Fri Jan 05 16:22:50 2018 +0000
@@ -1,62 +1,187 @@
 #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[])
 {  
- Servo Servo1(PD_13);
- Servo Servo2(PD_12);
- Servo Servo3(PD_14);
- Servo Servo4(PD_15);
+
+//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
+  
+  //
  
-  Servo1.Enable(1500,20000);
-  Servo2.Enable(1500,20000);
-  Servo3.Enable(1500,20000);
- // Servo4.Enable(1500,20000);
- 
-  while(1) {
-      for (int pos = 500; pos < 2000; pos += 25) 
+  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) 
       {
-          Servo1.SetPosition(pos);  
+          Servo4.SetPosition(pos);  
           wait_ms(20);
       }
-      for (int pos = 2000; pos > 500; pos -= 25) {
-          Servo1.SetPosition(pos); 
+      
+      for (int pos = 2000; pos > 500; pos -= 25) 
+      {
+          Servo4.SetPosition(pos); 
           wait_ms(20); 
-      }
-      
-      //================================
-      for (int pos = 500; pos < 2000; pos += 25) 
+      } 
+}
+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) {
+      
+      for (int pos = 2000; pos > 500; pos -= 25) 
+      {
           Servo2.SetPosition(pos); 
           wait_ms(20); 
-      }
-      
-      //============================
-       for (int pos = 500; pos < 2000; pos += 25) 
+      } 
+}
+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) {
+      
+      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 = 0; pos < 2600; pos += 25) 
+      for (int pos = 2000; pos > 500; pos -= 25) 
       {
-          Servo4.SetPosition(pos);  
-          wait_ms(20);
-      }
-      for (int pos = 2600; pos > 0; pos -= 25) {
-         Servo4.SetPosition(pos); 
+          Servo5.SetPosition(pos); 
           wait_ms(20); 
-      }
-      
-      //===========================
-  }
-  return 0;
-  }
\ No newline at end of file
+      } 
+}
+
+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;       
+   }         
+
+
+   }
+
+}
+  
+  
\ No newline at end of file