#include <CyberLib.h>     

#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_rotation_left {D4_Low; D5_High; D6_Low; D7_High;}     
#define robot_rotation_right {D4_High; D5_Low; D6_High; D7_Low;}     

#define size_buff 5 //ðàçìåð ìàññèâà sensor     
uint16_t sensor[size_buff]; //ìàññèâ äëÿ õðàíåíèÿ çàìåðîâ äàëüíîìåðà     
uint8_t stat=0; //íàïðàâëåíèå ðàçâîðîòà     

void setup()       
{     
      motors_init;  //èíèöèàëèçàöèÿ âûõîäîâ ìîòîðîâ     
      D11_Out;     //äèíàìèê     
      D14_Out; D14_Low; //ïèí trig óëüòðàçâóêîâîãî ñîíàðà     
      D15_In;     //ïèí echo  óëüòðàçâóêîâîãî ñîíàðà     
      randomSeed(A6_Read); //Ïîëó÷èòü ñëó÷àéíîå çíà÷åíèå     
      for(uint8_t i=0; i<12; i++) beep(50, random(100, 1000)); //çâóêîâîå îïîâåùåíèå ãîòîâíîñòè ðîáîòà     
      wdt_enable (WDTO_500MS);    //Ñòîðîæåâàÿ ñîáàêà 0,5ñåê.     
}     

void loop()     
{Start       
      uint16_t dist=GetDistance(); //ïðîèçâîäèì çàìåð äèñòàíöèè     

        if( dist < 10) {rotation(stat, 255);} else   //åñëè 10ñì ìàêñèìàëüíûé óãîë ðàçâîðîòà     
        if( dist < 20) {rotation(stat, 200);} else   //åñëè 20ñì  ñðåäíèé óãîë ðàçâîðîòà     
        if( dist < 40) {rotation(stat, 130);} else   //åñëè 40ñì  ìèíèìàëüíûé óãîë ðàçâîðîòà     
                       {robot_go; stat=~stat;}       //ïîåõàëè!!!           
         wdt_reset(); //ñòîðîæåâîé òàéìåð     
End;}     
//***************************************************     
void rotation(uint8_t arr, uint8_t dur)      
{     
        switch (arr) //ñìîòðèì â êàêîì íàïðàâëåíèå ðàçâîðà÷èâàòüñÿ     
        {     
        case 0:    robot_rotation_right;     
          break;     
        case 255:    robot_rotation_left;      
          break;         
        }      
        delay_ms(dur);    //óãîë ðàçâîðîòà     
        robot_stop;      //ñòîï ìîòîð!     
}     
//***************************************************     
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, 2400); //ñ÷èòûâàåì äëèòåëüíîñòü âðåìåíè ïðîõîæäåíèÿ ýõà, îãðàíè÷èòü âðåìÿ îæèäàíèÿ     
       if(dist==0) dist=2400;       
       sensor[i]=dist;  //ñîõðàíèòü â ìàññèâå     
       delay_ms(40); //çàäåðæêà ìåæäó ïîñûëêàìè     
       wdt_reset(); //ñòîðîæåâîé òàéìåð
      }     
      dist=(find_similar(sensor, size_buff, 58))/58; // //ôèëüòðóåì ïîêàçàíèÿ äàò÷èêà è ïåðåâîäèì â ñì     
      return dist;     
}      
//***************************************************       
void beep(uint8_t dur, uint16_t frq)     
{     
      dur=(1000/frq)*dur;  //ðàññ÷åò äëèòåëüíîñòè áèïà     
      for(byte i=0; i<dur; i++)     
      {     
        D11_High;      
        delay_us(frq);      
        D11_Low;     
        delay_us(frq);     
      }      
}