//------------------------------------------------------------------------
// Окончательный вариант программы для простого мобильного робота
//
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 PORTA
#define MOTOR_F PA1
#define MOTOR_B PA2
#define TURN_R PA3
#define TURN_L PA4
#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 8000000
#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(150); // отъезд в течение 2.5 х 2 сек
Delay_10ms(150);
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(150); // отъезд в течение 2.5 х 2 сек
Delay_10ms(150);
this_move=F;
}
//------------------------------------------------------------------------------
// Главная программа
//------------------------------------------------------------------------------
int main(void)
{
DDRA = 0xff; // назначаем все линии порта B на выход
PORTA = 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);
}
