//  This is the IR sensor test code
//  2014.3
//  V 0.1
//  By Pepin

#include <Metro.h>
#define IrNumber 7 // the number of the IR sensor

float _iR[IrNumber];
float smooth(float newdata, float filterVal, float smoothedVal);

Metro IRsensorMetro = Metro(19,true);//every 19ms to collect the IR data

static float IRdata[IrNumber] = {
    80,80,80,80,80                  }; //init the value of the IR sensor

void setup() 
{
  Serial.begin(57600);
}

void loop() 
{
/********************** Date collection ***********************/
if(IRsensorMetro.check())
  {
    IRreader();
    /*
  for(int h=0;h<5;h++)
  {
    Serial.print(_iR[h]);
    Serial.print(" ");
  }
  Serial.println("");
  */
  }
}

/********************** IR sensor ***********************/

void IRreader()//detect distance on both sides 
{


  for(int h=0;h<IrNumber-2;h++)
  {
    float volts = analogRead(h + 1);
    _iR[h] = (6787 / (volts - 3)) - 4;
    if(_iR[h]<10)	_iR[h] = 80;

    _iR[h] = min(_iR[h],80);
    _iR[h] = max(_iR[h],12);
  }
    for(int h=5;h<7;h++)
    {
      float volts = analogRead(h-4);
      _iR[h] = (6787 / (volts - 3)) - 4;
      if(_iR[h]<10)	_iR[h] = 80;
  
      _iR[h] = min(_iR[h],80);
      _iR[h] = max(_iR[h],12);
    }

  for(int h = 0 ; h < 5 ; h++)
  {
    _iR[h] = smooth(_iR[h],0.9,IRdata[h]);
    IRdata[h] = _iR[h];
  }
  // check the IR sensor
  /*
  for(int h=0;h<5;h++)
  {
    Serial.print(_iR[h]);
    Serial.print(" ");
  }
  Serial.println("");
  */

}
// process the IR sensor
float smooth(float newdata, float filterVal, float smoothedVal)
{
  if (filterVal > 1)
    filterVal = .99;
  else if (filterVal <= 0)
    filterVal = 0;
  smoothedVal = (newdata * (1 - filterVal)) + (smoothedVal  *  filterVal);

  return smoothedVal;
}