//расположение датчиков
//
// 1 0 2
// \ | /
// [ ]
// / | \
// 3 4 5

unsigned char gporog[6]={100, 150, 150, 200, 125, 200}; //массив индивидуальных порогов срабатывания сенсора
//для белой картонки на 30 см
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}
};

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}
};

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< else sensor_state&=~(1< };
if(sensor_state!=0)
{
env_free=0; //глобальный флаг
step=0; //если препятствие есть, то обнуляем шаг
};
}

void delay_walk (unsigned int time) //на вход задержка милисекундах, макс 65535,
{ //схлапывающаяся задержка, обнуляется как только обнаруживается препятствие
gtime=time;
while(gtime!=0)
{
check_env();
if (sensor_state != sensor_state_last && sensor_enable == 1) //если произошло изменение значения сенсоров и они разрешены
{
delay_ms(200); //пусть машинка прокатится по инерции к препятствию 0,2с что бы исключить различную дальность срабатывания датчиков
break;
}; //если произошло изменение в показаниях сенсоров то рвем задержку
}; //время проверки окружения 20 мкс (0,2мс)
}

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

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

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

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

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

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

void stop(int t) //стоп
{
PORTD.1=0;
PORTD.2=0;
PORTD.3=0;
PORTD.4=0;
if(t!=0) delay_walk(t);
}

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

void led_off(void) //Выключить светодиод
{
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;
};
}

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

void start(void) //начинаем движение
{
while(1)
{
check_env();
decision();
h=next_move();
go(h);
led_on();
delay(100);

led_off();
};
};
}

void decision(void) //функция, которая будет принимать решения на базе показаний датчиков
{
if (sensor_state==0b00000010) //если сработал какой то датчик с боков то запрещаем движение соотв бока
disable_FL();
if (sensor_state==0b00000100)
disable_FR();
if (sensor_state==0b00001000)
disable_BL();
if (sensor_state==0b00100000)
disable_BR();
//ниже уже срабатывание 2 и более датчиков
if (sensor_state==0x03 || sensor_state==0x07 || sensor_state==0x05 || sensor_state==0x01)
disable_allF();

if (sensor_state==0x18 || sensor_state==0x10 || sensor_state==0x30 || sensor_state==0x38)
disable_allB();
//env_free у нас сбрасывается в 0 если есть препятствие
if (sensor_state==0 && env_free!=1) //если сенсторы дают ноль но вокруг где то препятствие было
{
restore();
};
if (sensor_state!=0) //в любом случае если сработал датчик надо остановиться подумать
{
now=0;
stop();
};

}

void main(void)
{
int i;
// инициализация портов
PORTB=0x00;
DDRB=0x00;
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();

};

}