roboforum.ru

#define M_F  PORTB.7   //обозначения
#define M_B  PORTB.6   //принятые для удобства восприятия
#define T_R  PORTB.5   //компилятор вместо первого слова поставит второе
#define T_L  PORTB.4   //т.е. вместо T_L будет PORTB.4
#define C_D  PIND.6

// Declare your global variables here
unsigned char this_move; //запоминаем текущее движение
enum {L,R,C};                  //массив видов движений

void turn(unsigned char direction) //объявляем ф-ю поворота
{
switch (direction) //выбор из вариантов ниже
{
case L: //первый вариант
    T_L=1; //запускаем поворотные двигатели
    T_R=0;
    if (this_move==R){delay_ms(300);};  //ждем некоторое время(измеряется опытным путем)
    if (this_move==C){delay_ms(150);};
    T_L=0;  //останавливаем двигатели
    T_R=0;
    this_move=L;  //теперь мы на левой позиции
    break;
case R: //второй вариант
    T_L=0; //аналогично
    T_R=1;
    if (this_move==L){delay_ms(300);};
    if (this_move==C){delay_ms(150);};
    T_L=0;
    T_R=0;
    this_move=R;
    break;
case C: //третий вариат
    switch (this_move) //выбираем где мы находимся
     {
     case L: //если слева, то
        T_L=0; //запускаем поворот вправо
        T_R=1;
        while (C_D){}; //ждем пока датчик не пискнет о том, что достигли центра
        T_L=0;T_R=0; //останавливаем двигатель
        this_move=C; //теперь мы на центре
        break;
     case R:
        T_L=1;
        T_R=0;
        while (C_D){};
        this_move=C;
        break;
     }
    break;
}
}

void init(void)
{
T_L=1;T_R=0;   //поворачиваем влево
delay_ms(300); //ждем
T_L=0;T_R=1;  //поворачиваем вправо
while(C_D){};   //до центра
T_L=0;T_R=0;  //и останавливаем движок
this_move=C; //теперь рулевая ось в центре
}