55262813 Lap Trinh Robot Don Gian Voi PIC 16F877A

Lp trnh Robot t ng n gin vi PIC16F877A Bin son: EM-BOT Robot Team

http://em-bot.tk (Feb 2010) DEMO VERSION Trang 1/14

LP TRNH ROBOT T NG N GINVI VI IU KHIN PIC16F877A

Cao Hong Long, EM-BOT Robot [email protected] ; [email protected]

Khoa Cng ngh, i hc Cn ThTm ttTi liu hng dn lp trnh cho robot t ng d ng theo vch trng v iu khincc c cu (nng h, gp nh qu) mt cch c bn nht.Vi iu khin c s dngtrong ti liu l PIC16F877A ca Microchip.Lp trnh bng ngn ng C vi trnh bindch CCS.

1 TM TT V THIT K ROBOT T NGRobot t ng trong cc cuc thi Robocon gm 3 thnh phn chnh: C kh,Mch in t, Lp trnh.

1.1 C khMt robot n gin gm 2 ng c truyn ng cho 2 bnh xe bn tri v bnphi gip robot di chuyn. Pha trc c th l 1 hoc 2 bnh t do (bnh tla, omni, mt tru,). thc hin c cc cng vic nh nng h trc,gp nh y qu, robot c trang b thm cc ng c khc truyn ngcho cc c cu ny.Tt c cc b phn trn c b tr trn mt khung bng nhm, st,Phn hng dn chi tit v thit k c kh s c trnh by trong mt tiliu khc. Ti liu ny ch tp trung vo phn lp trnh.

Hnh 1: M hnh robot d dng n gin1.2 Mch in t

Hnh 2: S hot ng ca robot t ng



1.2.1 Mch ng vo (cm bin, nt n, cng tc hnh trnh)Vi robot n gin, ng vo thng l mc logic ly t cm bin quang(quang tr, quang diode), nt n hoc cng tc hnh trnh. T mch viiu khin x l cc tn hiu ny xut ng ra (thng l ng c DC) choph hp.Cm bin quang phi c che chn cn thn hn ch nh hng t ccngun nh sng bn ngoi.

C1

104

CTHT

SW2BIT0

5V

R310K

Hnh 3: Mch cm bin, nt n v cng tc hnh trnh1.2.2 Mch vi iu khin

Mch s dng vi iu khin PIC16F877A ca Microchip. Mch nhn tnhiu t ng vo, x l v xut ng ra qua mt mch cch ly bng opto ramch cng sut.

RA5RA4

RC7

RC0

RA2RA3

RA0RA1

RB1RB2

RB4RB5

RB0

RB6RB7

RB3

RE0RE1

RC3RC4

RE2

RC5RC6

RD4

RD0RD1RD2RD3

RD5

Y1CRYSTAL

PIC16F877A

RA0/AN02

RA1/AN13

RA2/AN2/VREF-/CVREF4

RA3/AN3/VREF+5

RA4/T0CKI/C1OUT6

RA5/AN4/SS*/C2OUT7

RB0/INT33

RB134

RB235

RB3/PGM36

RB437

RB538

RB6/PGC39

RB7/PGD40

RC0/T1OSO/T1CKI15

RC1/T1OSI/CCP216

RC2/CCP117

RC3/SCK/SCL18

RC4/SDI/SDA23

RC5/SDO24

RC6/TX/CK25

RC7/RX/DT26

RD0/PSP019

RD1/PSP120

RD2/PSP221

RD3/PSP322

RD4/PSP427

RD5/PSP528

RD6/PSP629

RD7/PSP730

OSC1/CLKIN13

OSC2/CLKOUT14

VDD32VDD11

VSS31 VSS12

MCLR*/VPP1

RE0/RD*/AN58

RE1/WR*/AN69

RE2/CS*/AN710

C5 30p

C6 30p5V

RC1RC2

RESET

MCU MODULE

RD6RD7

Hnh 4: Mch vi iu khin1.2.3 Mch cng sut iu khin ng c DC

Mt s mch thng dng:

D141N5819

D13

1N581924V

C2104

MOTOR2

CON2

12

M2IRF540

R12330 2W

LS2RELAY

32

4

67

518

P-DIR2

P-PWM2

Q5B562

Q4D468

D121N5819

12V

D3QUANG TRO

D2LED

R1330

R233K

VCCVCC

-

+ U1A

LM324

3

21

R4 100K

CAMBIEN1

Vch trng: mc 1Nn: mc 0



Hnh 5: Mch cng sut s dng 1 FET v 1 relay

U3

IR 2184

COM3

LO4

VB8

IN1

VCC5

SD2

VS6

HO7

U4

IR2184

COM3

LO4

VB8

IN1

VCC5

SD2

VS6

HO7

+C54.7u

+C1

4.7u

C6100n

C2100n

D1

DIODE

R4 4.7R

R6 4.7R R7

4.7R

R5

4.7R

D2

DIODE

+ C34.7u

C4100n

12V

IN2

Q1

IRF540

Q3

IRF540

Q2IRF540

Q4IRF540

24V

C7470n

+ C8470u

J1

MOTOR

12C14

104

IN1

Hnh 6: Mch cu H iu khin ng c vi Half Bridge Driver IR2184

Phn hng dn chi tit v mch in t s c trnh by trong mt tiliu khc. Ti liu ny ch tp trung vo phn lp trnh.



1.3 Lp trnhy l phn chnh ca ti liu ny. Ngn ng lp trnh c s dng l C, vitrnh bin dch CCS cho vi iu khin PIC ca Microchip.Kin thc ban u: Lp trnh C cn bnCc ti liu tham kho: Ti liu CCS ting Vit: http://www.mediafire.com/?hd2ugmmzvwf Hng dn lp trnh PIC-DKS:http://www.mediafire.com/?e0ioxeideyx Gio trnh PIC (H Reading): http://www.mediafire.com/?iyewnoldyjk Lp trnh C cho PIC 8bit: http://www.mediafire.com/?tnjlztmmzdm CCS Manual (tra cu lnh): http://www.mediafire.com/?htljg1jg2mn

y l cc ti liu cn c qua trc khi vo lp trnh cho PIC c th bit cchm nhn tn hiu ng vo, xut tn hiu ng ra, ngt, timer, counter, PWM,Cc hm quan trng s c nhc li phn 2.

2 LP TRNH CHO ROBOT T NG D NG N GINChng trnh gip robot chy theo vch trng trn nn mu sm.

2.1 Phn cng 8 cm bin quang d ng Mch cng sut iu khin 2 ng c Mch vi iu khin:

o PORTD: ni vi tn hiu ra ca 8 cm bino ng c tri:

Chn C0: iu khin chiu (DIR_LEFT) Chn C1: iu khin cho php chy (EN_LEFT)

o ng c phi: Chn C3: iu khin chiu (DIR_RIGHT) Chn C2: iu khin cho php chy (EN_RIGHT)

(Cc ng vo v ng ra c th ni vi bt c PORT v chn no ca vi iu khin)2.2 Nguyn tc iu khin2.2.1 iu khin ng c

Trng thi DIRECTION ENABLEThun 1 1Ngc 0 1Dng x 0

Tng t i vi cc ng c iu khin cc c cu.2.2.2 Hng di chuyn ca robot

Trng thi ng c tri ng c phiThng Thun Thun

Quay tri Thun Ngc (Dng)Quay phi Ngc (Dng) Thun



2.2.3 X l tn hiu cm bin (xem hnh v)Mc ch ca vic d ng l hng cho robot i theo 1 vch thng mu trngtrn mt nn mu m (en, xanh,)Cm bin c t gia robot.

o Khi cm bin s 3,4 nm trn vch trng (mc 1): robot chy thngo Khi robot lch sang tri: quay phi iu chnh robot v ng vcho Khi robot lch sang phi: quay tri iu chnh robot v ng vch

7 6 5 4 3 2 1 0 Trng thi cmbin

Gia vch

Lch tri mc 1

Lch tri mc 2

Lch tri mc 3

Lch tri mc 4

Ngoi vch

Lch phi mc 1

Lch phi mc 2

Lch phi mc 3

Lch phi mc 4

Ngoi vch

Hnh 7: Cc mc lch ra khi vch trng ca robot

2.3 Chng trnh iu khin

#include #include #fuses HS,NOWDT,NOPROTECT,NOLVP#use delay(clock=20000000)

/* NH NGHA CC CHN V PORT */#define DIR_LEFT RC0#define EN_LEFT RC1

#define DIR_RIGHT RC3#define EN_RIGHT RC2

0

0

000000 0 0

000000 0 0

01

0 01 0 0 00

0 011 0 0 00

0011 0 0 00

1 0 0 00 0

1 1 0 0 00 0 0

10

0 1

000 0 0

00 10 0 0

00 11

0 1

00 0 0

1 0 00 0 0



#define SENSOR PORTD/* KHAI BO CC CHNG TRNH CON */

void motor_left_forward();void motor_left_reverse();void motor_left_stop();

void motor_right_forward();void motor_right_reverse();void motor_right_stop();

void forward();void reverse();void stop();void turn_left();void turn_right();

/* CC CHNG TRNH CON */// ng c tri chy thunvoid motor_left_forward(){DIR_LEFT=1; // chiu thunEN_LEFT=1; // cho php chy}// ng c tri chy ngcvoid motor_left_reverse(){DIR_LEFT=0; // chiu ngcEN_LEFT=1; // cho php chy}// ng c tri dngvoid motor_left_stop(){EN_LEFT=0; // khng cho php chy}// ng c phi chy thunvoid motor_right_forward(){DIR_RIGHT=1;// chiu thunEN_RIGHT=1; // cho php chy}// ng c phi chy ngcvoid motor_right_reverse(){DIR_RIGHT=0;// chiu ngcEN_RIGHT=1; // cho php chy}// ng c phi dngvoid motor_right_stop(){EN_RIGHT=0; // khng cho php chy}// Chy thngvoid forward()



{motor_left_forward();motor_right_forward();}

// Chy livoid reverse(){motor_left_reverse();motor_right_reverse();}// Dngvoid stop(){motor_left_stop();motor_right_stop();}// Quay trivoid turn_left(){motor_left_forward();motor_right_reverse(); // hoc motor_right_stop();}// Quay phivoid turn_right(){motor_left_reverse(); // hoc motor_left_stop();motor_right_forward();}

/* CHNG TRNH CHNH */void main (){

TRISC=0x00; // PORTC l ng ra ( ng c)TRISD=0x00; // PORTD l ng vo (cm bin quang)PORTC=0x00; // Khi to gi tr ban u 0x00 cho PORTCwhile(1){switch (SENSOR){case 0b00011000: forward(); break;case 0b00001100: turn_left(); break;case 0b00000110: turn_left(); break;case 0b00000011: turn_left(); break;case 0b00000001: turn_left(); break;case 0b00110000: turn_right(); break;case 0b01100000: turn_right(); break;case 0b11000000: turn_right(); break;case 0b10000000: turn_right(); break;}}

}



3 CI TIN CHNG TRNH D NG3.1 iu khin tc ng c vi cc trng thi lch khi vch trng3.1.1 Nguyn l

i vi chng trnh d ng n gin, khi robot lch tri hoc lch phi,robot s quay phi hoc quay tri iu chnh cho d lch t hay lch nhiu.Nh vy, trong qu trnh di chuyn, robot s lc lin tc v phi quay tri, quayphi lin tc. Do , vi cc mc lch ra khi vch trng khc nhau, ta iuchnh tc 2 bnh tri, phi cho ph hp qu trnh di chuyn theo vchca robot c nhuyn v mt hn.Bng gi tr tham kho tc ng c tri v ng c phi tng ng vi cctrng thi lch khi vch trng ca robot (xem li hnh 7):

Trng thi Tc ng c tri Tc ng c phiGia vch 100% 100%

Lch tri mc 1 90% 80%Lch tri mc 2 80% 70%Lch tri mc 3 70% 60%Lch tri mc 4 60% 50%Lch phi mc 1 80% 90%Lch phi mc 2 70% 80%Lch phi mc 3 60% 70%Lch phi mc 4 50% 60%

3.1.2 iu khin tc ng c DC bng phng php PWMi vi iu khin tc ng c DC trong robot, phng php c s dngph bin nht l iu bin rng xung (Pulse Width Modulation) hay cgi tt l iu xung, bm xung hoc PWM.Nguyn l ca phng php ny l bt tt nhanh ngun in cp vo ng cto ra mt tn hiu xung. Khi vic bt tt tn s ln (thng s dng t1kHz n 20kHz), ng c s chy vi 1 tc n nh nh moment quay.Thi gian cp ngun cho ng c l T-on, thi gian tt ngun ng c l T-off.Vic thay i thi gian T-on v T-off lm thay i in p hiu dng cp chong c. i vi ng c DC, tc ng c tng i t l thun vi in pcp cho ng c. V vy, bng cch thay i rng ca xung, ta thay ic tc ca ng c DC.i lng biu din mi quan h gia T-on v T-off c gi l duty cycle

offon

on

TTT

cycleduty _V d: Ta cp ngun ng c trong 0.8ms, sau tt 0.2ms.

Nh vy: T-on = 0.8ms; T-off = 1ms. Tn s PWM lkHz

msmsmsTTf

offon1

11

2.08.011



%808.02.08.0

8.0_ offon

on

TTT

cycleduty

V tc ng c DC t l vi duty cycle nn tc ng c t tngng 80% tc ti a.

Hnh 8: Tnh ton duty cycle iu khin tc ng c DC3.1.3 iu xung PWM dng vi iu khin iu xung PWM bng phn mm:

iu xung PWM mt cch n gin l a 1 chn no ca vi iu khinln mc 1, sau a xung mc 0. Cng vic ny c lp i lp li lintc s to ra xung, v tc ca ng c s tng ng vi duty cycle.V d: iu xung trn chn A0 :CodeRA0=1;Delay_ms(Ton);RA0=0;Delay_ms(Toff);

Tuy nhin, nu thc hin bng cch ny th vi iu khin s lun dnh thigian cho vic iu xung PWM. Do , cc cng vic khc nh nhn tn hiut cm bin, iu khin cc c cu s b nh hng.

iu xung PWM bng phn cng gii quyt vn vic iu xung PWM bng phn mm chim phn lnthi gian hot ng ca vi iu khin, PIC16F877A c h tr 2 knh iuxung bng phn cng 2 chn C1 (CCP2) v C2(CCP1) s dngTIMER2. Ngha l, khi ta khai bo iu xung PWM mt tn s v dutycycle no th vi iu khin s thc hin cng vic xut xung mt cchlin tc v t ng cho n khi ta thay i cc gi tr khai bo. Khi , tac th lm cc cng vic khc mt cch d dng m khng phi mt thigian cho vic duy tr xung PWM.

Cc hm h tr vic iu xung bng phn cng ca CCS:Ghi ch: Ch cp n cc i s ca cc hm c phc v cho vic iuxung PWM.

o setup_timer_2 (mode, period, postscale) mode: T2_DIV_BY_1, T2_DIV_BY_4, T2_DIV_BY_16

period: 0-255postscale: 1



Tn s iu xung PWM:1)odmode.(peri.4

oscffo setup_ccp1(mode) v setup_ccp2(mode)

mode:CCP_PWM: chn ch PWM.CCP_OFF: tt ch PWM.

o set_pwm1_duty(value) v set_pwm2_duty(value) Nu value l gi tr kiu int 8bit:

1_ period

valuecycleduty

Nu value l gi tr long int 16bit:

)1(*41023&

_ periodvalue

cycleduty

Nu khng cn iu xung qu mn th nn iu xung gitr value 8bit cho n gin.

V d: Ta mun iu xung PWM vi tn s 10kHz vi tn s thch anh(fosc) s dng l 20MHz (value 8bit).

500)1dmode(perio1)odmode.(peri.4

20000000100001)odmode.(peri.4

oscffVi mode = [1,4,16] v period = 0-255 ta c th chn

o mode = 4; period = 124o mode = 16; period = 32

cho vic iu xung c mn (chn c nhiu gi tr duty cycle) tachn mode = 4 v period = 124.Nh vy, duty_cycle t 0% n 100% ta cho value t 0 n 125.

o value = 30 %3232.01124

30_ cycleduty

o value = 63 %4,50504.01124

63_ cycleduty

o value = 113 %4,90904.01124

113_ cycleduty

Code: setup_timer_2(T2_DIV_BY_4,124,1); setup_ccp1(CCP_PWM); set_pwm1_duty(30);

S dng CCP1 v CCP2 cho ng c tri v ng c phi, ta c th iukhin c tc ca 2 ng c ph hp trng thi lch khi vch trngca robot.

Cc chng trnh con tham kho: vic lp trnh c d dng, ta nn to cc chng trnh con x l tc. Sau y l chng trnh tham kho ca hm speed.

o Speed (tc ng c tri, tc ng c phi) Tc : -100 n 100 (chy ngc 100% n chy thun 100%) V d: speed(80,60) ng c tri chy 80%, phi 60%



// Cc hm h trvoid left_motor_forward(int value){ MOTOR_LEFT_DIR=0; setup_timer_2(T2_DIV_BY_4,124,1); // Dieu xung 10kHz setup_ccp2(CCP_PWM); set_pwm2_duty(value);}void right_motor_forward(int value){ MOTOR_RIGHT_DIR=0; setup_timer_2(T2_DIV_BY_4,124,1); // Dieu xung 10kHz setup_ccp1(CCP_PWM); set_pwm1_duty(value);}void left_motor_reverse(int value){ MOTOR_LEFT_DIR=1; setup_timer_2(T2_DIV_BY_4,124,1); // Dieu xung 10kHz setup_ccp2(CCP_PWM); set_pwm2_duty(value);}void right_motor_reverse(int value){ MOTOR_RIGHT_DIR=1; setup_timer_2(T2_DIV_BY_4,124,1); // Dieu xung 10kHz setup_ccp1(CCP_PWM); set_pwm1_duty(value);}void left_motor_stop(){ setup_ccp1(CCP_OFF);}void right_motor_stop(){ setup_ccp1(CCP_OFF);}// Chng trnh x l tc 2 ng c// 0:Stop,100:FORWARD 100%,-100:Reverse 100%void speed (signed int left_motor_speed, signed int right_motor_speed){ int left_pwm_value=0,right_pwm_value=0; /* Left motor */ if( left_motor_speed >= 0 ) { left_pwm_value = 1.25*left_motor_speed; // (125*left_motor_speed/100) left_motor_forward(left_pwm_value); } else { left_motor_speed = -left_motor_speed; left_pwm_value = 1.25*left_motor_speed; // (125*left_motor_speed/100) left_motor_reverse(left_pwm_value);



} /* Right motor */ if( right_motor_speed >= 0 ) { right_pwm_value = 1.25*right_motor_speed; // (125*left_motor_speed/100) right_motor_forward(right_pwm_value); } else { right_motor_speed = -right_motor_speed; right_pwm_value = 1.25*right_motor_speed; // (125*left_motor_speed/100) right_motor_reverse(right_pwm_value); }}

3.2 Nhn bit vch ngangTrong qu trnh di chuyn ca robot, s c cc vch ngang mu trng. Nh ccvch ngang ny, robot bit c mnh ang i n u v s thc hin cngvic g tip theo (quay tri, quay phi, nng h trc, gp nh qu,) Mt cch n gin, khi robot n vch trng, tt c 8 cm bin s ln mc 1

ng vi gi tr c c ca cm bin l 0b11111111 hay 0xFF. Ta dngmt bin m bit th t ca vch ngang ang gp v thc hin cngvic mong mun.Code:int n // t n l s vch ngang nhnif (SENSOR==0xFF){

n++; // tng gi tr n ln 1 khi gp vch ngangswitch (n){case 1: (cng vic 1) break;case 2: (cng vic 2) break;.case n: (cng vic n) break;}

}else (chng trnh d ng)

Tuy nhin, v nhiu l do khc nhau (nhiu do nh sng bn ngoi, cc cmbin c nhy khng u nhau hoc robot tip cn vi vch ngang khngtheo phng vung gc), mt vi cm bin khi tip xc vi mu trngnhng khng nhn bit (vn gi trng thi mc 0 mu sm). iu ny gykh khn cho vic nhn bit vch ngang. V vy, khi 4/8 cm bin mc 1,ta nhn l mt vch ngang khc phc cc nh hng ny.Code://KIEM TRA VACH NGANG: 1: co vach, 0:khong co vachint check_cross_line(){ int temp_sensor=0,led_in_line=0,i=0; temp_sensor=SENSOR; for (i=0;i



if ((temp_sensor&0x01)==0x01) led_in_line++; if (led_in_line==4) break; temp_sensor=temp_sensor>>1; } if (led_in_line==4) return 1; else return 0;}

Do chng trnh nhn vch ngang c gi lin tc kim tra c vchngang xut hin hay khng nn s dn n tnh trng khi robot n vchngang, bin m s vch tng thm 1. Sau , khi robot cha kp chy quavch ngang m hm kim tra c gi dn n vic bin m tng lin tc.Vic ny khin cho robot thc hin sai cng vic.Hng gii quyt tnh hung ny nh sau:

o Khi robot gp vch ngang: chy thngo Khi ht vch ngang: bin m s vch tng thm 1o Thc hin cng vic tng ng

Code:while (check_cross_line() == 1) // gp vch ngang{

speed(100,100); // chy thng}number_cross_line++; // tng bin m s vch ngang thm 1

3.3 X l khi robot lch hon ton khi vchTrng hp robot lch ra khi vch (quay qu mnh hoc b va chm), gi trcm bin c c l 0x00, nh vy robot s b mt phng hng. gii quyt trng hp ny, ta t mt bin trng thi l bin ton cc.Gi bin ny l line_status:

o Line_status=0: gia vch;o Line_status=1: lch tri;o Line_status=2: lch phi;

Khi c gi tr cm bin d ng (tham kho hnh 7), ta gn lun gi trcho bin ny. Nh vy khi robot lch hn ra khi vch, ta vn bit c trngthi trc bit quay tri hoc quay phi hng robot di chuyn vpha line.Code

switch (SENSOR){case 0b00000000: // lch ra hn khi vch{

if (line_status==1) // trng thi c l lch triturn_right(); break; // quay phi di chuyn v pha vchif (line_status==2) // trng thi c l lch phiturn_left(); break; // quay tri di chuyn v pha vch

}// Trng thi lch thng thngcase 0b00011000: forward(); line_status=0; break;case 0b00001100: turn_left(); line_status=1; break;



case 0b00000110: turn_left(); line_status=1; break;case 0b00000011: turn_left(); line_status=1; break;case 0b00000001: turn_left(); line_status=1; break;case 0b00110000: turn_right(); line_status=2 ;break;case 0b01100000: turn_right(); line_status=2 ;break;case 0b11000000: turn_right(); line_status=2 ;break;case 0b10000000: turn_right(); line_status=2 ;break;}

3.4 ng dng encoder3.4.1 Kin thc c bn v encoder

Tham kho ti din n PICVietnam:http://www.picvietnam.com/forum/showthread.php?p=1062#post1062Trong Robocon, ta thng s dng incremental encoder (encoder tng i)hay cn gi l rotary encoder. Mc ch ca vic s dng encoder trongrobot l m s vng quay tnh s vng quay ca ng c (bnh xe), t suy ra qung ng di chuyn v tc ca robot.

Hnh 10: Encoder thng c s dng trong Robocon3.4.2 S dng PIC nhn v m xung t encoder

nhn xung t encoder, ta c th s dng ngt ngoi, ngt timer hoc ngin l tham d mc logic ca cc chn vi iu khin mt cch lin tc.Phn sau y gii thiu cch nhn v m xung ca PIC16F877A dng ngtngoi B0 (ni vi knh A ca encoder) v chn B1 (ni vi knh B caencoder). Ta c th lm tng t i vi cc cch nhn xung khc.

Khi to ngt ngoi theo cnh ln ti chn B0:Code:ext_int_edge(0,L_TO_H); // Ngt cnh ln ti RB0enable_interrupts(INT_EXT); // Cho php ngt ngoienable_interrupts(GLOBAL); // Cho php ngt ton cc

Chng trnh con phc v ngt:Code:#int_EXTvoid EXT_isr(void) //Chng trnh c gi khi c tc ng cnh ln ti chn B0{ if (RB1==1) pulse++; // Nu knh B mc cao th tng gi tr xung thm 1 else pulse--; // Nu knh B mc cao th gim gi tr xung xung 1}

T gi tr xung tnh c ti cc thi im ta c th tnh ra cc thng smong mun.

Documents

55262813 Lap Trinh Robot Don Gian Voi PIC 16F877A