roboforum.ru

//------------------------------------------------------------------------
// Окончательный вариант программы для простого мобильного робота
// http://projects.roboclub.ru/index.shtml?attempt
// (с) RodoClub.ru 2003
//------------------------------------------------------------------------

#include <inttypes.h>
#include <io.h>
#include <sig-avr.h>
#include <interrupt.h>
#include <stdlib.h>

// назначение определений для удобства работы с периферией
#define OUT PORTB
#define MOTOR_F  PB7
#define MOTOR_B  PB6
#define TURN_R   PB5
#define TURN_L   PB4

#define IN PIND
#define LIGHT_R  PD0
#define LIGHT_L  PD1
#define BUMPER_F PD2
#define BUMPER_B PD3

// Возможные режимы движения
enum {STOP, F, FR, FL, B, BR, BL};

//------------------------------------------------------------------------------
// Задержка t х 10ms
//------------------------------------------------------------------------------
#define F_CPU 4000000
#define K_DELAY_10ms   F_CPU/600
void Delay_10ms(unsigned char t) {
  unsigned int i;
  if (t==0) return;
  while (t--) for(i=0;i<K_DELAY_10ms; i++);
}

// таблица вероятностей для выбора направления движения
// исходя из текущего направления движения
unsigned char p[7][7] = {
{14,   43,   57,   71,   86,   93,   100},
{7,   43,   71,   100,   100,   100,   100},
{7,   50,   93,   100,   100,   100,   100},
{7,   50,   57,   100,   100,   100,   100},
{29,   29,   29,   29,   57,   79,   100},
{36,   36,   36,   36,   71,   93,   100},
{36,   36,   36,   36,   71,   79,   100},
};

// текущее направление движения
unsigned char this_move; 

//------------------------------------------------------------------------------
// Включение комбинации моторов для движения в заданном направлении
//------------------------------------------------------------------------------
void go(unsigned char direction){

  switch (direction) {
  case STOP:
    cbi(OUT, MOTOR_F);
    cbi(OUT, MOTOR_B);
    cbi(OUT, TURN_R);
    cbi(OUT, TURN_L);
    break;

  case F:
    sbi(OUT, MOTOR_F);
    cbi(OUT, MOTOR_B);
    cbi(OUT, TURN_R);
    cbi(OUT, TURN_L);
    break;

  case FR:
    sbi(OUT, MOTOR_F);
    cbi(OUT, MOTOR_B);
    sbi(OUT, TURN_R);
    cbi(OUT, TURN_L);
    break;

  case FL:
    sbi(OUT, MOTOR_F);
    cbi(OUT, MOTOR_B);
    cbi(OUT, TURN_R);
    sbi(OUT, TURN_L);
    break;

  case B:
    cbi(OUT, MOTOR_F);
    sbi(OUT, MOTOR_B);
    cbi(OUT, TURN_R);
    cbi(OUT, TURN_L);
    break;

  case BR:
    cbi(OUT, MOTOR_F);
    sbi(OUT, MOTOR_B);
    sbi(OUT, TURN_R);
    cbi(OUT, TURN_L);
    break;

  case BL:
    cbi(OUT, MOTOR_F);
    sbi(OUT, MOTOR_B);
    cbi(OUT, TURN_R);
    sbi(OUT, TURN_L);
    break;
  }
}

//------------------------------------------------------------------------------
// Выбор направления движения в следующем шаге по таблице вероятностей
//------------------------------------------------------------------------------
unsigned char next_move(void){
   unsigned char pp, i;

   pp = rand()/327;     // получаем случайное число 0..99
   for (i=0;i<7;i++){   // ищем соответствие в таблице вероятностей
      if (p[this_move][i] > pp) break;
   }
   this_move = i;       // записываем новое полученное направление как текущее
   return(i);
}

//------------------------------------------------------------------------------
// Обработка прерывания от переднего бампера (INT0 = PD2)
//------------------------------------------------------------------------------
SIGNAL(SIG_INTERRUPT0)
{
   if(this_move==FR) go(BL);
   if(this_move==FL) go(BR);
   else go(B);
   Delay_10ms(250);    // отъезд в течение 2.5 х 2 сек
   Delay_10ms(250);
   this_move=B;
}

//------------------------------------------------------------------------------
// Обработка прерывания от заднего бампера (INT1 = PD3)
//------------------------------------------------------------------------------
SIGNAL(SIG_INTERRUPT1)
{
   if(this_move==BR) go(FL);
   if(this_move==BL) go(FR);
   else go(F);
   Delay_10ms(250);    // отъезд в течение 2.5 х 2 сек
   Delay_10ms(250);
   this_move=F;
}

//------------------------------------------------------------------------------
// "Случайное блуждание"
//------------------------------------------------------------------------------
unsigned char walk(void){
   // этот цикл организует "свободное блуждание" пока
   // нет сигнала ни от одного из датчиков освещенности
   while((bit_is_set(IN, LIGHT_R)) && (bit_is_set(IN, LIGHT_L))){
       go(next_move());   // получаем следующее направление движения и
       Delay_10ms(250);   // движемся в этом направлении 2.5 сек
   }
   // этот цикл организует движение на свет, пока
   // есть сигнал хотя бы от одного из датчиков освещенности
   while((bit_is_clear(IN, LIGHT_R)) || (bit_is_clear(IN, LIGHT_L))){
       if((bit_is_clear(IN, LIGHT_R)) && (bit_is_clear(IN, LIGHT_L))) go(F);
       else if(bit_is_clear(IN, LIGHT_R)) go(FR);
       else if(bit_is_clear(IN, LIGHT_L)) go(FL);
   }

   return(0);
}

//------------------------------------------------------------------------------
// Главная программа
//------------------------------------------------------------------------------
int main(void)
{
  DDRB  = 0xff;  // назначаем все линии порта B на выход
  PORTB = 0x00;  // и устанавливаем на них низкий уровень

  DDRD  = 0x00;  // назначаем все линии порта D на вход
  PORTD = 0xff;  // подключаем внутренние нагрузочные резисторы

  // разрешаем прерывания int0 и int1
  outb(GIMSK, (1<<INT0)|(1<<INT1)); 

  // запрос на прерывание - по спадающим фронтам на int0 и int1
  outb(MCUCR, (1<<ISC01)|(1<<ISC11));

  // разрешаем прерывания
  sei();          

  // запускаем главный цикл
  while(1) walk();

}

c+=(f-- + r << ++v)>0 ? 43%f : 0