/*****************************************************
This program was produced by the
CodeWizardAVR V1.25.7 beta 5 Professional
Automatic Program Generator
© Copyright 1998-2007 Pavel Haiduc, HP InfoTech s.r.l.
http://www.hpinfotech.com

Project : 
Version : 
Date    : 02.03.2008 - 14.04.2008
Author  : Spiritus Sancti                            
Company : licrym.org                            
Comments: CC BY-NC-SA 


Chip type           : ATmega8
Program type        : Application
Clock frequency     : 16,000000 MHz
Memory model        : Small
External SRAM size  : 0
Data Stack size     : 256
*****************************************************/

#include <mega8.h>
#include <stdlib.h>
#include <delay.h> 

#define ADC_VREF_TYPE 0x40   

#define gTimeOfMove 976       //время элементарного хода в мсек (1 сек)  
#define StepsOfRecover 4      //количество шагов, за которые восстанавливается направление 




unsigned int gtime; 
bit ADC_complete;                     
unsigned char sensor_state, sensor_state_last;     //переменная состояния датчиков, соответствующий бит отвечает за соответствующий датчик
unsigned int gADC_result;
unsigned char now, h;
//расположение датчиков
//
//      1   0   2
//        \ | /
//         [ ]
//        / | \
//       3  4  5

const unsigned char gporog[6]={30, 30, 30, 30, 30, 30};       //массив индивидуальных порогов срабатывания сенсора
                                                                    //для белой картонки на 30 см    /-50
                                                                    //const unsigned char gporog[6]={50, 100, 100, 150, 75, 150}; 
unsigned char d[7][7]={  //таблица вероятностей переходов, строки - текущее движение, столбцы - следующее.
//          STO    F   FR    FL    B   BR   BL
/*STO*/     {10,  15,  15,   15,  15,  15,  15},
/*F  */     {17,  23,  25,   25,   0,   0,   0},
/*FR */     {17,  23,  25,   25,   0,   0,   0},
/*FL */     {17,  33,  25,   25,   0,   0,   0},
/*B  */     {25,   0,   0,    0,  25,  25,  25},
/*BR */     {25,   0,   0,    0,  25,  25,  25},
/*BL */     {25,   0,   0,    0,  25,  25,  25}
};   

const unsigned char etalon[7][7]={  //таблица вероятностей переходов, образец из которого восстанавливать будем
//          STO    F   FR    FL    B   BR   BL
/*STO*/     {10,  15,  15,   15,  15,  15,  15},
/*F  */     {17,  23,  25,   25,   0,   0,   0},
/*FR */     {17,  23,  25,   25,   0,   0,   0},
/*FL */     {17,  33,  25,   25,   0,   0,   0},
/*B  */     {25,   0,   0,    0,  25,  25,  25},
/*BR */     {25,   0,   0,    0,  25,  25,  25},
/*BL */     {25,   0,   0,    0,  25,  25,  25}
};   



interrupt [TIM1_COMPA] void timer1_compa_isr(void)  //прерывание по таймеру раз в 1/976 сек
{
  if (gtime!=0) --gtime;   
}  
     
void delay (unsigned int time)                      //на вход задержка милисекундах, макс 65535
{   
  gtime=time;
  while(gtime!=0){};
}     



// ADC interrupt service routine
interrupt [ADC_INT] void adc_isr(void)              //прерывание по завершению АЦП
{ 
  unsigned int adc_data;
  // Read the AD conversion result
  adc_data=ADCW;
  gADC_result=adc_data;
  ADC_complete=1;
}
                                           
int poll_sensor(unsigned char SensorNumber)         //процедура опроса сенсора
{  
  int delta, result1, result2; 
  ADMUX&=0b11110000;                                //сбрасываем биты микшера перед выбором входа
  switch(SensorNumber)                              //включаем соответствующий вход
  { 
     case 0:
        ADMUX|=0b00000000;
        break;
     case 1:
        ADMUX|=0b00000001;
        break;
     case 2:
        ADMUX|=0b00000010;
        break;
     case 3:
        ADMUX|=0b00000011;
        break;
     case 4:
        ADMUX|=0b00000100;
        break;            
     case 5:
        ADMUX|=0b00000101;
        break;
     default:
        ADMUX|=0b00000000;
        break;
  };
  PORTD|=1<<5;                                      //Светодиод на 5 ноге порта D
  delay_us(50);                                     //ждем 50 мкс пока светодиод разгорится
  ADCSRA|=0x40;                                     //записываем бит запускающий процесс измерения
  while(ADC_complete!=1);                           //ждем пока не закончится измерение, проверяя флаг
  result1=gADC_result;
  ADC_complete=0;                                   //сбрасываем флаг
 
  PORTD&=~(1<<5);                                   //выключаем светодиод и снова измеряем
  delay_us(10);
  ADCSRA|=0x40;
  while(ADC_complete!=1);
  result2=gADC_result;
  delta=result2-result1;                            //Если вдруг из-за шумов у нас result2<result1 то 
  if (delta < 0) delta=0;                           //получается отрицательное значение и сбой. Это грабля.
  ADC_complete=0;
  return delta;                                     //возвращаем значение разности 
} 

void show_sensors (void)                            //вывести значения сенсоров на светодиоды
{
  if(sensor_state & 0b00000001) PORTD|=1<<6; else PORTD&=~(1<<6);
  if(sensor_state & 0b00000010) PORTD|=1<<7; else PORTD&=~(1<<7);
  if(sensor_state & 0b00000100) PORTB|=1<<0; else PORTB&=~(1<<0);
  if(sensor_state & 0b00001000) PORTB|=1<<1; else PORTB&=~(1<<1);
  if(sensor_state & 0b00010000) PORTB|=1<<2; else PORTB&=~(1<<2);
  if(sensor_state & 0b00100000) PORTB|=1<<3; else PORTB&=~(1<<3);
}

void check_env(void)                                //процедура проверки окружающего пространства 
{   
  unsigned char i, a, b, c;

  for(i=0; i<6; i++)                                //в цикле опросим все сенсоры
  {      
    a=poll_sensor(i);                               //опрос сенсора 3 раза для исключения шума
    b=poll_sensor(i);                               //по результатам выставляем глобальный флаг с номером сработавших сенсоров
    c=poll_sensor(i);
    if(c>=gporog[i]) c=1; else c=0;
    if(b>=gporog[i]) b=1; else b=0;
    if(a>=gporog[i]) a=1; else a=0;
    c=c+b+a;
    if(c>=2)sensor_state|=1<<i; 
     else sensor_state&=~(1<<i);                    //если сенсор сработал то 1 в соотв бит переменной состояний, если нет - то сбратываем соотв бит
  }; 
  show_sensors();
}
  
void delay_walk (unsigned int time)                 //на вход задержка милисекундах, макс 65535, 
{   //схлапывающаяся задержка, обнуляется как только обнаруживается препятствие
  gtime=time;
  while(gtime!=0)
  {
      check_env();
      if (sensor_state != sensor_state_last)        //Если текущее состояние сенсоров необработано - рвем задержку
      {                                             //время проверки окружения 50 мкс (0,5мс)
        break;
      };                              
  };                                   
} 

//далее функции управления двигателями
void f(int t)                                       // Вперед
{   
 PORTD&=~(1<<1); //PORTD.1=0;
 PORTD|=1<<2;    //PORTD.2=1;
 PORTD&=~(1<<3); //PORTD.3=0;
 PORTD&=~(1<<4); //PORTD.4=0;
 if(t!=0) delay_walk(t);
}

void b(int t)                                       // назад
{  
 PORTD|=1<<1;    //PORTD.1=1;
 PORTD&=~(1<<2); //PORTD.2=0;
 PORTD&=~(1<<3); //PORTD.3=0;
 PORTD&=~(1<<4); //PORTD.4=0;
 if(t!=0) delay_walk(t);
}

void fl(int t)                                      // вперед налево
{   
 PORTD&=~(1<<1); //PORTD.1=0;
 PORTD|=1<<2;    //PORTD.2=1;
 PORTD&=~(1<<3); //PORTD.3=0;
 PORTD|=1<<4;    //PORTD.4=1;
 if(t!=0) delay_walk(t);
} 

void fr(int t)                                      // вперед направо
{  
 PORTD&=~(1<<1); //PORTD.1=0;
 PORTD|=1<<2;    //PORTD.2=1;
 PORTD|=1<<3;    //PORTD.3=1;
 PORTD&=~(1<<4); //PORTD.4=0;
 if(t!=0) delay_walk(t);
}  

void bl(int t)                                      // назад налево
{ 
 PORTD|=1<<1;    //PORTD.1=1;
 PORTD&=~(1<<2); //PORTD.2=0;
 PORTD&=~(1<<3); //PORTD.3=0;
 PORTD|=1<<4;    //PORTD.4=1;
 if(t!=0) delay_walk(t);
} 

void br(int t)                                      // назад направо
{ 
 PORTD|=1<<1;    //PORTD.1=1;
 PORTD&=~(1<<2); //PORTD.2=0;
 PORTD|=1<<3;    //PORTD.3=1;
 PORTD&=~(1<<4); //PORTD.4=0;
 if(t!=0) delay_walk(t);
}
 
void stop(int t)                                    //стоп
{ 
 PORTD&=~(1<<1); //PORTD.1=0;
 PORTD&=~(1<<2); //PORTD.2=0;
 PORTD&=~(1<<3); //PORTD.3=0;
 PORTD&=~(1<<4); //PORTD.4=0;
 if(t!=0) delay_walk(t);
} 

void led_on(void)                                   //Включить светодиод            
{ 
 PORTD|=1<<0;    //PORTD.0=1;
} 

void led_off(void)                                  //Выключить светодиод
{ 
PORTD&=~(1<<0); //PORTD.0=0;
}    

void go(unsigned char napr)                         //Выборка направления и отдача команды двигателям  
{     
  switch(napr)
  {
     case 0:
        stop(gTimeOfMove);
        break; 
     case 1:
        f(gTimeOfMove);
        break;
     case 2:
        fr(gTimeOfMove);
        break;
     case 3:
        fl(gTimeOfMove);
        break;
     case 4:
        b(gTimeOfMove);
        break;
     case 5:
        br(gTimeOfMove);
        break;
     case 6:
        bl(gTimeOfMove);
        break;
  };
}   
                                                     
void disable_way(unsigned char col)                     //функция, запрещающая полученую колонку таблицы
{
unsigned char zeroes, notZeroes, i, s, part;
   for (s = 0; s <= 6; s++)                             //для каждой строки таблицы вероятностей
   { 
     for (i=0; i<=6; i++)                               //для каждой колонки текущей строки найдем кол-во нулей, за исключением текущей 
     { 
       if (d[s][i]==0 && i!=col) zeroes++;
     };
     notZeroes = 6 - zeroes;                            //кол-во ненулей
     part = d[s][col] / notZeroes;                      //порция, на которую наращиваем
     
     for (i=0; i<=6; i++)
     {                                             
       if (d[s][i]!=0 && i!=col) d[s][col]+=part;       //наращиваем
     };                                             
     
     d[s][col]=0;                                       //собственно само обнуление
   
   };
}   

void enable_way (unsigned char col)                     //функция восстановления значений таблицы вероятностей
{unsigned char s;
   for (s=0; s<=6; s++)                                 //для каждой строки таблицы вероятностей
   {        
        if (d[s][col] != etalon[s][col])
        { 
            if (d[s][col] > etalon[s][col])
            {
            d[s][col]-= etalon[s][col] / StepsOfRecover;
            };
            
            
            if (d[s][col] < etalon[s][col]) 
            {
            d[s][col]+= etalon[s][col] / StepsOfRecover;
            };
                      
        };
               
   };
  
}




unsigned char next_move(void)                           //выясняем следующий ход     
{   
  int a, b, c;
  
  do
  {
        a = rand()/327; 		                //0..99 число 
        c=0;                                                
        for (b=0; b<7; b++)                             //поик по таблице
        {      	  
            if(a > c && a < (d[now][b]+c) ) break;
            c=c+d[now][b];
        };
  }
  while(b==7);                                          //Если сумма вероятностей в строке не 100, то возможно пролететь всю таблицу без выбора и тогда b==7. Если такое случается, то снова ищем но с новым случайным числом
  
  now = b;
  return b;
};    

 
void decision(void)                                     //функция, которая будет принимать решения на базе показаний датчиков
{ 
  /* //Кусок кода, для распознавания препятствий по 6 направлениям
  if(sensor_state & 0b00000010) disable_way(3); else enable_way(3);   //FL sensor
  if(sensor_state & 0b00000100) disable_way(2); else enable_way(2);   //FR sensor
  if(sensor_state & 0b00001000) disable_way(6); else enable_way(6);   //BL Sensor
  if(sensor_state & 0b00100000) disable_way(5); else enable_way(5);   //BR Sensor
  if(sensor_state & 0b00010000) 
  {
    disable_way(4);
    disable_way(5);
    disable_way(6);
     
  } else enable_way(4);   //B sensor
  if(sensor_state & 0b00000001)
  {
    disable_way(1);
    disable_way(2);
    disable_way(3);
     
  } else enable_way(1);   //F sensor
  */
  
  //Кусок кода для распознавания препятствий по 2 направлениям
  if (sensor_state & 0b00000001 || sensor_state & 0b00000010 || sensor_state & 0b00000100)    //front
  {
    disable_way(1);
    disable_way(2);
    disable_way(3);
  } else 
  {
    enable_way(1);
    enable_way(2);
    enable_way(3);
  };
  
  if (sensor_state & 0b00010000 || sensor_state & 0b00001000 || sensor_state & 0b00100000) //back
  { 
    disable_way(4);
    disable_way(5);
    disable_way(6);
  } else
  {
    enable_way(4);
    enable_way(5);
    enable_way(6);
  };
  
  sensor_state_last = sensor_state;
  
} 

 
void start(void)                                        //начинаем движение
{   
  while(1)
  {                                                     //проверяем окружение, обрабатываем показания датчиков, получаем случайное направление и идем по нему
     check_env();
     decision();
     h=next_move();
     go(h);
     led_on();
     delay(100);
     led_off();
     
  };
}  


void main(void)
{  
  int i;
 // unsigned int delta;
  // инициализация портов
  PORTB=0x00;
  DDRB=0xFF;
  PORTC=0x00;
  DDRC=0x00;
  PORTD=0x00;
  DDRD=0xFF;
  TCCR0=0x00;
  TCNT0=0x00;

  // Timer/Counter 1 initialization
  // Clock source: System Clock
  // Clock value: 15,625 kHz
  // Mode: Normal top=FFFFh
  // OC1A output: Discon. 
  // OC1B output: Discon.
  // Noise Canceler: Off
  // Input Capture on Falling Edge
  // Timer 1 Overflow Interrupt: Off
  // Input Capture Interrupt: Off
  // Compare A Match Interrupt: On
  // Compare B Match Interrupt: Off
 
  TCCR1A=0x00; 
  TCCR1B=0x0D;
  TCNT1H=0x00;
  TCNT1L=0x00;
  ICR1H=0x00;
  ICR1L=0x00;
  OCR1AH=0x00;
  OCR1AL=0x0F;
  OCR1BH=0x00;
  OCR1BL=0x00;
 
  // Timer/Counter 2 initialization
  // Clock source: System Clock
  // Clock value: Timer 2 Stopped
  // Mode: Normal top=FFh
  // OC2 output: Disconnected
  ASSR=0x00;
  TCCR2=0x00;
  TCNT2=0x00;
  OCR2=0x00;

  // External Interrupt(s) initialization
  // INT0: Off
  // INT1: Off
  MCUCR=0x00;

  // Timer(s)/Counter(s) Interrupt(s) initialization
  TIMSK=0x10;

  // Analog Comparator initialization
  // Analog Comparator: Off
  // Analog Comparator Input Capture by Timer/Counter 1: Off
  ACSR=0x80;
  SFIOR=0x00;

  // ADC initialization
  // ADC Clock frequency: 1000,000 kHz
  // ADC Voltage Reference: AVCC pin
  ADMUX=ADC_VREF_TYPE & 0xff;
  ADCSRA=0x8C;

  // Global enable interrupts
  #asm("sei")
 

  while (1)
  {
      now=1;
      for (i=0;i<=5;i++)                                //Помигаем светодиодом после включения, заодно задержка перед троганием что бы успеть отбежать
      { 
        led_on();
        delay(500);
        led_off();
        delay(500);
       }; 
        
      start();   
      
  
  };
}