SUPER-BETA M&M's sorter

#define RBLU 271
#define RVERDE 338
#define RARANCIO 562
#define RROSSO 451
#define RMARRONE 321
#define RGIALLO 615

#define GBLU 340
#define GVERDE 455
#define GARANCIO 450
#define GROSSO 337
#define GMARRONE 323
#define GGIALLO 697

#define BBLU 280
#define BVERDE 192
#define BARANCIO 171
#define BROSSO 161
#define BMARRONE 156
#define BGIALLO 208

#include <Servo.h>

Servo myservo;  // create servo object to control a servo
Servo canale;
                // a maximum of eight servo objects can be created

int inz = 0;    // variable to store the servo position

int trip[3] = {0,0,0};
int pos[4] = {60,90,120,150};

void setup()
{
  Serial.begin(9600);
  myservo.attach(9);
  canale.attach(10);
  pinMode(2,INPUT);
  pinMode(3,INPUT);
  pinMode(4,OUTPUT);
  pinMode(13,OUTPUT);
  analogReference(INTERNAL);



}


void loop()
{
    for(int i=0;i<4;i++) {
    delay(2000);
    canale.write(pos[i]);
  }

  myservo.write(104);

  while(!digitalRead(2));

  inz = millis();
  delay(400);
  while(!digitalRead(2));
  inz = millis()-inz;


  while(true) {
  while(digitalRead(2));
  myservo.write(97);
  digitalWrite(13,HIGH);


  lol();
  Serial.print("Posiziono canale:");
  Serial.println(trip[1]);
  canale.write(pos[trip[1]]);
  //while(digitalRead(3));
  myservo.write(104);
  digitalWrite(13,LOW);
  delay(inz-200);
  myservo.write(101);
  while(!digitalRead(2));
  }
}


void lol() {
  int r,g,b;
  int l;
  int letture[11] = {0,0,0,0,0,0,0,0,0,0,0};
  //Serial.println("\n\n\nOra:");
    Serial.print("\n");
  for(int i=0;i<5;i++) {
  digitalWrite(4,HIGH);
  delay(8);
  r = analogRead(0);
    delay(3);
  g = analogRead(1);
    delay(3);
  b = analogRead(2);
  digitalWrite(4,LOW);

  //Serial.print(" (");
  Serial.print(r,DEC);
  Serial.print("\t");
  Serial.print(g,DEC);
  Serial.print("\t");
  Serial.print(b,DEC);
  Serial.print("\n");
  delay(10);

  letture[getscore(r,g,b)] += 1;

  }
  Serial.print("-->");
  l = getmaxl(letture);

  if (l == 3) { l=2; }
  if (l == 4) { l=3; }


  Serial.println(getmaxl(letture), DEC);
  trip[0] = trip[1];
  trip[1] = trip[2];
  trip[2] = l;




}
 //                   0      1          2          3        4          5
//char nomi[][] = {"BLU", "VERDE", "ARANCIO", "ROSSO", "MARRONE", "GIALLO"};
int getscore(int r, int g, int b) {
  //                BLU    VERDE  ARAN   ROSS   MARR   GIAL
  int score[6] = {  1000,  1000,  1000,  1000,  1000,  1000};
  score[0] = abs(RBLU - r) + abs(GBLU - g) + abs(BBLU - b);
  score[1] = abs(RVERDE - r) + abs(GVERDE - g) + abs(BVERDE - b);
  score[2] = abs(RARANCIO - r) + abs(GARANCIO - g) + abs(BARANCIO - b);
  score[3] = abs(RROSSO - r) + abs(GROSSO - g) + abs(BROSSO - b);
  score[4] = abs(RMARRONE - r) + abs(GMARRONE - g) + abs(BMARRONE - b);
  score[5] = abs(RGIALLO - r) + abs(GGIALLO - g) + abs(BGIALLO - b);

  Serial.print("PUNTEGGI: ");
  for (int i=0;i<6;i++) {
    Serial.print(score[i],DEC);
    Serial.print(",");
  }
  Serial.print(" --- ");
  Serial.print(getmax(score),DEC);
  Serial.print("\n");
  return getmax(score);

  //getmax(score);

}

int getmax(int score[6]) {
  int small = score[0];
  int indez = 0;
  for (int i=1;i<6;i++) {
    if (score[i] < small) { small = score[i]; indez = i;}
  }

  return indez;
}

int getmaxl(int score[11]) {
  int small = score[0];
  int indez = 0;
  for (int i=1;i<11;i++) {
    if (score[i] > small) { small = score[i]; indez = i;}
  }

  return indez;
}

boolean compreso(int num,int a, int b, int offset) {
  if (a>b) {
    a = a + offset;
    b = b - offset;
    if ((num < a) && (num > b)) {
      return true;
    } else {
      return false;
    }
  } else {
    return (compreso(num,b,a,offset));
  }
}

void rec(int r, int g, int b, int offset) {
  if (compreso(r,265,240,offset)) {
    if (compreso(g,165,150,offset)) {
      if (compreso(b,80,70,offset)) {
        Serial.println("Faccia rossa");
        return;
      }
    }
  }
  if (compreso(r,130,120,offset)) {
    if (compreso(g,205,90,offset)) {
      if (compreso(b,85,75,offset)) {
        Serial.println("Faccia verde");
        return;
      }
    }
  }
  if (compreso(r,334,315,offset)) {
    if (compreso(g,310,295,offset)) {
      if (compreso(b,80,90,offset)) {
        Serial.println("Faccia arancio");
        return;
      }
    }
  }
  if (compreso(r,130,140,offset)) {
    if (compreso(g,173,183,offset)) {
      if (compreso(b,163,171,offset)) {
        Serial.println("Faccia blu");
        return;
      }
    }
  }
  if (compreso(r,337,352,offset)) {
    if (compreso(g,382,368,offset)) {
      if (compreso(b,105,97,offset)) {
        Serial.println("Faccia gialla");
        return;
      }
    }
  }
  if (compreso(r,337,354,offset)) {
    if (compreso(g,334,358,offset)) {
      if (compreso(b,227,217,offset)) {
        Serial.println("Faccia bianca");
        return;
      }
    }
  }
  //Serial.print("*");
  rec(r,g,b,offset+5);
  return;
}