#include <CyberLib.h>
#define encoders_init D2_In; D3_In; D2_High; D3_High
#define motors_init D4_Out; D5_Out; D6_Out; D7_Out
#define robot_go D4_Low; D5_High; D6_High; D7_Low
#define robot_stop D4_Low; D5_Low; D6_Low; D7_Low
#define robot_left D4_Low; D5_High; D6_Low; D7_Low
#define robot_right D4_Low; D5_Low; D6_High; D7_Low
#define robot_rotation_left D4_Low; D5_High; D6_Low; D7_High

#define size_buff 5 //размер массива sensor
uint16_t sensor[size_buff]; //массив для хранения замеров дальномера
volatile int8_t course=0;

void setup()
{
motors_init; //инициализация выходов моторов
encoders_init; //инициализация входов Энкодеров
D14_Out; D14_Low; //пин trig ультразвукового сонара
D15_In; //пин echo ультразвукового сонара
for(uint8_t i=0; i<12; i++) beep(70, random(100, 2000)); //звуковое оповещение готовности робота
IntOn(); //включить прерывания
robot_go; //поехали!!!
}

void loop()
{Start
uint16_t dist=GetDistance(); //делаем замер
if( dist < 40) Rotation(); //если дистанция меньше 38см
if( dist > 39 && course==0) robot_go;
End;}

//***************************************************
void MotorL() // обработка внешнего прерывания левое колесо
{
course--;
if(course > 0) { robot_left; }else{ robot_right;}
}

void MotorR() // обработка внешнего прерывания правое колесо
{
course++;
if(course < 0){ robot_right; }else{ robot_left;}
}
//***************************************************
uint16_t GetDistance()
{
uint16_t dist;
for (uint8_t i = 0; i < size_buff; ++i) //производим несколько замеров
{
D14_High; delay_us(10); D14_Low; //запустить измерение
dist = pulseIn(15,HIGH); //считываем длительность времени прохождения эха
delay_ms((50000-dist)/1000); //задержка между посылками
sensor[i]=dist;
}
dist=(find_similar(sensor, size_buff, 58))/58; //фильтруем показания датчика и переводим в см
return dist;
}
//***************************************************
void Rotation()
{
IntOff();
robot_rotation_left;
delay_ms(180);
robot_stop;
course=0;
IntOn();
}

//***************************************************
void IntOn()
{
attachInterrupt(0, MotorL, RISING); // настроить срабатывание прерывания interrupt0 pin 2 по перерпапду с низкого (Low) на высокий(HIGH)
attachInterrupt(1, MotorR, RISING); // настроить срабатывание прерывания interrupt1 pin 3 по перерпапду с низкого (Low) на высокий(HIGH)
}

void IntOff()
{
detachInterrupt(0); // отключить срабатывание прерывания interrupt0
detachInterrupt(1); // отключить срабатывание прерывания interrupt1
}