Multiskills' OpenRoby Project

///CodyColor 3.0 A.S. 2019/20
// Team Multiskill

/* PINOUT ARDUINO - CodyColor 3.0
 * [~] PWM
 * [A] Analog Input
 * [D] Digital Input
 * [@] Used by
 *
 * [D] 13
 * [D] 12
 * [~] 11 @ MD Forward
 * [~] 10 @ MS Backward
 * [~] 9  @ MS Forward
 * [D] 8
 * [D] 7  @ S0
 * [~] 6  @ S3
 * [~] 5  @ S2
 * [D] 4  @ OUT
 * [~] 3  @ MD Backward
 * [D] 2
 * [TX] 1
 * [RX] 0
 *
 */


/*
 * Funzioni di movimento:
 *
 * go(passo) -> esegue passo avanti
 * stopM(M[2]); -> ferma il motore M
 * dx(passo) -> gira soltanto a destra (senza passo avanti) (passo in ms)
 * sx(passo) -> gira soltanto a sinistra (idem)
 *
 * front(M[2], speed) -> manda avanti il motore M con velocita speed
 * back(M[2], speed) -> manda indietro
 */

// Controllo sensore
#define S0 7
#define S2 5
#define S3 6
#define out 4


// Controllo motori

int MD[2] = {11,3};
int MS[2] = {9, 10};




int pulse, R, G, B, toll, ColorR, ColorG, ColorB;


///////
/////Parametri



int passo = 500;
int pausa = 1000;

int velMD = 72;
int velMS = 86.5;


////////

void setup(){

  Serial.begin(9600);  // Comunicazione Seriale per Test

  for(int i = S0; i<= S3; i++){
    pinMode(i,OUTPUT);
  }

  pinMode(out, INPUT);
  digitalWrite(S0, HIGH); //Scaling colore a 20%

  pinMode(MD[0], OUTPUT);
  pinMode(MD[1], OUTPUT);
  pinMode(MS[0], OUTPUT);
  pinMode(MS[1], OUTPUT);

  go(passo);
}


void loop(){
   delay(1000);
  /* Combinazioni
   *     S2   S3
   * R: LOW  | LOW
   * G: HIGH | HIGH
   * B: LOW  | HIGH */

   //R
   digitalWrite(S2, LOW);
   digitalWrite(S3, LOW);
   pulse = pulseIn(out, LOW); //frequenza
   R = map(pulse,25,72,255,0);

   //G
   digitalWrite(S2, HIGH);
   digitalWrite(S3, HIGH);
   pulse = pulseIn(out, LOW);
   G = map(pulse,30,90,255,0);

   //B
   digitalWrite(S2, LOW);
   digitalWrite(S3, HIGH);
   pulse = pulseIn(out, LOW);
   B = map(pulse,25,70,255,0);

// Serie if/else per definire come muoversi

   if(isWhite(R,G,B) == true){
      ///FERMA TUTTO
      stopAll(MD, MS);
   }
   else{
      if(isGray(R,G,B) == true) {
      ///VAI AVANTI
      go(passo);

      Serial.println("GRIGIO");  // Comunicazione Seriale per Test
      }
      else if (isYellow(R,G,B) == true){
        //SINISTRA
        sx(passo);
        delay(pausa);
        go(passo);

        Serial.println("GIALLO");  // Comunicazione Seriale per Test
        }
      else if (isRed(R,G,B) == true){
          //DESTRA
          dx(passo);
          delay(pausa);
          go(passo);

          Serial.println("ROSSO");  // Comunicazione Seriale per Test
         }
        else
          {
          Serial.print("COLORE NON RILEVATO: ");  // Comunicazione Seriale per Test
          Serial.print(R);
          Serial.print(G);
          Serial.println(B);
          }
      }

}

//
////Funzioni di movimento
//

void go(int timing){
  moveM(MS, MD, true, true, velMS, velMD);
  delay(timing);
  stopAll(MD, MS);

}
void dx(int timing){
  moveM(MS, MD, true, false, velMS, velMD);
  delay(timing);
  stopAll(MS, MD);
}

void sx(int timing){
  moveM(MS, MD, false, true, velMS, velMD);
  delay(timing);
  stopAll(MS, MD);
}

//Core for Movement

void moveM(int M1[2], int M2[2], bool fob1, bool fob2, int vel1, int vel2){
  /// inputs
  /// M1 @ {int, int} (pin motore1)
  /// M2 @ {int, int} (pin motore2)
  /// fob1 @ bool (1 front, 0 back per M1)
  /// fob2 @ bool (1 front, 0 back per M2)
  /// vel1 @ int (velocita M1)
  /// vel1 @ int (velocita M2)

  if(fob1 == true){
    digitalWrite(M1[1], LOW);
    analogWrite(M1[0], vel1);
  }
  else if(fob1 == false){
    digitalWrite(M1[0], LOW);
    analogWrite(M1[1], vel1);
  }

  if(fob2 == true){
    digitalWrite(M2[1], LOW);
    analogWrite(M2[0], vel2);
  }
  else if(fob2== false){
    digitalWrite(M2[0], LOW);
    analogWrite(M2[1], vel2);
  }

}

void stopAll(int M1[2], int M2[2]){
  digitalWrite(M1[0], LOW);
  digitalWrite(M1[1], LOW);
  digitalWrite(M2[0], LOW);
  digitalWrite(M2[1], LOW);
}



//
// Riconoscimento del Colore
//

bool isGray(int R, int G, int B){
//  352 352 368

  ColorR = 352;
  ColorG = 352;
  ColorB = 368;
  bool iscolor = isColor(ColorR, ColorG, ColorB, R ,G ,B);
  return iscolor;
 }

bool isYellow(int R, int G, int B){
// 374 365 357
  ColorR = 374;
  ColorG = 365;
  ColorB = 357;
  bool iscolor = isColor(ColorR, ColorG, ColorB, R ,G ,B);
  return iscolor;
 }

bool isRed(int R, int G, int B){
  //352 293 311
  ColorR = 352;
  ColorG = 293;
  ColorB = 311;
  bool iscolor = isColor(ColorR, ColorG, ColorB, R ,G ,B);
  return iscolor;
 }

 bool isWhite(int R, int G, int B){
  ColorR = 255;
  ColorG = 255;
  ColorB = 255;
  bool iscolor = isColor(ColorR, ColorG, ColorB, R ,G ,B);
  return iscolor;
 }

bool isColor(int ColorR, int ColorG, int ColorB, int R, int G, int B){
  toll = 15; //tolleranza

  if((R <= ColorR+toll) && (R>= ColorR-toll)){
    if((G <= ColorG+toll) &&(G>= ColorG-toll)){
      if((B <= ColorB+toll) && (B>= ColorB-toll)){
      return true;
      } else return false;
    } else return false;
  } else return false;

}