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();

}