source: rtos_arduino/trunk/arduino_lib/libraries/Robot_Motor/src/lineFollow.cpp@ 136

//#include <ArduinoRobotMotorBoard.h>
#include "LineFollow.h"

//#define KP 19 //0.1 units
//#define KD 14
//#define ROBOT_SPEED 100 //percentage

//#define KP 11
//#define KD 5
//#define ROBOT_SPEED 50

//#define INTEGRATION_TIME 10 //En ms

/*uint8_t KP=11;
uint8_t KD=5;
uint8_t robotSpeed=50; //percentage
uint8_t intergrationTime=10;*/

#define NIVEL_PARA_LINEA 50 

/*int lectura_sensor[5], last_error=0, acu=0;

//Estos son los arrays que hay que rellenar con los valores de los sensores
//de suelo sobre blanco y negro. 
int sensor_blanco[]={
  0,0,0,0,0};
int sensor_negro[]={
  1023,1023,1023,1023,1023};
*/
//unsigned long time;

//void mueve_robot(int vel_izq, int vel_der);
//void para_robot();
//void doCalibration(int speedPct, int time);
//void ajusta_niveles();        //calibrate values

LineFollow::LineFollow(){
        /*KP=11;
        KD=5;
        robotSpeed=50; //percentage
        intergrationTime=10;*/
        config(11,5,50,10);
                
        for(int i=0;i<5;i++){
                sensor_blanco[i]=0;
                sensor_negro[i]=1023;
        }
}

void LineFollow::config(uint8_t KP, uint8_t KD, uint8_t robotSpeed, uint8_t intergrationTime){
        this->KP=KP;
        this->KD=KD;
        this->robotSpeed=robotSpeed;
        this->intergrationTime=intergrationTime;
        /*Serial.print("LFC: ");
        Serial.print(KP);
        Serial.print(' ');
        Serial.print(KD);
        Serial.print(' ');
        Serial.print(robotSpeed);
        Serial.print(' ');
        Serial.println(intergrationTime);*/
        
}
void LineFollow::calibIRs(){
        static bool isInited=false;//So only init once
        if(isInited)return ;
        
        delay(1000);
        
        doCalibration(30,500);
        doCalibration(-30,800);
        doCalibration(30,500);
        
        delay(1000);
        isInited=true;
}

void LineFollow::runLineFollow(){
  for(int count=0; count<5; count++)
  {
    lectura_sensor[count]=map(_IRread(count),sensor_negro[count],sensor_blanco[count],0,127);
    acu+=lectura_sensor[count];
  }

  //Serial.println(millis());
  if (acu > NIVEL_PARA_LINEA)
  {
    acu/=5;

    int error = ((lectura_sensor[0]<<6)+(lectura_sensor[1]<<5)-(lectura_sensor[3]<<5)-(lectura_sensor[4]<<6))/acu;

    error = constrain(error,-100,100);

    //Calculamos la correcion de velocidad mediante un filtro PD
    int vel = (error * KP)/10 + (error-last_error)*KD;

    last_error = error;

    //Corregimos la velocidad de avance con el error de salida del filtro PD
    int motor_left = constrain((robotSpeed + vel),-100,100);
    int motor_right =constrain((robotSpeed - vel),-100,100);

    //Movemos el robot
    //motorsWritePct(motor_left,motor_right);
    motorsWritePct(motor_left,motor_right);

    //Esperamos un poquito a que el robot reaccione
    delay(intergrationTime);
  }
  else
  {
    //Hemos encontrado una linea negra
    //perpendicular a nuestro camino
    //paramos el robot
    motorsStop();

    //y detenemos la ejecución del programa
    //while(true);
        reportActionDone();
        //setMode(MODE_SIMPLE);
  }
}  


void LineFollow::doCalibration(int speedPct, int time){
        motorsWritePct(speedPct, -speedPct);
        unsigned long beginTime = millis();
        while((millis()-beginTime)<time)
                ajusta_niveles();
        motorsStop();
}
void LineFollow::ajusta_niveles()
{
        int lectura=0;

        for(int count=0; count<5; count++){
                lectura=_IRread(count);

                if (lectura > sensor_blanco[count])
                        sensor_blanco[count]=lectura;

                if (lectura < sensor_negro[count])
                        sensor_negro[count]=lectura;
        } 
}