SHARE
TWEET

Arduino Project

razvanth21 May 24th, 2018 (edited) 14 Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
  1.  /*
  2.  * Skittles Color Sorter using Arduino UNO R3.
  3.  * S0, S1, S2, S3 - PINs used by the color sensor.
  4.  * Colours return IDs: 1 - red, 2 - green, 3 - yellow, 4 - orange, 5 - blue
  5.  */
  6.  
  7. #include <Servo.h>
  8.  
  9.  /*
  10.  * PIN-urile pentru senzorul de lumina.
  11.  */
  12. #define S0 3
  13. #define S1 4
  14. #define S2 5
  15. #define S3 6
  16.  
  17. #define sensorOut 7
  18.  
  19.  /*
  20.  * Coduri pentru culori.
  21.  */
  22. #define RED     0
  23. #define GREEN   1
  24. #define YELLOW  2
  25. #define BLUE    3
  26.  
  27. #define START_POS   100 // 100°
  28. #define SENSOR_POS  47  // 47°
  29. #define END_POS     29  // 29°
  30.  
  31. // #define DEBUG
  32.  
  33. Servo topServo;     // motor sus
  34. Servo bottomServo;  // motor jos
  35.  
  36. int frequency = 0, color = 0;
  37. int R, G, B;
  38.  
  39. unsigned short colors[4][6] = {
  40.   // R        G        B
  41.   { 75, 82, 90, 108, 78, 91 },  // RED
  42.   { 82, 89, 88, 96, 81, 89 },   // GREEN
  43.   { 60, 70, 78, 86, 75, 84 },   // YELLOW
  44.   { 81, 90, 87, 97, 70, 80 }    // BLUE
  45. };
  46.  
  47. /*unsigned short colorsMap[4][3] = {
  48.   // R, G, B
  49.   {76, 100, 85}, // RED
  50.   {76, 84, 79}, // GREEN
  51.   {69, 84, 82}, // YELLOW
  52.   {85, 92, 75}  // BLUE
  53. };*/
  54.  
  55. void setup() {
  56.   pinMode(S0, OUTPUT);
  57.   pinMode(S1, OUTPUT);
  58.   pinMode(S2, OUTPUT);
  59.   pinMode(S3, OUTPUT);
  60.   pinMode(sensorOut, INPUT);
  61.  
  62.   // Setting frequency-scaling to 20%
  63.   digitalWrite(S0, HIGH);
  64.   digitalWrite(S1, LOW);
  65.  
  66.   topServo.attach(8);     // PIN #8
  67.   bottomServo.attach(9);  // PIN #9
  68.  
  69.   bottomServo.write(0);
  70.  
  71.   Serial.begin(9600);
  72. }
  73.  
  74. void loop() {
  75.   topServo.write(START_POS); // seteaza servomotorul sus la 115° (ia bomboana)
  76.  
  77.   delay(500); // asteapta 500ms
  78.  
  79.   for (int i = START_POS; i >= SENSOR_POS; i--) { // muta motorul sub senzor
  80.     topServo.write(i);
  81.     delay(6);
  82.   }
  83.  
  84.   delay(500); // 500ms
  85.  
  86.   getRGB(&R, &G, &B); // afla culoarea
  87.   color = mapToColour(R, G, B); // afla culoarea (getColor())
  88.  
  89.   delay(10); // delay 10ms
  90.  
  91.   switch (color) {
  92.     case RED: {
  93.       Serial.println("RED");
  94.       bottomServo.write(0); // 0°
  95.       break;
  96.     }
  97.     case GREEN: {
  98.       Serial.println("GREEN");
  99.       bottomServo.write(25); // 25°
  100.       break;
  101.     }
  102.     case YELLOW: {
  103.       Serial.println("YELLOW");
  104.       bottomServo.write(50); // 50°
  105.       break;
  106.     }
  107.     case BLUE: {
  108.       Serial.println("BLUE");
  109.       bottomServo.write(75); // 75°
  110.       break;
  111.     }
  112.   }
  113.  
  114.   delay(300); // delay 300ms
  115.  
  116.   for (int i = SENSOR_POS; i >= END_POS; i--) {
  117.     topServo.write(i);
  118.     delay(6);
  119.   }
  120.  
  121.   delay(200); // 200ms
  122.  
  123.   for (int i = END_POS; i <= START_POS; i++) {
  124.     topServo.write(i); // revine la pozitia initiala de 115°
  125.     delay(4);
  126.   }
  127.  
  128.   color = 0; // reseteaza variabila de culoare
  129. }
  130.  
  131. void getRGB(int *R, int *G, int *B)
  132. {
  133.   // Setting red filtered photodiodes to be read
  134.   digitalWrite(S2, LOW);
  135.   digitalWrite(S3, LOW);
  136.  
  137.   frequency = pulseIn(sensorOut, LOW);
  138.   *R = frequency;
  139.   // frequency = map(frequency, 165, 180, 255, 0);
  140.  
  141.   #if defined DEBUG
  142.     Serial.print("R= ");//printing name
  143.     Serial.print(frequency);//printing RED color frequency
  144.     Serial.print("  ");
  145.   #endif
  146.   delay(100);
  147.  
  148.   // Setting Green filtered photodiodes to be read
  149.   digitalWrite(S2, HIGH);
  150.   digitalWrite(S3, HIGH);
  151.  
  152.   frequency = pulseIn(sensorOut, LOW);
  153.   *G = frequency;
  154.   // frequency = map(frequency, 195, 210, 255, 0);
  155.  
  156.   #if defined DEBUG
  157.     Serial.print("G= ");//printing name
  158.     Serial.print(frequency);//printing RED color frequency
  159.     Serial.print("  ");
  160.   #endif
  161.  
  162.   delay(100);
  163.  
  164.   digitalWrite(S2, LOW);
  165.   digitalWrite(S3, HIGH);
  166.  
  167.   frequency = pulseIn(sensorOut, LOW);
  168.   *B = frequency;
  169.   // frequency = map(frequency, 30, 90, 255, 0);
  170.  
  171.   #if defined DEBUG
  172.     Serial.print("B= ");//printing name
  173.     Serial.print(frequency);//printing RED color frequency
  174.     Serial.println("  ");
  175.   #endif
  176.   delay(100);
  177. }
  178.  
  179. double getDist(double from_r, double from_g, double from_b, double r, double g, double b)
  180. {
  181.   return sqrt((r - from_r) * (r - from_r) + (g - from_g) * (g - from_g) + (b - from_b) * (b - from_b));
  182. }
  183.  
  184. unsigned short mapToColour(int r, int g, int b)
  185. {
  186.   double d1, d2, minDist;
  187.   unsigned short minIdx;
  188.  
  189.   minDist = getDist((double)colors[0][0], (double)colors[0][2], (double)colors[0][4], (double)r, (double)g, (double)b) + getDist((double)colors[0][1], (double)colors[0][3], (double)colors[0][5], (double)r, (double)g, (double)b);
  190.   minIdx = 0;
  191.  
  192.   for (unsigned short j = 1; j < 4; j++)
  193.   {
  194.     d1 = getDist((double)colors[j][0], (double)colors[j][2], (double)colors[j][4], (double)r, (double)g, (double)b);
  195.     d2 = getDist((double)colors[j][1], (double)colors[j][3], (double)colors[j][5], (double)r, (double)g, (double)b);
  196.  
  197.     #if defined DEBUG
  198.       Serial.print("("); Serial.print(r); Serial.print(","); Serial.print(g); Serial.print(","); Serial.print(b); Serial.print(") dist for color id ");
  199.       Serial.print(j); Serial.print(": "); Serial.print(d1 + d2); Serial.println();
  200.     #endif
  201.  
  202.     if (d1 + d2 < minDist)
  203.     {
  204.       minDist = d1 + d2;
  205.       minIdx = j;
  206.     }
  207.   }
  208.  
  209.   return minIdx;
  210. }
  211.  
  212. /*int mapToColour2(int r, int g, int b)
  213. {
  214.   double d, minDist;
  215.   int minIdx;
  216.  
  217.   minDist = getDist((double)colorsMap[0][0], (double)colorsMap[0][1], (double)colorsMap[0][2], (double)r, (double)g, (double)b);
  218.   minIdx = 0;
  219.  
  220.   #if defined DEBUG
  221.     Serial.print("("); Serial.print(r); Serial.print(","); Serial.print(g); Serial.print(","); Serial.print(b); Serial.print(") dist for color id 0: ");
  222.     Serial.print(minDist); Serial.println();
  223.   #endif
  224.  
  225.   for (int j = 1; j < 4; j++)
  226.   {
  227.     d = getDist((double)colors[j][0], (double)colors[j][1], (double)colors[j][2], (double)r, (double)g, (double)b);
  228.  
  229.     #if defined DEBUG
  230.       Serial.print("("); Serial.print(r); Serial.print(","); Serial.print(g); Serial.print(","); Serial.print(b); Serial.print(") dist for color id ");
  231.       Serial.print(j); Serial.print(": "); Serial.print(d); Serial.println();
  232.     #endif
  233.  
  234.     if (d < minDist)
  235.     {
  236.       minDist = d;
  237.       minIdx = j;
  238.     }
  239.   }
  240.  
  241.   return minIdx;
  242. }
  243.  
  244. int getColor(int R, int G, int B)
  245. {
  246.   if ((R <= 105 && R >= 90) && (G <= 128 && G >= 110) && (B >= 90 && B <= 105))
  247.     return RED; // RED
  248.  
  249.   if ((R >= 92 && R <= 103) && (G >= 96 && G <= 109) && (B >= 88 && B <= 100))
  250.     return GREEN; // GREEN
  251.  
  252.   if ((R >= 70 && R <= 78) && (G >= 85 && G <= 92) && (B >= 82 && B <= 90))
  253.     return YELLOW; // YELLOW
  254.  
  255.   if (((R >= 70 && R <= 85) && (G >= 104 && G <= 113) && (B >= 86 && B <= 98)) || ((R >= 90 && R <= 98) && (G >= 112 && G <= 122) && (B >= 95 && B <= 105)))
  256.     return BLUE; // ORANGE
  257.  
  258.   return 0; // INVALID
  259. }*/
RAW Paste Data
We use cookies for various purposes including analytics. By continuing to use Pastebin, you agree to our use of cookies as described in the Cookies Policy. OK, I Understand
 
Top