Arduino Project

 /*
 * Skittles Color Sorter using Arduino UNO R3.
 * S0, S1, S2, S3 - PINs used by the color sensor.
 * Colours return IDs: 1 - red, 2 - green, 3 - yellow, 4 - orange, 5 - blue
 */

#include <Servo.h>

 /*
 * PIN-urile pentru senzorul de lumina.
 */
#define S0 3
#define S1 4
#define S2 5
#define S3 6

#define sensorOut 7

 /*
 * Coduri pentru culori.
 */
#define RED     0
#define GREEN   1
#define YELLOW  2
#define BLUE    3

#define START_POS   100 // 100°
#define SENSOR_POS  47  // 47°
#define END_POS     29  // 29°

// #define DEBUG

Servo topServo;     // motor sus
Servo bottomServo;  // motor jos

int frequency = 0, color = 0;
int R, G, B;

unsigned short colors[4][6] = {
  // R        G        B
  { 75, 82, 90, 108, 78, 91 },  // RED
  { 82, 89, 88, 96, 81, 89 },   // GREEN
  { 60, 70, 78, 86, 75, 84 },   // YELLOW
  { 81, 90, 87, 97, 70, 80 }    // BLUE
};

/*unsigned short colorsMap[4][3] = {
  // R, G, B
  {76, 100, 85}, // RED
  {76, 84, 79}, // GREEN
  {69, 84, 82}, // YELLOW
  {85, 92, 75}  // BLUE
};*/

void setup() {
  pinMode(S0, OUTPUT);
  pinMode(S1, OUTPUT);
  pinMode(S2, OUTPUT);
  pinMode(S3, OUTPUT);
  pinMode(sensorOut, INPUT);

  // Setting frequency-scaling to 20%
  digitalWrite(S0, HIGH);
  digitalWrite(S1, LOW);

  topServo.attach(8);     // PIN #8
  bottomServo.attach(9);  // PIN #9

  bottomServo.write(0);

  Serial.begin(9600);
}

void loop() {
  topServo.write(START_POS); // seteaza servomotorul sus la 115° (ia bomboana)

  delay(500); // asteapta 500ms

  for (int i = START_POS; i >= SENSOR_POS; i--) { // muta motorul sub senzor
    topServo.write(i);
    delay(6);
  }

  delay(500); // 500ms

  getRGB(&R, &G, &B); // afla culoarea
  color = mapToColour(R, G, B); // afla culoarea (getColor())

  delay(10); // delay 10ms

  switch (color) {
    case RED: {
      Serial.println("RED");
      bottomServo.write(0); // 0°
      break;
    }
    case GREEN: {
      Serial.println("GREEN");
      bottomServo.write(25); // 25°
      break;
    }
    case YELLOW: {
      Serial.println("YELLOW");
      bottomServo.write(50); // 50°
      break;
    }
    case BLUE: {
      Serial.println("BLUE");
      bottomServo.write(75); // 75°
      break;
    }
  }

  delay(300); // delay 300ms

  for (int i = SENSOR_POS; i >= END_POS; i--) {
    topServo.write(i);
    delay(6);
  }

  delay(200); // 200ms

  for (int i = END_POS; i <= START_POS; i++) {
    topServo.write(i); // revine la pozitia initiala de 115°
    delay(4);
  }

  color = 0; // reseteaza variabila de culoare
}

void getRGB(int *R, int *G, int *B)
{
  // Setting red filtered photodiodes to be read
  digitalWrite(S2, LOW);
  digitalWrite(S3, LOW);

  frequency = pulseIn(sensorOut, LOW);
  *R = frequency;
  // frequency = map(frequency, 165, 180, 255, 0);

  #if defined DEBUG
    Serial.print("R= ");//printing name
    Serial.print(frequency);//printing RED color frequency
    Serial.print("  ");
  #endif
  delay(100);

  // Setting Green filtered photodiodes to be read
  digitalWrite(S2, HIGH);
  digitalWrite(S3, HIGH);

  frequency = pulseIn(sensorOut, LOW);
  *G = frequency;
  // frequency = map(frequency, 195, 210, 255, 0);

  #if defined DEBUG
    Serial.print("G= ");//printing name
    Serial.print(frequency);//printing RED color frequency
    Serial.print("  ");
  #endif

  delay(100);

  digitalWrite(S2, LOW);
  digitalWrite(S3, HIGH);

  frequency = pulseIn(sensorOut, LOW);
  *B = frequency;
  // frequency = map(frequency, 30, 90, 255, 0);

  #if defined DEBUG
    Serial.print("B= ");//printing name
    Serial.print(frequency);//printing RED color frequency
    Serial.println("  ");
  #endif
  delay(100);
}

double getDist(double from_r, double from_g, double from_b, double r, double g, double b)
{
  return sqrt((r - from_r) * (r - from_r) + (g - from_g) * (g - from_g) + (b - from_b) * (b - from_b));
}

unsigned short mapToColour(int r, int g, int b)
{
  double d1, d2, minDist;
  unsigned short minIdx;

  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);
  minIdx = 0;

  for (unsigned short j = 1; j < 4; j++)
  {
    d1 = getDist((double)colors[j][0], (double)colors[j][2], (double)colors[j][4], (double)r, (double)g, (double)b);
    d2 = getDist((double)colors[j][1], (double)colors[j][3], (double)colors[j][5], (double)r, (double)g, (double)b);

    #if defined DEBUG
      Serial.print("("); Serial.print(r); Serial.print(","); Serial.print(g); Serial.print(","); Serial.print(b); Serial.print(") dist for color id ");
      Serial.print(j); Serial.print(": "); Serial.print(d1 + d2); Serial.println();
    #endif

    if (d1 + d2 < minDist)
    {
      minDist = d1 + d2;
      minIdx = j;
    }
  }

  return minIdx;
}

/*int mapToColour2(int r, int g, int b)
{
  double d, minDist;
  int minIdx;

  minDist = getDist((double)colorsMap[0][0], (double)colorsMap[0][1], (double)colorsMap[0][2], (double)r, (double)g, (double)b);
  minIdx = 0;

  #if defined DEBUG
    Serial.print("("); Serial.print(r); Serial.print(","); Serial.print(g); Serial.print(","); Serial.print(b); Serial.print(") dist for color id 0: ");
    Serial.print(minDist); Serial.println();
  #endif

  for (int j = 1; j < 4; j++)
  {
    d = getDist((double)colors[j][0], (double)colors[j][1], (double)colors[j][2], (double)r, (double)g, (double)b);

    #if defined DEBUG
      Serial.print("("); Serial.print(r); Serial.print(","); Serial.print(g); Serial.print(","); Serial.print(b); Serial.print(") dist for color id ");
      Serial.print(j); Serial.print(": "); Serial.print(d); Serial.println();
    #endif

    if (d < minDist)
    {
      minDist = d;
      minIdx = j;
    }
  }

  return minIdx;
}

int getColor(int R, int G, int B)
{
  if ((R <= 105 && R >= 90) && (G <= 128 && G >= 110) && (B >= 90 && B <= 105))
    return RED; // RED

  if ((R >= 92 && R <= 103) && (G >= 96 && G <= 109) && (B >= 88 && B <= 100))
    return GREEN; // GREEN

  if ((R >= 70 && R <= 78) && (G >= 85 && G <= 92) && (B >= 82 && B <= 90))
    return YELLOW; // YELLOW

  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)))
    return BLUE; // ORANGE

  return 0; // INVALID
}*/