/*
 *        SEGUIDOR DE LINEA CON ARDUINO
 *                CODIGO BASE
 *  Este código es la base para iniciar el seguidor de linea
 *  aun no esta terminado, pero se ira completando hasta la
 *  version final.
 *  
 *  NOTA: Esta codigo no usa la libreria QTR, por lo que el
 *  procesamieto es manual y se puede ajustar de una mejor manera
 */

//------------------------------------------------------------
//----------------------VARIABLES-----------------------------
//------------------------------------------------------------
int sensores[]={A0,A1,A2,A3,A4,A5,A6,A7};//Sensores
int valores[8];
int umbral[]={1023,1023,1023,1023,1023,1023,1023,1023};
int numSensores=8;//total de sensres en la barra
int ir =2;//encender y apagar la barra

#define PWMA  9
#define AIN2  8
#define AIN1  7
#define BIN1  5
#define BIN2  4
#define PWMB  3

#define LED_DER 12
#define LED_IZQ 2


//------------------------------------------------------------
//----------------------------PID-----------------------------
//------------------------------------------------------------
float kp=.5;
float ki=0;
float kd=0;
int velocidad=100;



//------------------------------------------------------------
//----------------------INICIALIZACION------------------------
//------------------------------------------------------------
void setup() {
  //pines para controlar motores
  pinMode(PWMA,OUTPUT);
  pinMode(PWMB,OUTPUT);
  pinMode(AIN1,OUTPUT);
  pinMode(AIN2,OUTPUT);
  pinMode(BIN1,OUTPUT);
  pinMode(BIN2,OUTPUT);
  //establecer direccion de los motores
  digitalWrite(AIN1,HIGH);
  digitalWrite(AIN2,LOW);
  digitalWrite(BIN1,LOW);
  digitalWrite(BIN2,HIGH);
  
  Serial.begin(115200);
  //Inicializamos los pines de los sensores
  for(int x=0;x<numSensores;x++){
    pinMode(sensores[x],INPUT);
  }
  
  pinMode(LED_BUILTIN,OUTPUT);
  pinMode(LED_DER,OUTPUT);
  pinMode(LED_IZQ,OUTPUT);
  //pin para controlar el encendido de la barra
  pinMode(ir,OUTPUT);

  
  
  digitalWrite(ir,HIGH);//Encendemos la barra
  calibracion();
}


//------------------------------------------------------------
//---------------------------LOOP-----------------------------
//------------------------------------------------------------
void loop() {
   int pos=getPosicion();
/*
    //PINTAR LOS VALORES
  for(int x=0;x<numSensores;x++){  
    Serial.print(valores[x]);
    Serial.print("\t");
  }
  Serial.println(pos);
*/
  int error=map(pos,500,4000,-255,255);
  Serial.print(pos);
  Serial.print("->");
  Serial.println(error);
  luces(false,false);
  float pid=kp*error;
  acelerar(70+pid,70-pid);
  
}

void acelerar(int motIzq, int motDer){
    if(motIzq<0){
      analogWrite(PWMB,0);
    }else{
      if(motIzq>255){
        analogWrite(PWMB,255);
      }else{
        analogWrite(PWMB,motIzq);
      }
    }
    if(motDer<0){
      analogWrite(PWMA,0);
    }else{
      if(motDer>255){
        analogWrite(PWMA,255);
      }else{
        analogWrite(PWMA,motDer);
      }
    }
}

void luces(boolean izq,boolean der){
  digitalWrite(LED_IZQ,izq);
  digitalWrite(LED_DER,der);
}


/*
 * Funcion que permite la lectura de los sensores de forma
 * analógica sin procesar nada, el resultado queda en el
 * arreglo llamado "valores"
 */
void leerRAW(){
   //LEER LOS VALORES DE LOS SENSORES
    for(int x=0;x<numSensores;x++){
      valores[x]=analogRead(sensores[x]);
    }
}

/*
 * Función que permite mediante un promedio de valores
 * encontrar la posición de la linea negra en todo momento
 * es similar a la función de la libreria QTR, por lo que regresará
 *  un valor entre 0 y 4000
 */
int getPosicion(){
  leerDigital();
  int activos=0;
  int suma=0;
   for(int x=0;x<numSensores;x++){
    suma=suma+valores[x];
    if(valores[x]!=0){
      activos++;
    }
   }
   if(activos==0){
    return 0;
   }else{
    return suma/activos;
   }
  
}

/*
 * Esta función permite leer los valores de los ensores y
 * procesar cada uno de los valores para encontrar la linea
 * negra, similar a la lectura de la libreria QTR, donde cada
 * linea es multipo de 500
 * 
 *      500  1000  1500  2000  2500  3000  3500  4000
 */
void leerDigital(){
   //LEER LOS VALORES DE LOS SENSORES
    for(int x=0;x<numSensores;x++){
      valores[x]=analogRead(sensores[x])>(umbral[x])?(x*500)+500:0;
    }
}


/*
 * Esta funcion tiene como objetivo encontrar el valor mas alto
 * y mas bajo de los sensores, para establecer su umbral, por el momento solo
 * encuentra los valores bajos (blancos) y queda pendiente encontrar
 * los valores negros para obtener su promedio para el umbral.
 * 
 * TODO:reacomodar esta función
 */
void calibracion(){
  int min[]={1023,1023,1023,1023,1023,1023,1023,1023};
  int max[]={0,0,0,0,0,0,0,0};
  
  digitalWrite(LED_BUILTIN,HIGH);
  for(int y=0;y<400;y++){
  for(int x=0;x<numSensores;x++){
    int v=analogRead(sensores[x]);
    if(min[x]>v){
      min[x]=v;
    }
    if(max[x]<v){
      max[x]=v;
    }
  }
  delay(10);
  }
  for(int x=0;x<numSensores;x++){
    umbral[x]=(min[x]+max[x])/2;
  }
  digitalWrite(LED_BUILTIN,LOW);
}