dist

#include "Distance.h"
#include <Arduino.h>
#include <avr/io.h>
#include <math.h>

uint16_t ADC_mV_Faktor;
int i=0;
uint16_t dist_old;
static uint16_t d[3];

/**
 * @brief Configures the ADC related to mode, clock speed, gain
 *        resolution and refrence voltage
 *
 *
 * chose the reference voltage carfully related to the output
 * range of the sensor.
 */
void configADC()
{
  //MUX
  //ADCSR
  //REFS0 auf 1 setzen, damit wird Voltage Reference gesetzt
  //AVCC with external capacitor on AREF pin
  ADMUX |= (1<<REFS0);
  //ADCSRA-Register
  ADCSRA = 0;
  //ADEN auf 1 setzen (ADC aktivieren)
  //ADSC auf 1 setzen (ADC Conversion starten)
  //ADPS0 auf 1 setzen (Prescaler setzen) - Division Faktor = 128
  ADCSRA |= (1<<ADEN) | (1<<ADSC) | (1<<ADPS2) | (1<<ADPS1) | (1<<ADPS0);
}

/**
 * @brief Starts a single conversion and receives a result return in mV
 */
int16_t readADC(uint8_t channel)
{
  	ADMUX = channel;
 	configADC();

  	while ((ADCSRA & (1<<ADIF)) == 0){
  	}
    int16_t mV=0;

  	mV = ADC;
  	mV = mV*(3300/1023);
    return mV;

}

bool checkDistance()
{
	 uint16_t valueSR=linearizeSR(readADC(1));
 uint16_t valueLR=linearizeLR(readADC(0));
 //uint16_t d [3];
 uint16_t dist;
  i=i%3;
  if ((valueSR < 200)&&(valueSR!=0)){
                  d[i] = valueSR;
                }
          else {
                  d[i] = valueLR;
                }
  i++;
 // if (i==3){
  dist=max(min(d[0],d[1]),min(max(d[0],d[1]),d[2]));
    if (dist<150){
    return false;
    }
  else{
    return true;
  }

}
/**
 * @brief Maps the digital voltage information on a distance
 *        in mm
 */
uint16_t linearizeSR(uint16_t distmV)
{
  //short range
  //4 bis 30 cm
  //Look-up Tabelle: mapping Voltage(mV), Distanz (mm)
  uint16_t lookup[17][2]={{2247,40},{2025,50},{1809,60},{1593,70},{1365,80},{1223,90},
                          {1104,100},{1014,110},{921,120},{861,130},{804,140},{765,150},
                          {615,180},{537,210},{462,240},{402,270},{327,300}
  						};
  //der beste Wert muss gefunden werden
  int i=0;
  while(distmV < lookup[i][0]){
    i++;
  }

  if (i==17){
    return 0;
  }
  if (i==0){
    return lookup[0][1];
  }
  else if ((lookup[i-1][0]-distmV)<=(distmV-lookup[i][0])){
    return lookup[i-1][1];
  }
  else{
    return lookup[i][1];
  }
}

/**
 * @brief Maps the digital voltage information on a distance
 *        in mm
 */
uint16_t linearizeLR(uint16_t distmV)
{
  //long range
  //20 bis 150 cm
  //Look-up Tabelle: mapping Voltage (mV), Distanz (mm)
  uint16_t lookup[13][2]={{2750,150},{2500,200},{2214,250},{2000,300},{1641,350},
                          {1500,400},{1300,450},{1250,500},{1068,550},{1050,600},
                          {882,650},{711,700},{682,800}
  						};
  //der beste Wert muss gefunden werden
  int i=0;
  while(distmV < lookup[i][0]){
    i++;
  }
  if (i==13){
    return 0;
  }
  if (i==0){
    return lookup[0][1];
  }
  else if ((lookup[i-1][0]-distmV)<=(distmV-lookup[i][0])){
    return lookup[i-1][1];
  }
  else{
    return lookup[i][1];
  }

}