/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * *                                                               
  *                                                                 *
  *    Tïtol: Tercer repte ClauTIC                                  *
  *    Objectiu: Seguir la línia fins arribar a creta.              *
  *    Versió: 3.3.3                                                * 
  *    Robot: Tossut, Prototip A                                    *
  *                                                                 *
  * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */
 
// Definició dels pins dels motors
int mesq_av = 12; // Motor esquerra per endavant al pin 12
int mesq_re = 11; // Motor esquerra per enradera al pin 11
int mesq_vel = 10; // Motor esquerra control velocitat al pin PWM 10
 
int mdret_av = 7; // Motor dret per endavant al pin 7
int mdret_re = 6; // Motor dret per enradera al pin 6
int mdret_vel = 5; // Motor dret control velocitat al pin PWM 5
 
//Definició dels pins del sensors
int sen_esq = A0;
int sen_dre = A1;
int sen_cen = A2;
 
//Definició de les variables que guardaran els valors
int val_esq;
int val_dre;
int val_cen;
 
int estat_color;
 
//Definició del límit entre colors
int limit = 220;  //més amunt negre, més avall blanc.
 
// Definim les variables pel contador de temps
unsigned long temps, tempsant=0, interval=50;
 
void setup()
{
    // Activem pins dels motors com sortida
// Motor esquerra
    pinMode(mesq_vel, OUTPUT);
    pinMode(mesq_av, OUTPUT);
    pinMode(mesq_re, OUTPUT);
// Motor dret
    pinMode(mdret_vel, OUTPUT);
    pinMode(mdret_av, OUTPUT);
    pinMode(mdret_re, OUTPUT);
//Sensors de línea com entrada
    pinMode(sen_esq, INPUT);
    pinMode(sen_dre, INPUT);
    pinMode(sen_cen, INPUT);
 
    Serial.begin(9600);
}
 
void loop()
{
    val_esq = analogRead(sen_esq);
    val_dre = analogRead(sen_dre);
    val_cen = analogRead(sen_cen);
 
/*  if (segona==0){
    Serial.print(val_esq);
    Serial.print("        ");
    Serial.print(val_cen);
    Serial.print("        ");
    Serial.print(val_dre);
    Serial.print("  - ");
    Serial.print(segona);
    Serial.println(" -  ");
  }*/
    estat_color = sequencia_color(limit, val_esq, val_cen, val_dre);
/*  if (segona==0){
    Serial.print("        ");
    Serial.println(estat_color);
    //delay(200);
  }*/
    switch (estat_color) {
      case 10:  // blanc negre blanc 010
        temps = millis();
        tempsant=millis();
        while (temps < (tempsant+interval)){
          Motors(HIGH, LOW, 150, HIGH, LOW, 150);  // motors endavant
          temps=millis();
        }
        Motors(LOW, LOW, 0, LOW, LOW, 0);
        break;
      case 1:  // blanc blanc negre 001
        temps = millis();
        tempsant=millis();
        while (temps < (tempsant+interval)){
          Motors(HIGH, LOW, 100, HIGH, LOW, 0);  // gira a la dreta ++
          temps=millis();
        }
        Motors(LOW, LOW, 0, LOW, LOW, 0);
        break;
      case 100:  // negre blanc blanc 100
        temps = millis();
        tempsant=millis();
        while (temps < (tempsant+(interval))){
          Motors(HIGH, LOW, 0, HIGH, LOW, 120);// gira a l'esquerra --
          temps=millis();
        }
        Motors(LOW, LOW, 0, LOW, LOW, 0);
        break;
      case 110:  // negre negre blanc 110
        temps = millis();
        tempsant=millis();
        while (temps < (tempsant+interval)){
          Motors(HIGH, LOW, 0, HIGH, LOW, 120);// gira a l'esquerra
          temps=millis();
        }
        Motors(LOW, LOW, 0, LOW, LOW, 0);
        break;
      case 11:  // blanc negre negre 011
        temps = millis();
        tempsant=millis();
        while (temps < (tempsant+interval)){
          Motors(HIGH, LOW, 100, HIGH, LOW, 0);  // gira a la dreta
          temps=millis();
        }
        Motors(LOW, LOW, 0, LOW, LOW, 0);
        break;
      case 111:  // negre negre negre 111
          temps = millis();
          tempsant=millis();
          while (temps < (tempsant+interval)){
            Motors(HIGH, LOW, 0, HIGH, LOW, 120);// gira a l'esquerra
            temps=millis();
          }
          Motors(LOW, LOW, 0, LOW, LOW, 0);
        break;
      case 101:  // negre blanc negre 101
          temps = millis();
          tempsant=millis();
          while (temps < (tempsant+interval)){
            Motors(HIGH, LOW, 0, HIGH, LOW, 120);// gira a l'esquerra
            temps=millis();
          }
          Motors(LOW, LOW, 0, LOW, LOW, 0);
        break;
      case 0:  // blanc blanc blanc 000
           Motors(LOW, LOW, 0, LOW, LOW, 0);
        break;
    }
}
 
//FUNCIONS
 
void Motors (char me_avan, char me_retr, int me_vel, char md_avan, char md_retr, int md_vel)
{
// Motor esquerra endavant o enrradera
  digitalWrite (mesq_av, me_avan); // Es passa al motor esquerra si anar endavant es HIGH o LOW
  digitalWrite (mesq_re, me_retr); // Es passa al motor esquerra si anar enradera es HIGH o LOW
// Motor dret endavant o enrradera
  digitalWrite (mdret_av, md_avan); // Es passa al motor dret si anar endavant es HIGH o LOW
  digitalWrite (mdret_re, md_retr); // Es passa al motor dret si anar enradera es HIGH o LOW
// Velocitat als motors
  analogWrite (mesq_vel, me_vel);  // Es passa al motor esquerra la velocitat
  analogWrite (mdret_vel, md_vel);  // Es passa al motor dret la velocitat
}
 
int sequencia_color(int valor_limit, int lec_esq, int lec_cen, int lec_dre)
{
  int tira_color = 0;
 
  if (lec_esq > valor_limit) {
    tira_color = 100; // resultat de 1 * 100
  }else {
    tira_color = 0; // resultat de 0 * 100
  }
  if (lec_cen > valor_limit) {
    tira_color = tira_color + 10; // resultat de 1 * 10
  } else {
    tira_color = tira_color + 0; // resultat de 0 * 10
  }
  if (lec_dre > valor_limit) {
    tira_color = tira_color + 1;  // el valor és 1
  } else {
    tira_color = tira_color + 0;  // el valor és 0
  }
 
  return tira_color;  //es retorna el valor de la variable tira_color
}