Mach Robot

Tài liệu kỹ thuật BKpro – http://www.bkpro.info

I.Maïch Robot töï ñoäng Bkpro: J6

D30

Q13 B688

1 2

U5

D31 LED

RESET

6 5 4 3

CLK CLR

Q3 Q2 Q1 Q0

Q4 Q5 Q6 Q7

8 9

TXD

10 11 12 13

O4 O5 O6 O7

5V

5V

TXD RXD

31

P0.7/AD7 P0.6/AD6 P0.5/AD5 P0.4/AD4 P0.3/AD3 P0.2/AD2 P3.2/INT0 P0.1/AD1 P3.3/INT1 P0.0/AD0 P3.4/T0 P3.5/T1

5V 1 2 3 4 5 6

4K7 J1 6 5 4 3 2 1

NAP 89SXX

12 13 14 15 5V

X1

DK PWM2 DK PWM3 DK PWM4 RST

P2.0/A8 P2.1/A9 P2.2/A10 P2.3/A11 P2.4/A12

P1.0 P1.1 P1.2 P1.3 P1.4 P1.5 P1.6 P1.7

8051

32 33 34 35 36 37 38 39

CAMBIEN8 CAMBIEN7 CAMBIEN6 CAMBIEN5 CAMBIEN4 CAMBIEN3 CAMBIEN2 CAMBIEN1

5 6

P2.2 P2.3 P2.4

11 10 9 7

Y Y

Y1

ZTA

2 3 4 5 6 7 8 9

A B C

2K2

J4

P2.1

5 6

P2.2 P2.3 P2.4

11 10 9

C5 30P

4 3 2 1 15 14 13 12

D0 D1 D2 D3 D4 D5 D6 D7

Y Y

5V

2 3 4 5 6 7 8 9

A B C

2K2

J5

G

8

5

DP

5

LED7.4

1K

D1

2

100K

R22

LEDTHU2

U12B 7

+

6

R28

2

U13A 1

D6

LED

D7

LED

D8

LED

LED

R30

-

12 +

CAMBIEN3 LEDTHU4

9 -

U13B 7

+

6

-

CAMBIEN4

LM324

100K

R32

100K 5V

10 +

CAMBIEN6 LEDTHU7

LM324

U12D 14

13 -

5V

5 CAMBIEN5 LEDTHU6

100K 5V

U12C 8 LM324

5V +

D5

LED

R24 10 +

LEDTHU3

-

100K

5V 3

D4

LED

100K

CAMBIEN2

LM324

100K

LEDTHU5

D3

LED

5V

5 CAMBIEN1

-

J10

D2

LED

5V U12A 1 LM324

2K2

DP

LED7.3

CTHT

R20

+

RN6

R26

8

5

RN4

10 9 8 7 6 5 4 3 2 1

5V

100K

LEDTHU1

LEDTHU8 LEDTHU7 LEDTHU6 LEDTHU5 LEDTHU4 LEDTHU3 LEDTHU2 LEDTHU1

DP

LED7.2

5V

3

12 11 10 9 8 7 6 5 4 3 2 1

8

CTHT

5V

2LEDTHU8 3LEDTHU7 4LEDTHU6 5LEDTHU5 6LEDTHU4 7LEDTHU3 8LEDTHU2 9LEDTHU1

A B C D E F G

7447

74151

1

5

DP

RN3 1

7

5V

8

5V

U4

R18

A B C D E F G

LED7.1

10 9 8 7 6 5 4 3 2 1

G

74151

ENCODER C4 30P

P2.0

4 3 2 1 15 14 13 12

D0 D1 D2 D3 D4 D5 D6 D7

330 330 330 330 330 330 330

U10 A 7 B 6 A C 4 B D 2 C E 1 D F 9 E G 10 F G

CAMBIEN8

ROLE1 1 ROLE2 2 ROLE3 3 ROLE4 4 PWM1 5 PWM2 6 PWM3 7 PWM4 8

P2.0 P2.1 P2.2 P2.3 P2.4

R3 R4 R5 R6 R7 R8 R9

U9 A 7 B 6 A C 4 B D 2 C E 1 D F 9 E G 10 F G

CAMBIEN7

DK DK DK DK DK DK DK DK

21 22 23 24 25

5V

13 12 11 10 9 15 14

A B C D E F G

LT RBI BI/RBO

Q4 R13 1K

1

2 3 4 5 6 7 8 9

1

5V

ALE

1

COI BIP

5V RN2

U3

D0 D1 D2 D3

3 5 4

U8 A 7 B 6 C 4 D 2 E 1 F 9 G 10

CAMBIEN6

5V

30

J3 LA BAN

7 1 2 6

U7 A 7 B 6 C 4 D 2 E 1 F 9 G 10

A B C D E F G

C1815

Q3 R12 1K

9 8 7 6 5 4 3 2

RN5

P2.6/A14

5 4 3 2 1

X2

1k

2 1

27

U6 O0 O1 O2 O3

5V P2.5 P3.6 P3.7

18

P2.6

R56 330

5V O7

C1815

O6 Q2

R11 1K

CAMBIEN5

SW DIP-2

R2

19

5V Q5 Bxxx

11 10 26 16 17

P3.1/TXD P3.0/RXD P2.5/A13 P3.6/WR P3.7/RD

PSEN P2.7/A15

J2 NAP PHILLIP

CAMBIEN3

RST

29 28

J9

J11

9

EA

U2 RST

SW2

4 3 2 1

CAMBIEN2

5V

10K

5V

C1815

O5 Q1

R10 1K

CAMBIEN1

R1

5V

C1815

O4

74164

3 8

O3 O2 O1 O0

T L

C15 + 1000UF

3 8

3 C14 104

T L

+ C13 0.33UF

SW1

+

VOUT

3 8

3W

VI N

T L

3

A B

U15 7805

3 8

R57 1

C1 10UF

1 2

5V 1000UF

T L

RXD

+ C12

NGUON VXL

CAMBIEN4

DIODE

5V

U13C 8

12 +

CAMBIEN7 LEDTHU8

9 -

LM324

U13D 14

CAMBIEN8

13 -

LM324

LM324

5V LED DO ROLE1

M1 IRF540DC1

Q5DC1 Dxxx

ISO2DC1

D11DC1

4

1

ISO6DC3

D15DC3

10

3

2

IN1 IN2 IN3 IN4 IN5 IN6 IN7 IN8

OUT1 OUT2 OUT3 OUT4 OUT5 OUT6 OUT7 OUT8

COM

GND

Q6DC1 Bxxx

PWM1 ROLE1 PWM2 ROLE2 PWM3 ROLE3 PWM4 ROLE4

18 17 16 15 14 13 12 11

2 3 4 5 6 7 8 9

ISO4DC2

DK PWM2

R46DC2 330 2W

3 D21 DX

1 2 C17DC2 104

2

CON2

PWM1

F1

J8

12V

M4DC4 IRF540

1

4 4

PWM4

DC4_DC4

5 6

D25DC4 24V

7 4

DIODE Q12DC4 Bxxx

D26DC4 12V DX 8 1

Q11DC4 Dxxx R52DC4 330 2W

3 DX

1 2 C19DC4 104

2

CON2

3

C ROLE4

DK ROLE4 2

C ROLE2 LED

CON2

LS4DC4 RELAY

12V D12

ISO9DC4

3

DK ROLE2

1

4

OPTO8 D18DC4

2

LED

DIODE

12V

ISO5DC2

3 DX

ROLE4

LED

D14DC2

R49DC3 330 2W

D29

3

2

Q10DC3 B468

1 2 C18DC3 104

D36 1 2

12V

7 4

DIODE

R55 2.2K

DC3_DC3

6

D23DC3 24V PWM3

FUSE J7

8 1 5

24V

NGUON 24 ISO8DC4

NGUON 12 LED

M3DC3 IRF540

Q9DC3 D468

PWM4

D24DC3 12V DX

LS3DC3 RELAY

12V D11

PWM3

1 2

D17DC4

3

2

7 4

DIODE

PWM2

DK PWM4

1

DC2_DC2

6

ROLE3

C PWM4 OPTO7

4

1

C PWM2

2

8 1 5 24V

PWM2

D22DC2 12V DX

LS2 RELAY M2DC2 IRF540

Q7DC2 Dxxx

ULN2803

1K

D13DC2

OPTO4

CON2

1

12V

OPTO1 OPTO2 OPTO3 OPTO4 OPTO5 OPTO6 OPTO7 OPTO8

1K OPTO3

2

RN9 2 3 4 5 6 7 8 9

1K

3 DX

12V D10

9

LED 1

OPTO1 5V OPTO2 OPTO3 OPTO4 OPTO5 OPTO6 OPTO7 OPTO8

R43DC1 330 2W

1 2 C16DC1 104

ROLE2

3

3

2 2 3 4 5 6 7 8 9

12V

1 2 3 4 5 6 7 8

Q8DC2 Bxxx

RN11

1

PWM1 ROLE1 PWM2 ROLE2 PWM3 ROLE3 PWM4 ROLE4

C ROLE3

DK ROLE3

LED RN10 5V

4k7 ISO7DC3

D16DC3 2

C ROLE1

DK ROLE1

4

OPTO6 ISO3DC1

D12DC1

1

12V

4

OPTO2

1

12V

C C C C C C C C

2 3 4 5 6 7 8 9

LED

6 7 4

DIODE

1 12V

3

LED

PWM1

U20CS

DC1_DC1

5 D19DC1 24V

RN8

DK PWM3 2

DK PWM1

8 1

C PWM3 OPTO5

4

1

C PWM1 OPTO1

D20DC1 12V DX

LS1DC1 RELAY

12V D9

LED

Mạch Led doø ñöôøng: 1 2 3 4 5 6 7 8 9 10 11 12

J1 CON12

LEDTHU8 LEDTHU7 LEDTHU6 LEDTHU5 LEDTHU4 LEDTHU3 LEDTHU2 LEDTHU1

5V LED

5V LED

5V LED

R1 100

5V LED

R2 100

D1

D2

LED

R8 100

D7

LED PH_R5 33K

5V LED

R7 100

D6

LED PH_R4 33K

5V LED

R6 100

D5

LED PH_R3 33K

5V LED

R5 100

D4

LED PH_R2 33K

5V LED

R4 100

D3

LED PH_R1 33K

5V LED

R3 100

D8

LED PH_R6 33K

LED PH_R7 33K

PH_R8 33K

<--------> 2.5cm

Khoaûng caùch 2 led lieân tieáp : 2.5 cm, Led ñöôïc boá trí ñoái xöùng.

Trang - 1 -

Tài liệu kỹ thuật BKpro – http://www.bkpro.info

II.Nguyeân lyù caùc khoái chöùc naêng: Maïch nguyeân lyù ñieàu khieån ñoäng cô:

LOAD

24V

5V

12V

R1 330

12V

12V R3 330

R2 4K7

D2

RL D3 DIODE L1

Q2 D468 M1 IRF540

Q1 ULN2803

4

1

DIODE Q3 B562

R4 220 1W

D1 LED

3

2

DK PWM1 PC817

(Chöông trình ñieàu khieån : DONGCO.ASM ) Maïch nguyeân lyù caûm bieán doø ñöôøng: 5V R18

100K

R20 100

5V

LEDTHU1

3

+

2

-

D4

U12A 1

R19 QUANG TRO

LED

CAMBIEN1

LM324

Maïch nguyeân lyù hieån thò Led: U5

10 11 12 13

O4 O5 O6 O7

5V

5V

3 5 4

D0 D1 D2 D3 LT RBI BI/RBO

A B C D E F G

13 12 11 10 9 15 14

R3 R4 R5 R6 R7 R8 R9

330 330 330 330 330 330 330

A B C D E F G

Q3

R11 1K

A B C D E F G

8 DP

LED7.1

5

U8 A 7 B 6 C 4 D 2 E 1 F 9 G 10

Q4

R12 1K

A B C D E F G

8 DP

LED7.2

C1815

5

U9 A 7 B 6 C 4 D 2 E 1 F 9 G 10

R13 1K

A B C D E F G

3 8

U7 A 7 B 6 C 4 D 2 E 1 F 9 G 10

5V O7

C1815

Q2

R10 1K

U6 7 1 2 6

5V O6

C1815

Q1

74164 O0 O1 O2 O3

5V O5

C1815

8 DP

LED7.3

U10 A 7 B 6 A C 4 B D 2 C E 1 D F 9 E G 10 F G

T L

5V O4

3 8

Q4 Q5 Q6 Q7

TXD

T L

Q3 Q2 Q1 Q0

8 9

3 8

CLK CLR

T L

6 5 4 3

A B

3 8

O3 O2 O1 O0

1 2

T L

RXD

8

5

DP

5

LED7.4

7447

(Chöông trình : Led.ASM )

Trang - 2 -

Tài liệu kỹ thuật BKpro – http://www.bkpro.info

Maïch nguyeân lyù ñoïc phím nhaán: RN2

U3

1

P2.0

5 6

P2.2 P2.3 P2.4

11 10 9 7

Y Y

D0 D1 D2 D3 D4 D5 D6 D7

4 3 2 1 15 14 13 12

A B C

5V

2 3 4 5 6 7 8 9 2K2

5V SWITCH LIMIT

G

74151

RN3

U4

1

P2.1

5 6

P2.2 P2.3 P2.4

11 10 9 7

Y Y

A B C G

J4 10 9 8 7 6 5 4 3 2 1

D0 D1 D2 D3 D4 D5 D6 D7

4 3 2 1 15 14 13 12

2 3 4 5 6 7 8 9

5V

2K2

J5 10 9 8 7 6 5 4 3 2 1

5V SWITCH LIMIT

74151

(Chöông trình: NHAP_PHIM.ASM ) III.Giaûi thuaät doø ñöôøng keát hôïp Encoder vaø caûm bieán doø ñöôøng: Quaõng ñöôøng robot chaïy ñöôïc xaùc ñònh bôûi bieán DEM_VACH (ñeám vaïch ngang). Chia quaõng ñöôøng giöõa 2 vaïch lieân tieáp thaønh 2 ñoaïn : ñoaïn 1 – doø ñöôøng chaïy thaúng nhöng khoâng nhaän vaïch ngang ( choáng nhaän nhaàm vaïch ngang), ñoaïn 2 – doø ñöôøng chaïy thaúng tìm vaïch ngang coù taùc duïng choáng nhaän thieáu vaïch ngang ( neáu nhaän ñöôïc vaïch ngang cho pheùp hieäu chænh laïi encoder giaûm DEM_VACH ; neáu khoâng nhaän ñöôïc vaïch ngang khi heát quaõûng ñöôøng ,töï ñoâïng giaûm DEM_VACH , ñoaïn ñöôøng tieáp theo phaûi ngaén hôn…)

Trang - 3 -

Tài liệu kỹ thuật BKpro – http://www.bkpro.info

Caùch chia quaõng ñöôøng giöõa 2 vaïch ngang lieân tieáp (1 oâ traéng 50cm) ñeå choáng nhaän nhaàm vaø nhaän thieáu vaïch ngang:

Caùch xöû lí cuûa robot ôû nhöõng ñoaïn ñöôøng: Robot döïa vaøo vaïch ngang ñeå ñònh vò ñuùng toïa ñoä vì ñaây laø nhöõng vò trí toïa ñoä chuaån . Moãi laàn qua vaïch ngang ,neáu khoâng xaûy ra loãi caûm bieán khoâng nhaän vaïch ngang thì robot quaõng ñöôøng tieáp theo ñöôïc ñònh vò theo vaïch vöøa nhaän . Neáu xaûy ra loãi thì quaõng ñöôøng tieáp theo vaãn ñöôïc ñònh vò theo vaïch cuõ tröôùc ñoù ( phaûi tính ñaõ qua 1 vaïch ) .

Trang - 4 -

Tài liệu kỹ thuật BKpro – http://www.bkpro.info

Sau ñaây laø ví duï cuï theå chöông trình doø ñöôøng chaïy thaúng qua 4 vaïch ngang. Chöông trình chính: (chi tieát: DO_DUONG.ASM) … ;************** CHUONG_TRINH_CHINH ************************* CHUONG_TRINH_CHINH: ;--CHAY THANG 3 VACH CALL VAN_TOC_CHAM ;1 CALL CHAY_THANG_TIM_VACH_NGANG MOV DEM_VACH,#3 CALL VAN_TOC_MAX CALL CHAY_THANG

;3

…

Chöông trình chính coù 2 coâng vieäc : CHAY_THANG_TIM_VACH_NGANG ñeå ñònh vò toïa ñoä xuaát phaùt ; CHAY_THANG qua 3 vaïch ngang .

Trang - 5 -

Tài liệu kỹ thuật BKpro – http://www.bkpro.info

Löu ñoà giaûi thuaät:

Trang - 6 -