roboforum.ruТехнический форум по робототехнике. |
|
|
AsonD писал(а):В общем-то частично я уверен в правильности этого, но я не уверен в правильном подключении блока l293d с фотоэлементами и еще я не уверен с количеством тактов за секунду, я примерно экспериментально мерял, дак получаетсо что за 2,5 секунды МК делает в районе 6500 тактов... а как будет в реале хз
/*****************************************************************************************************************
// Программа для мобильного робота (пока только ездит на свет; свободный поиск в разработке)
// Компилятор: CodeVision AVR
// (с) ASAR.my1.ru 2008
******************************************************************************************************************/
#include "mega16.h"
#include "delay.h"
// обьявляем переменные
int p;
long int g;
int x = 0;
int y = 6;
int Q = 0;
/****************
Таблица вероятностей
*******************/
int ai[5][6] = {
{ 1, 5, 2, 4, 3, 2 },
{ 5, 3, 6, 2, 3, 1 },
{ 2, 4, 3, 1, 1, 2 },
{ 3, 2, 1, 5, 3, 4 },
{ 1, 1, 5, 3, 2, 6 }
};
/*********************************************************************************************************
Варианты выбора направления
*********************************************************************************************/
void F() // Forward - движение вперед
{
PORTD.0 = 1;
PORTD.1 = 0;
PORTD.2 = 0;
PORTD.3 = 0;
PORTD.5 = 0;
PORTD.6 = 1;
}
void B() // Backward - движение назад
{
PORTD.0 = 0;
PORTD.1 = 1;
PORTD.2 = 0;
PORTD.3 = 0;
PORTD.6 = 0;
PORTD.5 = 1;
}
void L() // движение просто влево и вправо - запасные(просто так их использовать не получится), вдруг пригодятся))
{
PORTD.0 = 0;
PORTD.1 = 0;
PORTD.2 = 1;
PORTD.3 = 0;
}
void R()
{
PORTD.0 = 0;
PORTD.1 = 0;
PORTD.2 = 0;
PORTD.3 = 1;
}
void FL() // Forward-Left - движение вперед-влево
{
PORTD.0 = 1;
PORTD.1 = 0;
PORTD.2 = 1;
PORTD.3 = 0;
PORTD.5 = 0;
PORTD.6 = 1;
}
void FR() // Forward-Right - движение вперед-вправо
{
PORTD.0 = 1;
PORTD.1 = 0;
PORTD.2 = 0;
PORTD.3 = 1;
PORTD.5 = 0;
PORTD.6 = 1;
}
void BL() // Backward-Left - движение назад-влево
{
PORTD.0 = 0;
PORTD.1 = 1;
PORTD.2 = 1;
PORTD.3 = 0;
PORTD.5 = 1;
PORTD.6 = 0;
}
void BR() // Backward-Right - движение назад-вправо
{
PORTD.0 = 0;
PORTD.1 = 1;
PORTD.2 = 0;
PORTD.3 = 1;
PORTD.5 = 1;
PORTD.6 = 0;
}
void STOP() // стоп
{
PORTD = 0x00;
}
/**********************************
Выбор случайного направления
*********************************/
void snum()
{
Q++; // каждый цикл этой переменной будет добавляться единица
if (Q == 6500) // 6 500 взято на вскидку экспериментально в протеусе; примерно за 2,5 секунды программа сделает 6 500 циклов
{
Q = 0; //после проделывания 6500 циклов переменная обнуляется и отчет начинается сначала (и так до бесконечности)
++x; //каждые 6500 циклов или 2,5 секунды добавляем переменной единицу
if (x == 6) //каждый раз как x будет набирать число 6 по единицам, от у будет вычитаться единица
{
x = 0;//обнуляем переменную
--y; // вычитаем 1 (каждый цикл при х = 6)
}
if (y == -1) // если у становится меньше нуля - возвращаем переменной первоначальное значение, а тоесть 6
{y = 6;}
}
p = ai[x][y]; //переменная будет равна такому числу в таблице вероятностей, где x -номер числа по горизонтали, а у - по вертикали
}
/*******************************************
Обработка информации с контактных датчиков
***********************************************/
void contact()
{
//*********************************************************
if ((PINB.4 == 1)&&(PINB.5 == 0)&&(PINB.6 == 0)&&(PINB.7 == 0))
{
BL();
delay_ms(150);
FR();
delay_ms(150);
F();
delay_ms(50);
}
//*********************************************************
if ((PINB.5 == 1)&&(PINB.4 == 0)&&(PINB.6 == 0)&&(PINB.7 == 0))
{
FR();
delay_ms(150);
F();
delay_ms(50);
}
//*********************************************************
if ((PINB.6 == 1)&&(PINB.5 == 0)&&(PINB.4 == 0)&&(PINB.7 == 0))
{
FL();
delay_ms(150);
F();
delay_ms(50);
}
//*********************************************************
if ((PINB.7 == 1)&&(PINB.5 == 0)&&(PINB.6 == 0)&&(PINB.4 == 0))
{
BR();
delay_ms(150);
FL();
delay_ms(150);
F();
delay_ms(50);
}
//*********************************************************
}
/******************************
Обработка информации с фотоэлементов
*********************************/
void photo()
{
if ((PINB.0 == 0)&&(PINB.1 == 1)&&(PINB.2 == 1)&&(PINB.3 == 1)&&(PINB.4 == 0)&&(PINB.5 == 0)&&(PINB.6 == 0)&&(PINB.7 == 0))
{
STOP();
FR();
delay_ms(25);
STOP();
}
////////////////////////////////////////////////////////////*
if ((PINB.1 == 0)&&(PINB.0 == 1)&&(PINB.2 == 1)&&(PINB.3 == 1)&&(PINB.4 == 0)&&(PINB.5 == 0)&&(PINB.6 == 0)&&(PINB.7 == 0))
{
STOP();
FL();
delay_ms(25);
STOP();}
////////////////////////////////////////////////////////////*
if ((PINB.2 == 0)&&(PINB.1 == 1)&&(PINB.0 == 1)&&(PINB.3 == 1)&&(PINB.4 == 0)&&(PINB.5 == 0)&&(PINB.6 == 0)&&(PINB.7 == 0))
{
STOP();
BR();
delay_ms(100);
FL();
delay_ms(100);
STOP();}
////////////////////////////////////////////////////////////*
if ((PINB.3 == 0)&&(PINB.1 == 1)&&(PINB.2 == 1)&&(PINB.0 == 1)&&(PINB.4 == 0)&&(PINB.5 == 0)&&(PINB.6 == 0)&&(PINB.7 == 0))
{
STOP();
BL();
delay_ms(100);
FR();
delay_ms(100);
STOP();}
////////////////////////////////////////////////////////////*
if ((PINB.0 == 0) && (PINB.1 == 0)&&(PINB.2 == 1) && (PINB.3 == 1)&&(PINB.4 == 0)&&(PINB.5 == 0)&&(PINB.6 == 0)&&(PINB.7 == 0))
{
STOP();
F();
delay_ms(25);
STOP();}
////////////////////////////////////////////////////////////*
if ((PINB.2 == 0) && (PINB.3 == 0) && (PINB.0 == 1) && (PINB.1 == 1)&&(PINB.4 == 0)&&(PINB.5 == 0)&&(PINB.6 == 0)&&(PINB.7 == 0))
{
STOP();
B();
delay_ms(25);
STOP();}
////////////////////////////////////////////////////////////*
}
/***************************
варианты "случайного" направления
************************/
void direction()
{
snum(); //выбираем значение случайного направления (р)
//-----
//Дальше выбираем направление по ранее найденому значению
if (p == 1)
{F();}
if (p == 2)
{FR();}
if (p == 3)
{BL();}
if (p == 4)
{FL();}
if (p == 5)
{BR();}
if (p == 6)
{B();}
}
/********************************************************
Главная программа
**********************************************************/
void main(void)
{
DDRD = 0xff; // назначаем все линии порта B на выход
PORTD = 0x00;
DDRB = 0x00; // назначаем все линии порта D на вход
PORTB = 0x00;
while(1) //бесконечный цикл
{
photo(); //запускаем фототранзисторное "зрение"
contact(); //запускаем контактные датчики
/****************************************
Свободный поиск
*******************************/
if (PORTB == 0) // Если с датчиков не поступает информация, включить свободный поиск
{
direction();
}
}
}
Вернуться в Осторожно — грабли ;)
Сейчас этот форум просматривают: нет зарегистрированных пользователей и гости: 8