View difference between Paste ID: vCp10s4d and bS8k3qt0

  /*
  * 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     1
+#define RED     0
-#define GREEN   2
+#define GREEN   1
-#define YELLOW  3
+#define YELLOW  2
-#define ORANGE  4
+#define BLUE    3
-#define BLUE    5
+
 #define START_POS   100 // 100°
 #define SENSOR_POS  47  // 47°
-#define SENSOR_POS  50  // 65°
+
 
 // #define DEBUG
-#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);
 
-    topServo.write(i); // 110 - 66° cu delay de 2ms intre fiecare °
+
-    delay(4); // 2ms
+
   digitalWrite(S1, LOW);
 
   topServo.attach(8);     // PIN #8
   bottomServo.attach(9);  // PIN #9
 
-  color = getColor(R, G, B); // afla culoarea
+  bottomServo.write(0);
 
   Serial.begin(9600);
 }
 
 void loop() {
-      //Serial.println("RED");
+
 
   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
 
-      bottomServo.write(100); // 100°
+
   color = mapToColour(R, G, B); // afla culoarea (getColor())
 
-    case ORANGE: {
+
-      Serial.println("ORANGE");
+
-      bottomServo.write(125); // 125°
+
     case RED: {
       Serial.println("RED");
       bottomServo.write(0); // 0°
-      //Serial.println("BLUE");
+
-      bottomServo.write(150); // 150°
+
     case GREEN: {
       Serial.println("GREEN");
-
+      bottomServo.write(25); // 25°
-    case 0: break; // invalid colour
+
     }
     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
-  else if ((R >= 85 && R <= 95) && (G >= 85 && G <= 100) && (B >= 83 && B <= 100))
+
     Serial.println("  ");
   #endif
-  else if ((R >= 70 && R <= 80) && (G >= 85 && G <= 90) && (B >= 79 && B <= 90))
+
-    return YELLOW; // YELLOW
+
 
-  else if ((R >= 70 && R <= 90) && (G >= 94 && G <= 114) && (B >= 82 && B <= 106))
+double getDist(double from_r, double from_g, double from_b, double r, double g, double b)
-    return ORANGE; // ORANGE
+
   return sqrt((r - from_r) * (r - from_r) + (g - from_g) * (g - from_g) + (b - from_b) * (b - from_b));
-  //return 0; // INVALID
+
-}
+
 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
 }*/