//  код был написан на языке С в среде CVAVR 3 для mega128
#define HS PORTB.6 // ножка МК для управления ультразвковым датчиком 
// состояния флагов
#define _TRUE 1
#define _ FALSE 0

unsigned int distance; // сырые данные расстояния
unsigned char takt = 0; // этап измерения
unsigned char isErrorHsFlag = _FALSE; // флаг ошибки измерения (переполнение таймера)
unsigned char isFlag = _FALSE; // флаг состояния измерения (измерено или еще нет)

// нужно настроить таймер 1 так, чтобы число 0xFFFF в его счетном регистре 
// набиралось больше, чем за 38 мс.
// в данном примере для МК использовался кварц 16 МГц.

// прерывание таймера 1 по переполнению, нужно для обнаружения ошибки 
interrupt [TIM1_OVF] void timer1_ovf_isr(void) {
 isErrorHsFlag = _TRUE;
}

// внешнее прерывание 
interrupt [EXT_INT4] void ext_int4_isr(void) {
 takt++;
 switch (takt) {
 case 1:
  // обнуляем таймер
  TCNT1 = 0;
  isErrorHsFlag = _FALSE;
 break;
 case 2: 
  // получаем дистанцию
  distance = TCNT1;
  takt = 0;
  isFlag = _TRUE;
 break;
}; 
 
 if (isErrorHsFlag == _TRUE) {
  // в случае переполнения таймера была ошибка
  distance = 0xFFFF;
  isErrorHsFlag = _FALSE;
 }
}

// функция возвращает расстояние
unsigned int getDistance(void) {
 unsigned char whileTakt = 0; // для количества циклов (лимит ожидания)
 // устанавливаем ножку МК в лог. 0
 HS = 0;
 // обнуляем флаги и регистр состояния
 takt = 0;
 isFlag = _FALSE;
 isErrorHsFlag = _FALSE;
 // запускаем измерение расстояния
 HS=1;
 delay_us(10);
 HS=0; 
 whileTakt=0;
 // ждем измерения
 while (isFlag == _FALSE) {
  whileTakt++; 
  delay_ms(1);
  // если лимит времени превышен (измерение длится больше 38 мс)
  if (whileTakt > 38) {
   isFlag = _TRUE; // выходим из цикла
  };
 };  
 return distance;  
};

// для медианного фильтра
#define NULL         0
#define STOPPER   0 /* Smaller than any datum */
#define MEDIAN_FILTER_SIZE 5

// медианный фильтр
typedef struct pair{
  struct pair *point; /* Pointers forming list linked in sorted order */
  unsigned int value; /* Values to sort */
} PAIR_T;

static PAIR_T small = {NULL, STOPPER};

static PAIR_T buffer[MEDIAN_FILTER_SIZE] = {0};


/* Pointer into circular buffer of data */
static PAIR_T *datpoint = buffer;
/* Chain stopper */

/* Pointer to head (largest) of linked list.*/
static PAIR_T big = {&small, 0};

/* Pointer to successor of replaced data item */
struct pair *successor;
/* Pointer used to scan down the sorted list */
struct pair *scan;
/* Previous value of scan */
struct pair *scanold;
/* Pointer to median */
struct pair *median;
unsigned int i;



unsigned int MedianFilter(unsigned int datum)
{


if (datum == STOPPER){
   datum = STOPPER + 1; /* No stoppers allowed. */
}

if ( (++datpoint - buffer) >= MEDIAN_FILTER_SIZE){
   datpoint = buffer; /* Increment and wrap data in pointer.*/
}

datpoint->value = datum; /* Copy in new datum */
successor = datpoint->point; /* Save pointer to old value's successor */
median = &big; /* Median initially to first in chain */
scanold = NULL; /* Scanold initially null. */
scan = &big; /* Points to pointer to first (largest) datum in chain */

/* Handle chain-out of first item in chain as special case */
if (scan->point == datpoint){
   scan->point = successor;
}

scanold = scan; /* Save this pointer and */
scan = scan->point ; /* step down chain */

/* Loop through the chain, normal loop exit via break. */
for (i = 0 ; i < MEDIAN_FILTER_SIZE; ++i){
  /* Handle odd-numbered item in chain */
  if (scan->point == datpoint){
    scan->point = successor; /* Chain out the old datum.*/
  }

  if (scan->value < datum){ /* If datum is larger than scanned value,*/
    datpoint->point = scanold->point; /* Chain it in here. */
    scanold->point = datpoint; /* Mark it chained in. */
    datum = STOPPER;
  };

  /* Step median pointer down chain after doing odd-numbered element */
  median = median->point; /* Step median pointer. */
  if (scan == &small){
    break; /* Break at end of chain */
  }
  scanold = scan; /* Save this pointer and */
  scan = scan->point; /* step down chain */

  /* Handle even-numbered item in chain. */
  if (scan->point == datpoint){
    scan->point = successor;
  }

  if (scan->value < datum){
    datpoint->point = scanold->point;
    scanold->point = datpoint;
    datum = STOPPER;
  }

  if (scan == &small){
    break;
  }

  scanold = scan;
  scan = scan->point;
}

return median->value;
}

#define CONST_RAD 5092.95817  //константа множителя для углов
unsigned int ac,bc,rad; // катеты ac bc и угол поворота
float bck; // коэффициент для катета bc

// катет AC это расстояние до цели
ac = MedianFilter(getDistance());
if (ac > 50000) ac = 50000; // ограничение дальности
// чтобы уменьшить погрешность, считаем сразу без перевода в сантиметры
// катет bc - это расстояние между лазерами
rad = atan(ac / (bc * bck)); 
// устанавливаем угол поворота сервопривода
pwmServo(CONST_RAD*rad);

// код был написан на языке С в среде CVAVR 3 для mega128
// для работы с SD картой и файловой системой FAT необходимы библиотеки:
// #include <sdcard.h>
// #include <ff.h>

//для работы с SD картой
static FRESULT f_err_code; // FRESULT для функций модуля
static FATFS FATFS_Obj; // структура - логический раздел
unsigned int ByteRead = 255; //количество релаьно считанных байт основного файла
FIL fil_obj; //структура файла, с которым работаем
char var[127]; //буфер, сюда мы поместим то, что считаем из основного файла.

// функция открывает трек по его номеру
// для каждого номера прописывается название файла.
// Для работы функции нужно иметь два таймера, таймер 1 и таймер 2. 
// таймер 2 нужен для вывода ШИМ сигнала, частота ШИМ - максимальная. Настройка - "быстрый ШИМ"
// в данном примере для МК использовался кварц 16 МГц.
void openSnd(unsigned char nSnd) {
 switch (nSnd) {
 case 0:
  f_err_code = f_open(&fil_obj, "but_1.wav", FA_READ);  //пытаемся открыть файл "but_1.wav"
 break; 
 case 1:
  f_err_code = f_open(&fil_obj, "but_2.wav", FA_READ); 
 break;
 case 2:
  f_err_code = f_open(&fil_obj, "but_no.wav", FA_READ); 
 break;
 case 3:
  f_err_code = f_open(&fil_obj, "but_ok.wav", FA_READ);
 break;
 case 4:
  f_err_code = f_open(&fil_obj, "but_sa.wav", FA_READ); 
 break; 
 case 5:
  f_err_code = f_open(&fil_obj, "but.wav", FA_READ); 
 break;
 case 6:
 f_err_code = f_open(&fil_obj, "warning.wav", FA_READ); 
 break;
 case 7:
  f_err_code = f_open(&fil_obj, "on.wav", FA_READ); /
 break;
 case 8:
  f_err_code = f_open(&fil_obj, "laz_sys.wav", FA_READ); 
 break;
 case 9:
  f_err_code = f_open(&fil_obj, "laz_act.wav", FA_READ);
 break;
 case 10:
  f_err_code = f_open(&fil_obj, "laz_actk.wav", FA_READ);
 break;
 case 11:
  f_err_code = f_open(&fil_obj, "voice_s.wav", FA_READ);
 break;
 case 12:
  f_err_code = f_open(&fil_obj, "bat_full.wav", FA_READ); 
 break; 
 case 13:
  f_err_code = f_open(&fil_obj, "bat_at.wav", FA_READ); 
 break;
 case 14:
  f_err_code = f_open(&fil_obj, "Bat_a.wav", FA_READ); 
 break;
 case 15:
  f_err_code = f_open(&fil_obj, "zel_no.wav", FA_READ);
 break;
 case 16:
  f_err_code = f_open(&fil_obj, "new_1.wav", FA_READ);
 break;
 case 17:
  f_err_code = f_open(&fil_obj, "new_2.wav", FA_READ); 
 break;
 case 18:
  f_err_code = f_open(&fil_obj, "new_3.wav", FA_READ); 
 break;
 case 19:
  f_err_code = f_open(&fil_obj, "new_4.wav", FA_READ); 
 break;
 case 20:
  f_err_code = f_open(&fil_obj, "new_5.wav", FA_READ);
 break;
};  
     
 // сообщаем об ошибках
 if (f_err_code &  FR_OK) puts("FR_OK\r\n");
 else
 if (f_err_code & FR_NO_FILE ) puts("FR_NO_FILE\r\n");
 else
 if (f_err_code & FR_NO_PATH ) puts("FR_NO_PATH\r\n");
 else
 if (f_err_code & FR_INVALID_NAME ) puts("FR_INVALID_NAME\r\n");
 else
 if (f_err_code & FR_INVALID_DRIVE  ) puts("FR_INVALID_DRIVE\r\n");
 else
 if (f_err_code & FR_EXIST   ) puts("FR_EXIST\r\n");
 else
 if (f_err_code & FR_DENIED  ) puts("FR_DENIED\r\n");
 else
 if (f_err_code &  FR_NOT_READY  ) puts("FR_NOT_READY\r\n");
 else
 if (f_err_code & FR_WRITE_PROTECTED   ) puts("FR_WRITE_PROTECTED\r\n");
 else
 if (f_err_code & FR_DISK_ERR   ) puts("FR_DISK_ERR\r\n");
 else
 if (f_err_code & FR_INT_ERR   ) puts("FR_INT_ERR\r\n");
 else
 if (f_err_code & FR_NOT_ENABLED  ) puts("FR_NOT_ENABLED\r\n");
 else
 if (f_err_code & FR_NO_FILESYSTEM  ) puts("FR_NO_FILESYSTEM\r\n");

 // если ошибок нет, начинаем воспроизведение
 if (f_err_code == 0) {
   //пытаемся читать 1 байт с начала файла в переменную var
  f_err_code = f_read(&fil_obj,var,44,&ByteRead);
  //надо настроить таймер 1 на частоту дискретизации
  // Timer/Counter 1 initialization
  // Clock source: System Clock
  // Clock value: 16000,000 kHz
  TCCR1A = (0<<COM1A1) | (0<<COM1A0) | (0<<COM1B1) | (0<<COM1B0) | (0<<COM1C1) | (0<<COM1C0) | (0<<WGM11) | (0<<WGM10);
  TCCR1B = (0<<ICNC1) | (0<<ICES1) | (0<<WGM13) | (0<<WGM12) | (0<<CS12) | (0<<CS11) | (1<<CS10);
  DDRB.7 = 1; // настройка 7 ножки порта B на выход
  // тут начинается обработка сэмпла
  while(_TRUE) {
   TCNT1 = 0; // обнумяем счетный регистр
   f_err_code = f_read(&fil_obj,var,1,&ByteRead);
   OCR2 = var[0]; // загружаем в шим
   if (ByteRead == 0) break; // если конец файла, выходим
   while (TCNT1 < 1000); // задержка, которая зависит от частоты дискретизации.
  }
 }
 DDRB.7=0; // отключаем звук. Иначе ШИМ будет греть динамик.
 f_err_code = f_close(&fil_obj); // закрываем файл
 // настраиваем таймер 1 для работы с датчиком расстояния
 // Timer/Counter 1 initialization
 // Clock source: System Clock
 // Clock value: 2000,000 kHz
 TCCR1A=(0<<COM1A1) | (0<<COM1A0) | (0<<COM1B1) | (0<<COM1B0) | (0<<COM1C1) | (0<<COM1C0) | (0<<WGM11) | (0<<WGM10);
 TCCR1B=(0<<ICNC1) | (0<<ICES1) | (0<<WGM13) | (0<<WGM12) | (0<<CS12) | (1<<CS11) | (0<<CS10);
 };