#include <inttypes.h>
#include <avr/io.h>
#include <avr/interrupt.h>
#include <util/delay.h>
#include <stdlib.h>

#define sbi(port, bit) (port) |= (1 << (bit))
#define cbi(port, bit) (port) &= ~(1 << (bit))

#define Enable1 PB3
#define Enable2 PB4
#define Input1  PB2
#define Input2  PB1

volatile unsigned char Lspd, Rspd;

// Запуск обоих двигателей вперед
void Forward(void)
{
   sbi(PORTB, Input1);
   sbi(PORTB, Input2);
   OCR1AL = Lspd;
   OCR1BL = Rspd;
}

// Запуск обоих двигателей назад
void Backward(void)
{
   cbi(PORTB, Input1);
   cbi(PORTB, Input2);
   OCR1AL = Lspd;
   OCR1BL = Rspd;
}

// правый поворот
void TurnRight(void)
{
   cbi(PORTB, Input1);
   sbi(PORTB, Input2);
   OCR1AL = Lspd;
   OCR1BL = Rspd;
}

// левый поворот
void TurnLeft(void)
{
   sbi(PORTB, Input1);
   cbi(PORTB, Input2);
   OCR1AL = Lspd;
   OCR1BL = Rspd;
}

// Стоп обоих двигателей 
void Stop(void)
{
   OCR1AL = 1;
   OCR1BL = 1;
   
}

void main(void)
{
   // инициализация 
   DDRB = 0xFF;         // Настраиваем весь порт B на выход 
   PORTB = 0x00;        // и гасим всё?

   DDRD =  0x00;        // все на вход
   PORTD = 0xFF;        // все через подтягивающий внутренний резюк

   // скорость вращения будем задавать ШИМом
   TCCR1A = (1<<COM1A1) | (1<<COM1B1) | (1<<WGM10);
   TCCR1B = (1<<WGM12) | (1<<CS10);
   OCR1AH = 0;
   OCR1BH = 0;
   //ICR1L = 255;
   Lspd=165;
   Rspd=165;
   while(1)             // крутимся в бесконечном цикле
   {
      Forward();

      if(!(PIND & (1<<PD6)))
      {
         Stop();
         _delay_ms(100);
         Backward();
         _delay_ms(100);
         Stop();
         _delay_ms(100);
         TurnRight();
         _delay_ms(100);
         Stop();
         _delay_ms(100);
         //TurnLeft();
         //_delay_ms(100);
         //Stop();
         //_delay_ms(100);
      }
   }
}