Система привода робота

В техническом задании  на этот проект, предлагалось разработать автономный робот, который мог бы проходить через неизвестный лабиринт. Робот должен двигаться через лабиринт, находя стенки лабиринта с помощью инфракрасных (ИК) локаторов(пар излучатель-приемник), и принимая решения, продвигаться вперед или назад, чтобы продолжить путь через лабиринт. При своем движении робот должен также избегать «мин» (магнитов), скрытых в полу лабиринта. Робот обнаруживает «мины» с помощью датчика Холла. Если робот обнаруживает «мину», то он останавливается, дает задний ход и объезжает ее.

 

 

 

 

 

 

 

 

 

 

 

 

 

 

 

 

 

 

 

 

 

 

 

Програмный код

 

/*имя файла: robot.c */

/* Включенные  файлы*/

#include <912b32.h>

#include <stdio.h>

 

/*Пороги датчиков  были определены экспериментально*/

#define opto_threshold 0x50 /* порог оптического датчика */

#define hes_threshold 0x80 /* порог датчика Холла */

#define forward 0

#define half_left 1

#define half_right 2

#define left_turn 3

#define right_turn 4

#define back_up 5

 

/*глобальные  переменные*/

unsigned int i=0,j=0; /*переменные для программной задержки */

unsigned char sens[6]={0, 0, 0, 0, 0, 0};/*массив результатов АЦП */

/*прототипы функций*/

 

 

void init_adc(void); /*инициализация АЦП */

void read_adc(void); /*считывание значений АЦП */

void decision(void); /*передача решения о повороте, основанного на */

/*данных АЦП* /

void init_pwm(void); /*инициализация ШИМ */

void pwm_motors(const char a); */активация ШИМ для пересылки */

void lcd_init(void); /* инициализация дисплея */

int putchar(char c); /*вывод символа на дисплей */

int putcommand(char с); /*вывод комагнды на дисплей */

void delay_25(void); /*подпрограмма задержки на 2,5 с */

void lcd_print(char *string); /*вывод строки на ЖК дисплей */

void main()

(

asm(".area vectors(abs)\nr /*инициализация вектора сброса МК */

" org 0xFFF8\n"

" .word 0x8000, 0x8000, 0x8000, 0x8000\n"

".text");

lcd_init(); /*инициализация ЖК дисплея */

lcd_print ("LCD initialized");

void delay_25(void); /* задержки на 2,5 с */

init_adc(); /*инициализация  АЦП */

lcd_print("ADC initialized");

void delay_25(void); /* задержки на 2,5 с */

init_pwm (); /*инициализация  ШИМ */

lcd_print("PWM initialized");

void delay_25(void); /* задержки на 2,5 с */

while(1) / *непрерывный цикл */

{

read_adc(); /* считать текущее значение из АЦП */

decision(); /* принять решение о направлении движения */

}

} /*конец программы  main*/

 

/*initialize_adc: инициализация АЦП контроллера 68HC12

 

void init_adc()

{

ATDCTL2 = 0x80 /*Установить бит ADPU для подачи питания на АЦП */

ATDCTL3 = 0x00

ATDCTL4 = 0x7F /* частотцу P_CLK установить на 125 кГц */

/* время преобразования: 32 ATD CLK, */

/*1 считывание  каждые 256 мкс /*

for(i= i<67; i++) /*задержка 100 мкс при 8 МГц E_CLK */

{

;

}

}

 

/*read_adc: считывание результатов из АЦП

 

 

void read_adc()

{

ATDCTL5 = 0x50; /*Установить АЦП на режим многоканального,*/

/* преобразования 8 каналов */

while((ATDSTAT & 0x8000)= =0)/* проверка бита SCF для окончания */

/*преобразования */

{

;

}

/* сохранения  результата в глобальном массиве  */

sens[0] = ADR7H; /*дальний левый датчик */

sens[l] = ADR6H; /*средний прапвый датчик */

sens[2] = ADR5H; /*центральный датчик */

sens[3] = ADR4H; /* средний правый датчик */

sens[4] = ADR3H; /* дальний правый датчик */

sens[5] = ADR2H; /*датчик  Холла*/

}

 

/*decision(): решение о повороте основано на информации, полученной от*/

/* пяти датчиков. Порог датчика Холла (hes_threshold) и порог */

/* оптического  датчика (opto_threshold) определяются экспериментально. */

 

void decision()

{

if(sens[5] < hes_threshold){ /* датчик Холла нашел "мину", /*

pwm_motors(back_up); /* робот движется назад и определяются /*

/* дальнейшие  действия/*

if(sens[0] > opto_threshold)

pwm_motors(right_turn);

else

pwm_motors(left_turn);

for(i=0; i<0xFFFF; i++){ /*задержка вращения двигателя */

for(j=0 J<15; J++)

{

;

}

}

}

/*если обнаруживает  три стенки (тупик), то движется  назад */

else if((sens[2]>opto_threshold)&&(sens[0]>opto_threshold)

&&(sens[4]>opto_threshold))

{

pwm_motors(back_up);

}

/*если стенки  спереди и слева, поворачивает  направо */

else if((sens[0]>opto_threshold)&&(sens[2]>opto_threshold))

{

 

pwm_motors(right_turn);

}

/*если стенки  спереди и справа, поворачивает  налево */

else if((sens[2]>opto_threshold)&&(sens[4]>opto_threshold))

{

pwm_motors(left_turn);

}

/*если стенка  спереди справа, делает полуповорот  направо*/

else if(sens[1]>opto_threshold)

{

pwm_motors(half_right);

}

/*если стенка  спереди слева, делает полуповорот  налево */

else if(sens[3] > opto_threshold)

{

pwm_motors (half_left) ;

}

/*если стенки  вблизи нет, продолжает движение  вперед */

else

{

pwm_motors(forward);}

}

}

 

/*init_pwm(): инициализация ШИМ контроллера 68HС12

 

void init_pwm()

{

PWTST= 0x00;

PWCTL= 0x00; /*Режим фронтовой ШИМ */

WCLK= 0x3F; /*Каналы без каскадного соединения, E_CLK/128 */

PWPOL= OxOF; /*set pins high then low transition */

DDRP = OxFF; /*Порт PORT T на вывод */

PWEN = OxOF; /*Активизировать выход ШИМ */

PWPERO = 250; /*Установить частоту ШИМ 250 Гц */

PWPER1 = 250;

PWPER2 = 250;

PWPER3 = 250;

PWDTY0 = 0; /*начальная установка ШИМ на отсутствие движения*/

PWDTYl = 0;

PWDTY2 = 0;

PWDTY3 = 0;

}

 

/*pwm_motors: /*выполнение определенного поворота

 

void pwm_motors(const char a)

{

for (i = 0;i<2000;i + +) /*задержка на 3 мс чтобы позволить двигателю*/

{ /* отреагировать*/

}

switch(a){ /*определение вида поворота */

case 0: /* движение вперед */

PWDTY0 = 200; /*регистры коэффициента заполнения ШИМ */

PWDTYl = 250;

PWDTY2 = 250;

PWDTY3 = 200;

lcd_print("Forward\n");

break;

case 1: /*полуповорот налево */

PWDTY0 = 0; /*регистры коэффициента заполнения ШИМ */

PWDTYl = 250;

PWDTY2 = 250;

PWDTY3 = 125;

lcd_print ("Half Left\n");

break;

case 2: /*полуповорот направо*/

PWDTYO = 125; /*регистры коэффициента заполнения ШИМ */

PWDTY1 = 250;

PWDTY2 = 250;

PWDTY3 = 0;

lcd_print("Half Right\n");

break;

 

case 3: /*поворот налево*/

PWDTYO = 125; /*регистры коэффициента заполнения ШИМ */

PWDTY1 = 250;

PWDTY2 = 0;

PWDTY3 = 12 5;

lcd_print("Left Turn\n");

break;

case 4: /*поворот направо*/

PWDTYO = 125; /*регистры коэффициента заполнения ШИМ */

PWDTY1 = 0;

PWDTY2 = 250;

PWDTY3 = 12 5;

lcd_print("Right Turn\n");

break;

case 5: /*задний ход*/

PWDTYO = 125; /*регистры коэффициента заполнения ШИМ */

PWDTY1 = 0;

PWDTY2 = 0 ;

PWDTY3 = 125;

for(i=0; i<0xFFFF; i++)

{ /* Задержка в 1,25 с перед движением назад*/

for(j=0; j<15; J++)

{

;

}

lcd_print ( "Back Up\n" ) ;

break;

default: /*по умолчанию движение вперед, малая скорость */

PWDTYO = 63; /*регистры коэффициента заполнения ШИМ */

PWDTY1 = 250;

PWDTY2 = 250;

PWDTY3 = 63;

lcd_print("Error\n");

break;

}

}

 

/*lcd_init(): инициализация режима работы ЖК дисплея */

/*Последовательность  команд инициализации определяется  изготовителем */

/*PORTA: магистраль данных, PORTB[2:1]: линия R/S, линия разрешения E*/

 

void lcd_init()

DDRA=0xff; /*порт PORTA на вывод */

DDRB=0xO6; /* порт PORTB [2:1] на вывод */

/*последовательности  команд для инициализации ЖК  дисплея */

putcommand(0x38)

putcommand(0x38)

putcommand(0x38)

putcommand(0x38)

putcommand(0x0f)

putcommand(0x01)

putcommand(0x06)

putcommand(0x00)

/*очистка дисплея,  возврат курсора */

putcommand(0x00)

}

 

/*putchar(char c): вывод символа на дисплей

 

int putchar(char c)

(

P0RTA=C;

PORTB= PORTB |0x04;

PORTB= PORTB |0x02;

PORTB= PORTB&Oxfd;

for (i=0; i<100; i++) ; */задержка на *150 мкс до того, как ЖКД */

*/ сможет принять  информацию */

return с;

}

 

/*putcommand(char c): выдача команды управления для ЖК дисплея

 

 

int putcommand(char с)

(

PORTA= с;

PORTB= PORTB&Oxfb;

PORTB= PORTB|0x02;

PORTB= PORTB&Oxfd;

for (i=0; i<100; i++) */ задержка на *150 мкс до того, как ЖКД сможет*/

*/принять информацию */

{

;

}

return c;

)

 

/*delay_25(): задержка на 2.5 с

 

void delay_25()

(

for(i=0; i<0xFFFF; i++)

(

for (j=0; j<30; j++)

{

;

}

}

}

 

/*lcd_print(): вывод строки символов на дисплей.

 

void lcd_print(char *string)

{

putcommand{0x02); /*возврат  курсора ЖКД */

while (* {string) != '\0') {

{

putchar(*string);

string++;

}

}