/*
 *        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;
int ultimaPosicion=0;
int errorAnterior=0;
int P,I,D;



//------------------------------------------------------------
//----------------------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();
   int pos=getPosicion();
   if(pos!=0){
      ultimaPosicion=pos;
   }else{
    pos=ultimaPosicion;
   }
/*
    //PINTAR LOS VALORES
  for(int x=0;x<numSensores;x++){  
    Serial.print(valores[x]);
    Serial.print("\t");
  }
  Serial.println(pos);
*/

/*
 *El error generado de 500 a 4000, se mapea a un valor 
 *aceptable de -255 a 255 para establecer un valor adecuado
 *del error en función de la salida de los motores
 */
  int error=map(pos,500,4000,-255,255);

  luces(false,false);

/*
* TODO: se debe agregar el código para incluir los valores de
*  D e I, pues actualmante solo tutiliza control P
*/
  P=error;
  D=error-errorAnterior;
  I=I+error;
  errorAnterior=error;
  float pid=(kp*P)+(ki*I)+(kd*D);
  
  acelerar(velocidad-pid,velocidad+pid); 
  
}

/*
 * Funcion que controla la velocidad de cada motor, por lo
 *  que se envia el PWM que será aplicado a cada motor, 
 *  acepta cualquier valor, pero esta validado para que solo
 *  se pueda utilizar un rango de 0 - 255, si envias otro 
 *  valor, por defecto se coloca el extremo correspondiente
 */
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);
      }
    }
}


/*
 * Función que permite controlar dos led's uno derecho y
 * el otro en la izquierda del seguidor
 */
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
 *  
 *  NOTA: despues de llamar a esta función pueces ver los
 *  valores en el arreglo llamado "valores"
 */
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
 *  
 *NOTA: si no se detencta linea, se regresa un 0    
 */
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, lo cual
 * se consigue sacando el promedio de el valor mas bajo (blanco)
 * y el mas alto (negro)
 */
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);
}