Robot Dò Line [PDF]

Zitiervorschau

ROBOT TỰ HÀNH TRONG NHÀ HÀNG

CHƯƠNG 1: TỔNG QUAN.............................................................................................................................3 1.1. Giới thiệu robot tự hành trong nhà hàng..........................................................................................3 1.2 Mục đích chọn đề tài..........................................................................................................................4 1.3 Sơ lược các bước thực hiện:...............................................................................................................4 Chương 2 Lựa chọn phương án thiết kế.......................................................................................................6 2.1 Cấu trúc..............................................................................................................................................6 2.2 Lựa chọn động cơ...............................................................................................................................6 2.3 Lựa chọn phương pháp dẫn đường....................................................................................................9 2.4 Lựa chọn phương án bộ điều khiển..................................................................................................11 Chương 3 Thiết kế hệ thống cơ khí............................................................................................................13 3.1 Lựa chọn các phương án thiết kế.....................................................................................................13 3.2 Tính toán chọn động cơ....................................................................................................................14 3.3 Thiết kế robot trên phần mềm soildworks.......................................................................................17 3.3.1 Khung xe........................................................................................................................................18 Chương 4 : GIỚI THIỆU VI XỬ LÍ VÀ THÀNH PHẦN CỦA ROBOT.................................................................20 4.1 Giới thiệu STM32f103c8t6................................................................................................................20 4.1.1 Đặc trưng của STM32F103C8T6:...................................................................................................21 4.1.2 Memory map:................................................................................................................................24 4.1.3 Pinouts:.........................................................................................................................................25 4.1.4 Power Control................................................................................................................................26 4.1.5 Reset and clock control:................................................................................................................27 4.2 Mạch điều khiển động cơ L298:.......................................................................................................28 4.3 Động Cơ Giảm Tốc Màu Vàng 1 Trục GT130-148-1T.........................................................................32 4.4 Bánh xe mecanum wheel omni wheel 60mm...................................................................................34 4.6 Nguồn...............................................................................................................................................35 4.7 Mạch dò line 6 quang trở.................................................................................................................36 Chương 5: Giải thuật và chương trình điều khiển......................................................................................38 5.1 Giải thuật..........................................................................................................................................38 5.1.1 Giải thuật tính góc và hướng rẽ:....................................................................................................38

1

5.1.2 Giải thuật PID điều khiển tốc độ 4 động cơ xe...............................................................................38 5.2 Chương trình điều khiển...................................................................................................................48 5.2.1 Sơ đồ mạch điện............................................................................................................................48 5.2.2 Code lập trình điều khiển..............................................................................................................52 Chương 6 KẾT LUẬN VÀ HƯỚNG PHÁT TRIỂN...........................................................................................66 6.1.Kết quả.............................................................................................................................................66 6.2.Hướng phát triển..............................................................................................................................66 Tài liệu tham khảo......................................................................................................................................67

2

CHƯƠNG 1: TỔNG QUAN Chương này trình bày tổng quan về đề tài: “Robot tự hành hoạt động trong nhà hàng”. Nội dung chương này gồm những dạng robot tự hành, những nghiên cứu và thực nghiệm về robot sử dụng cho nhà hàng Từ đó, rút ra được mục tiêu, cơ sở và định hướng thực hiện đề tài. 1.1. Giới thiệu robot tự hành trong nhà hàng Nhà hàng là khu vực tập trung đông người , trang thiết bị, nguyên vật liệu phục vụ cho quá trình sản xuất, bảo quản, di chuyển thức ăn. Di chuyển thức ăn trong nhà hang theo cách truyền thống là sử dụng xe đẩy con người vận hành. Hơn nữa việc di chuyển thức ăn cần tốn thêm nhân công và thời gian. Trên thế giới, robot tự hành đã và đang ứng dụng rộng rãi trong công đoạn di chuyển đó, mang lại hiệu quả to lớn trong công việc. Robot tự hành là dạng mobile robot tự di chuyển theo tín hiệu dẫn đường như laser, hình ảnh, từ trường, đường ray….Từ cuối thế kỷ XX, Robot tự hành ngày càng phổ biến hơn nhờ tiến bộ của khoa học – kỹ thuật. Những cải tiến quan trọng trong công nghệ cảm biến và dẫn đường, bộ xử lý và truyền thông, thiết bị truyền động, nguồn năng lượng giúp tạo ra hệ thống robot tự hành (có năng suất hoạt động cao, ổn định và chi phí vận hành thấp. Việc áp dụng robot vào nhà hàng có lợi ích sau:  Hoạt động liên tục: cả ngày và đêm.  Thay thế con người vận chuyển đồ ăn .  Giảm chi phí vận hành, đặc biệt là chi phí đào tạo, sử dụng nhân công.  Tính chính xác và độ ổn định cao giúp quá trình sản xuất được liên tục.  Giảm thời gian vận chuyển.

3

1.2 Mục đích chọn đề tài Robot dò line và tránh vật cản vừa có nhiều ứng dụng trong thực tế vừa dễ dàng để sinh viên vận dụng những kiến thức tiếp thu được trên giảng đường vào nó. Với những kết cấu cơ khí đơn giản nhưng lại có thể kết hợp được với khá nhiều thành phần điện tử (encoder, sensor xác định đường line, sensor đo khoảng cách…) nên những Robot này rất phù hợp để sinh viên học tập và nghiên cứu thêm về ngành Tự động hóa một cách cụ thể. 1.3 Sơ lược các bước thực hiện: a) Phân tích và lựa chọn phương án Đây là bước đầu tiên của quá trình thiết kế nhằm đưa ra hướng giải quyết vấn đề: - Đề xuất phương án khả thi: cơ khí, điện, điều khiển - Đánh giá và lựa chọn phương án thiết kế. - Tìm hiểu và đánh giá các thiết bị trên thị trường để thực hiện phương án: động cơ, vi điều khiển…. b) Thiết kế hệ thống điện – điện tử Với các phương án điện đã chọn, tiến hành lựa chọn thông số và sử dụng các thiết bị, kết hợp các thiết bị thành một khối hoàn chỉnh. Công việc bao gồm: - Lựa chọn thông số vi điều khiển, các mô đun công suất và cảm biến. - Sử dụng được các cảm biến của robot, thiết kế bộ điều khiển sử dụng hồi tiếp vận tốc để điều khiển động cơ.

4

c) Xây dựng giải thuật điều khiển Tiến hành thiết kế các giải thuật điều khiển cho robot với các công việc sau: - Thiết kế giải thuật tìm đường trên máy tính cho một hệ thống kho hàng cụ thể. - Thiết kế giải thuật điều khiển robot tự hành để đáp ứng các yêu cầu. d) Lập trình Lập trình trên máy tính và vi điều khiển, các công việc gồm:  Lập trình giao diện chọn sản phẩ, trên máy tính - Lập trình giải thuật tìm đường đi đến tất cả vị cần đến . - Gửi dữ liệu đã xử lý cho robot.  Lập trình vi điều khiển trên robot - Nhận dữ liệu từ máy tính, thực hiện di chuyển. - Gửi về máy tính những thông số về trạng thái của robot. e) Thực nghiệm và đánh giá kết quả - Tiến hành thực nghiệm. - Đánh giá kết quả thực nghiệm, nêu lên những hạn chế của giải thuật và mô hình

5

Chương 2 Lựa chọn phương án thiết kế 2.1 Cấu trúc Cấu trúc của robot gồm: - Hai bánh xe sau được nối với hai motor truyền động cho xe. - Hai bánh xe trước được nối với hai motor truyền động cho xe. - Bo mạch chủ dùng chíp stm32f103c8t6 để tiếp nhận tín hiệu từ sensor và phát lệnh điều khiển hoạt động của xe. - Hệ thống dò đường ( Sensor) gồm 6 cặp quang trở- led siêu sáng - Mạch điều khiển động cơ L298N

2.2 Lựa chọn động cơ Robot tự hành di động nên thông thường sử dụng động cơ dùng dòng điện DC. Một số loại động cơ dùng dòng điện DC như: động cơ bước, động cơ DC, động cơ DC servo. Robot tự hành cần điều khiển vận tốc nên động cơ DC phải có hồi tiếp vận tốc (mặc dù một số động cơ DC điều khiển được chính xác vận tốc, không cần hồi tiếp nhưng giá sẽ rất cao). Do đó chọn động cơ bước hoặc động cơ DC servo là phương án khả thi. Sau đây là những phân tích về ưu, nhược điểm của hai loại động cơ này, từ đó có thể xác định được loại động cơ cần dùng để phù hợp với yêu cầu đặt ra. Động cơ bước  Động cơ bước thông thường là loại động cơ điều khiển vòng hở, biến đổi các xung điện rời rạc kế tiếp nhau thành chuyển động quay của rotor. Giá trị góc quay phụ thuộc và số xung điện cấp vào động cơ. Mỗi vòng quay có số bước cố định, thay đổi tốc độ quay bằng cách thay đổi tốc độ tín hiệu điện cấp cho động cơ

6

Hình 2.1: Động cơ bước Ưu điểm: - Không cần hồi tiếp tín hiệu. - Điều khiển chính xác, không có vọt lố và có momen giữ tại một vị trí. Nhược điểm: - Dễ bị trượt bước khi mang tải lớn. - Tốc độ của động cơ không cao, tối đa từ 1000 ÷ 2000 (rpm). - Gây rung động. Động cơ DC có hộp giảm tốc Động cơ DC có hộp giảm tốc là động cơ DC có thêm bộ phận đo thông số đầu ra của động cơ thông thường là góc quay, vận tốc quay. Từ các sai số góc quay, vận tốc quay thì bộ điều khiển sẽ tăng hay giảm dòng cấp cho động cơ để đạt góc quay, vận tốc mong muốn.

7

Hình 2.2: Động cơ giảm tốc Ưu điểm: - Momen khởi động và vận tốc lớn. - Động cơ chạy êm khi đạt vận tốc cố định. - Có tín hiệu hồi tiếp nên có thể điều khiển được góc quay và vận tốc. Nhược điểm: - Khi dừng lại, tùy theo chất lượng của bộ điều khiển ảnh hưởng đến đáp ứng nên có thể gây dao động, rung lắc. Dựa vào phân tích và điều kiện hoạt động của robot, động cơ DC có hộp giảm tốc sẽ là phương án được lựa chọn để thiết kế robot.

8

2.3 Lựa chọn phương pháp dẫn đường Từ chương tổng quan chúng ta có thể thấy được các loại đường dẫn cố định phù hợp cho đề tài này. Sau đây sẽ phân tích đặc điểm các loại đường dẫn thông dụng là: màu sắc, từ trường, dây cảm ứng điện. Đường dẫn màu: Dán đường dẫn có màu sắc tương phản với mặt sàn, robot sử dụng các loại cảm biến quang học để nhận biết. Đường dẫn màu có các ưu điểm là việc lắp đặt, thay đổi đường dẫn màu không ảnh hưởng đến mặt sàn. Tuy nhiên, các loại cảm biến quang dễ bị nhiễu bởi ánh sáng đèn điện, mặt trời. Bề mặt đường dẫn dễ bám bẩn, mài mòn gây ra sai lệch khi robot di chuyển.

Hình 2.3: Đường dẫn màu đen trên nền trắng. Đường dẫn từ trường: Có 2 cách lắp đặt đường dẫn từ trường là dán trực tiếp lên mặt sàn hoặc thi công âm sàn. Đường dẫn từ trường âm sàn cần máy móc để lắp đặt. Cách dán băng từ lên mặt sàn có ưu điểm là dễ lắp đặt, 9

điều chỉnh đường dẫn. Tuy nhiên, tuổi thọ đường dẫn giảm khi có nhiều phương tiện hoạt động với tải trọng nặng. Ưu điểm của đường dẫn từ là không ảnh hưởng bởi ánh sáng, bụi bẩn. Nhược điểm của đường dẫn là nhiễu do từ trường.

Hình 2.4: Đường dẫn từ và cảm biến từ. Đường dẫn cảm ứng điện: Sử dụng dòng điện thay đổi để tạo ra từ trường quanh dây dẫn. Đường đẫn được chôn dưới bề mặt sàn, robot được lắp anten gồm các cuộn dây quấn để nhận biết đường đi. Ưu điểm của loại này là nhiều đường dẫn có thể phân biệt bằng tần số khác nhau. Nhược điểm của đường dẫn là: phải có bộ nguồn điều khiển dòng điện qua dây dẫn, khi lắp đặt cần cắt xuống bề mặt sàn dẫn đến chi phí cao. Ngoài ra, những thiết bị dẫn đường này rất hiếm trên thị trường trong nước.

Hình 2.5: Nguyên lý dẫn đường bằng cảm ứng điện. 10

Hình 2.6: Anten dò đường dẫn cảm ứng điện. Từ những phân tích trên và điều kiện thiết kế hệ thống robot cũng như giá thành, đường dẫn màu được chọn cho đề tài này. 2.4 Lựa chọn phương án bộ điều khiển Có hai phương án điều khiển có thể sử dụng cho robot đó là: - Sử dụng PLC: Bộ điều khiển PLC được sử dụng nhiều trong công nghiệp bởi độ bền và tính tin cậy cao. Các thành phần trong hệ thống PLC đã được chuẩn hóa và dễ dàng tìm mua trên thị trường. Tuy vậy, hệ thống điều khiển dùng PLC có kích

thước khá lớn và giá thành cao.

11

Hình 2.7: PLC Miubishi. - Sử dụng vi điều khiển: Phương án này rất phổ biến cho cách dạng robot di chuyển. Ưu điểm là kích thước nhỏ gọn, giá thành rẻ. Tuy nhiên chỉ sử dụng được cho những hệ thống vừa và nhỏ, thời gian hoạt động không dài

Hình 2.8 Kit STM 32 Phương án lựa chọn là vi điều khiển giúp tiết kiệm chi phí đối với những mô hình thử nghiệm.

12

Chương 3 Thiết kế hệ thống cơ khí 3.1 Lựa chọn các phương án thiết kế Dưạ trên các yêu cầu đưa ra ta lựa chọn phương án thiết kế robot:

Hình 3.1: Sơ đồ nguyên lý mô hình xe dò line

1

Bánh xe

2

Động cơ DC

3

Mạch dò line

4

Thân robot

13

3.2 Tính toán chọn động cơ Địa hình làm việc của robot là địa hình phẳng, giả thiết bỏ qua sự biến dạng của bánh xe, bỏ qua lực cản của không khí trong quá trình robot di chuyển. Các thông số của robot: - Khối lượng robot ước tính: = 3 (kg). - Chọn bánh xe có đường kính: D= 60 (mm). - Tốc độ tối đa là: 0,1 (m/s).

Hình 3.2: Bánh xe dẫn động

Thông số cơ bản của bánh xe là: đường kính là 60 (mm), bề rộng của bánh xe là 28 (mm )

14

Phân tích lực tác dụng lên một bánh xe chủ động gồm 4 lực tác dụng như sau:

Hình 3.3: Sơ đồ đặt lực Với:

 - Fms là lực ma sát nghỉ giữa bánh xe và bề mặt sàn bê tông.  - Fw là lực kéo sinh ra do momen của động cơ.  - N là phản lực của mặt sàn.  - P là một nửa trọng lực của xe.  - M là momen xoắn của động cơ.

- m là một nửa tổng trọng lượng của robot . Chọn phương án chuyển động của robot là chuyển động thẳng. Hai bánh dẫn động di chuyển cùng tốc độ về phía trước với vận tốc v = 0,3 (m/s). Tốc độ quay của bánh xe là: n

60  1000  0.1  2 xR 318 (rpm)

Phương trình cân bằng lực: 15

     Fw  Fms  N  P  ma

Với a là gia tốc của xe. Khi xe chuyển động đều Vận tốc không đổi là v=0, 1 (m/s), nên gia tốc a =0 ( m/ s ) . Khi đó:

N

mg 9.8 1.5   7.35( N ) 2 2

Lực kéo sinh ra do momen của động cơ là: Fw  Fms    N  0.015  7.35  0.11(N)

Với: hệ số ma sát lăn =0,015 khi di chuyển trên sàn bê tông. Công suất cần thiết để xe di chuyển ổn định với vận tốc 0,1 (m/s) là: P1  Fw  v  0.11 0.1  0.011 (w)

Khi xe tăng tốc Xe tăng tốc với gia tốc a=0, 1 ( m/s ) . Lực kéo sinh ra do momen của động cơ là: Fw  Fms  ma  0.11  0.11.5  0.26( N )

Với hệ số ma sát tĩnh là 0.015 Momen xoắn cần thiết để xe tăng tốc là: M  Fw  r  0.26  0.03  0.0078 (N.m)

Công suất cần thiết để xe tăng tốc lên 0,1 (m/s) là: P1  Fw  v  0.26  0.1  0.026 (W)

Từ các kết quả trên chúng ta chọn động cơ: Động Cơ Giảm Tốc Vàng 3V9V

16

Hình 3.4: Động Cơ Giảm Tốc Vàng 3V-9V 3.3 Thiết kế robot trên phần mềm soildworks

Hình 3.5: Mô hình robot

17

Hình 3.6: Mặt dứơi của robot

HÌnh3.7: Các kích thước của Robot 3.3.1 Khung xe 18

Hình 3.8: Khung Xe

Hinh 3.9: Các kích thước khung xe

19

Chương 4 : GIỚI THIỆU VI XỬ LÍ VÀ THÀNH PHẦN CỦA ROBOT

4.1 Giới thiệu STM32f103c8t6 STM32 là chip của ST, dựa trên nền lõi ARM Cortex-M. Dòng ARM Cortex™-M là thế hệ mới, thiết lập các tiêu chuẩn mới về hiệu suất, chi phí, ứng dụng cho các thiết bị cần tiêu thụ năng lượng thấp, và đáp ứng yêu cầu thời gian thực khắc khe.

Hình 4.1: KIT STM32F103C8T6  KIT STM32F103C8T6 Mini Board  được thiết kế nhỏ gọn, tích hợp sẳn giao tiếp USB, chân nạp SWD giúp sử dụng module dễ dàng để học tập và kết nối với những module khác. Thông tin module - Vi điều khiển : STM32F103C8T6 - ARM 32 CPU Cortex-M3. - Điện áp hoạt động : 2V-3.6V DC - Chế độ Debug: SWD. 20

-

Tần số làm việc : 72MHz . Bộ nhớ flash : 64K SRAM : 20K Tích hợp USB để cấp nguồn và giao tiếp. Hổ trợ các chuẩn giao tiếp : CAN, I2C, SPI UART/USART, USB. Có led báo nguồn Nút nhấn Reset Kích thước: 5.3cm x 2.2cm

Ưu điểm STM32F103C8T6 -

Tiêu thụ điện năng thấp so với các vi điều khiển khác Hiệu suất khá cao Dễ dàng tiếp cận và xử lý Mức giá phù hợp cho sinh viên

4.1.1 Đặc trưng của STM32F103C8T6:

Hình 4.2 STM32F103C8T6 Chip STM32F103C8T6 t có lõi ARM-Cotex-M3 32 bit chính hãng ST MicroElectronic. Bảng 4.1 Thông số kỹ thuật của STM32F103C8T6 Datasheets

STM32F103x(8,B) Datasheet

Product Photos

48-LQFP

21

Product Training Modules

KIT STM32

Featured Product

STM32 32-bit Cortex MCUs

Standard Package  

250

Category

Integrated Circuits (ICs)

Family

Embedded - Microcontrollers

Core Processor

ARM® Cortex®-M3

Core Size

32-Bit

Speed

72MHz

Connectivity

CAN, I²C, IrDA, LIN, SPI, UART/USART, USB

Peripherals

DMA, Motor Control PWM, PDR, POR, PVD, PWM, Temp Sensor, WDT

Number of I/O

37

Program Memory Size

64KB (64K x 8)

Program Memory Type

FLASH

EEPROM Size

-

RAM Size

20K x 8

Voltage - Supply (Vcc/Vdd)

2 V ~ 3.6 V

Data Converters

A/D 10x12b

Oscillator Type

Internal

Operating Temperature

-40°C ~ 85°C

Package / Case

48-LQFP

 Sơ đồ phần cứng:

22

Hình 4.3: Sơ đồ phần cứng

4.1.2 Memory map:

23

Hình 4.4: Memory map:

24

4.1.3 Pinouts:

Hình 4.5: Pinouts

25

4.1.4 Power Control

Hình 4.6: Power Control  Thiết bị này đòi hỏi một nguồn cung cấp điện áp hoạt động 1,8 đến 3,6 V (VDD). Một điều chỉnh điện áp tuyến tính nhúng được sử dụng để cung cấp 1,2 V điện kỹ thuật số nội bộ.  Các đồng hồ thời gian (RTC), sao lưu sổ đăng ký RTC, và SRAM sao lưu ( KP SRAM) có thể được cung cấp từ điện áp VBAT khi nguồn cung cấp VDD chính đang tắt. 26

4.1.5 Reset and clock control:

Hình 4.7: Reset anh Clock control

4.2 Mạch điều khiển động cơ L298:  Để lá i độ ng cơ DC ta sử dụ ng L298. IC L298 là mạ ch tích hợ p đơn chip có kiểu vỏ cô ng suấ t 15 châ n (multiwatt 15) hoặ c PowerSO20 (linh kiện dá n cô ng

27

suấ t). Là IC mạ ch cầ u đô i (dual full-bridge) có khả năng hoạ t độ ng ở điện thế cao, dò ng cao. Nó đượ c thiết kế tương thích chuẩ n TTL và lá i tả i cả m khá ng như relay, cuộ n solenoid, độ ng cơ DC và độ ng cơ bướ c. Nó có 2 châ n enable để cho phép/khô ng cho phép IC hoạ t độ ng, độ c lậ p vớ i cá c châ n tín hiệu và o. Cự c phá t (emitter) củ a transistor dướ i củ a mỗ i mạ ch cầ u đượ c nố i vớ i nhau và nố i ra châ n ngoà i để nố i vớ i điện trở cả m ứ ng dò ng khi cầ n.  Tần công suất ngõ ra: IC L298 tích hợ p 2 tầ ng cô ng suấ t (A, B). Tầ n cô ng suấ t chính là mạ ch cầ u và ngõ ra củ a nó có thể lá i cá c loạ i tả i cả m thô ng dụ ng ở nhiều chế độ hoạ t độ ng khá c nhau (tù y thuộ c và o sự điều khiển ở ngõ và o) Dò ng điện từ châ n ngõ ra chả y qua tả i đến châ n cả m ứ ng dò ng: điện trở ngoà i RSA, RSB cho phép việc cả m ứ ng cườ ng độ dò ng điện nà y.  Tần ngõ vào: Mỗ i cầ u đượ c điều khiển bở i 3 cổ ng ngõ và o In1, In2, EnA (và In3, In4, EnB cho cầ u cò n lạ i). Cá c châ n In có tá c dụ ng khi châ n En ở mứ c cao, khi châ n En ở mứ c thấ p, cá c châ n ngõ và o In ở trạ ng thá i cấ m. Tấ t cả cá c châ n đều tương thích vớ i chuẩ n TTL. 

Hình 4.8: Mạch điều khiển động cơ L298

28

Thông số kỹ thuật: - IC chính: L298 - Dual Full Bridge Driver - Điện áp đầu vào: 5~30VDC - Công suất tối đa: 25W 1 cầu (lưu ý công suất = dòng điện x điện áp nên áp cấp vào càng cao, dòng càng nhỏ, công suất có định 25W). - Dòng tối đa cho mỗi cầu H là: 2A - Mức điện áp logic: Low -0.3V~1.5V, High: 2.3V~Vss - Kích thước: 43x43x27mm Bảng 4.2: Chức năng của các chân

29

 Chế độ hoạ t độ ng: En 0

In1 x

In2 x

Chế độ hoạt động Khô ng hoạt độ ng

1

0

0

Phanh

1

0

1

Độ ng cơ quay trá i (phả i)

1

1

0

Độ ng cơ quay phải (trá i)

1

1

1

Phanh

 Sơ đồ nguyên lí: 

Hình 4.9: Sơ đồ nguyên lý 30

31

4.3 Động Cơ Giảm Tốc Màu Vàng 1 Trục GT130-148-1T

Động Cơ Giảm Tốc Vàng 3V-9V là mẫu động cơ được sử dung nhiều nhất để thiết kế các loại robot mô hình. Phục vụ cho sinh viên học tập và nghiên cứu chế tạo. Với giá thành rẻ, dễ sử dụng và mang tính ưu việt nên chiếc động cơ này được sử dụng rất nhiều với sinh viên các nghành kĩ thuật.

Hình 4.10: Động cơ giảm tốc Thông số kỹ thuật của động cơ giảm tốc Vàng    

Điện áp hoạt động:3V~ 9V DC (Hoạt động tốt nhất từ 6 - 8V)  Mômen xoắn cực đại: 800gf cm min 1:48 (3V)  Tốc độ không tải: 125 Vòng/ 1 Phút (3V)  (Với bánh 66mm: 26m/1p)  32

  

208 Vòng/ 1 Phút (5V)  (Với bánh 66mm: 44m/1p) Dòng không tải động cơ: 70mA (250mA MAX)

Hình 4.11: Sơ Đồ Động Cơ Giảm Tốc Vàng 3V-9V

Hình 4.12: Cấu tạo của động cơ giảm tốc

33

4.4 Bánh xe mecanum wheel omni wheel 60mm

Hình 4.13: Bánh xe mecanum Thông số kỹ thuật :  Đường kính: 60mm  Chiều rộng: 32 mm  Trọng lượng: 120g  Đường kính trung tâm: 3, 4, 5, 6, 8mm  Vật liệu cơ thể: Nhựa  Chất liệu con lăn: cao su  Số lượng con lăn: 8  Hướng lăn: 45 °  Tải trọng: 5kg / bánh

34

Hình 4.14: Các Kích thước chi tiết của bánh xe

4.6 Nguồn Sử dụng 3 viên pin 18650 Thông số kỹ thuật : -

Loại pin: Lithium Lion Kích thước: đường kính φ18 * chiều cao 65 (mm) Công suất danh nghĩa: 3500mAh Dòng xả Max: 10A Thời gian sạc và xả: 1000 lần Điện áp: 3.7V Kích thước: 18x65mm

35

-

Trọng lượng: 47g

Hình 4.15: Pin 18650 Đặc Điểm Nổi Bật Của Pin 18650: -

-

Tính nhất quán tốt và sức đề kháng nội bộ thấp Hiệu suất ổn định, tuổi thọ cao: sạc và xả 1000 lần, sau 500 chu kỳ sạc và xả liên tục dung lượng pin ≥ 85% công suất định mức. Độ an toàn cao Pin có khả năng chịu nhiệt tốt, có thể sử dụng được ở nhiệt độ khoảng 80°C

4.7 Mạch dò line 6 quang trở

36

Hình 4.16: Mạch dò line 6 quang trở

Hình 4.17: Sơ đồ mạch dò line

37

Chương 5: Giải thuật và chương trình điều khiển 5.1 Giải thuật 5.1.1 Giải thuật tính góc và hướng rẽ: Cần dò đường đưa ra phía trước, gồm có 6 photodiode nằm trên 1 đường thẳng . Khi hoạt động ánh sáng từ led chiếu xuống đường nếu gặp đường line màu trắng ánh sáng phản chiếu trở lại quang trở tương ứng, lúc đó tín hiệu được truyền về chân tương ứng của vi điều khiển là mức 0. Nếu ánh sáng từ led chiếu xuống gặp nền đen của đường line thì ánh sáng bị hấp thụ hết, lúc đó tín hiệu từ sensor báo về vi điều khiển là mức 1. 5.1.2 Giải thuật PID điều khiển tốc độ 4 động cơ xe a) Cơ bản về vòng điều khiển Một bộ điều khiển vi tích phân tỉ lệ (bộ điều khiển PID) là một cơ chế phản hồi vòng điều khiển (bộ điều khiển) tổng quát được sử dụng rộng rãi trong các hệ thống điều khiển công nghiệp – bộ điều khiển PID được sử dụng phổ biến nhất trong số các bộ điều khiển phản hồi. Một bộ điều khiển PID tính toán một giá trị "sai số" là hiệu số giữa giá trị đo thông số biến đổi và giá trị đặt mong muốn. ộ điều khiển sẽ thực hiện giảm tối đa sai số bằng cách điều chỉnh giá trị điều khiển đầu vào. Trong trường hợp không có kiến thức cơ bản về quá trình, bộ điều khiển PID là bộ điều khiển tốt nhất. Tuy nhiên, để đạt được kết quả tốt nhất, các thông số PID sử dụng trong tính toán phải điều chỉnh theo tính chất của hệ thống-trong khi kiểu điều khiển là giống nhau, các thông số phải phụ thuộc vào đặc thù của hệ thống. Giải thuật tính toán bộ điều khiển PID bao gồm 3 thông số riêng biệt, do đó đôi khi nó còn được gọi là điều khiển ba khâu: các giá trị tỉ lệ, tích phân và đạo hàm, viết tắt là P, I, và D. Giá trị tỉ lệ xác định tác động của sai số hiện

38

tại, giá trị tích phân xác định tác động của tổng các sai số quá khứ, và giá trị vi phân xác định tác động của tốc độ biến đổi sai số. Tổng chập của ba tác động này dùng để điều chỉnh quá trình thông qua một phần tử điều khiển như vị trí của van điều khiển hay bộ nguồn của phần tử gia nhiệt. Nhờ vậy, những giá trị này có thể làm sáng tỏ về quan hệ thời gian: P phụ thuộc vào sai số hiện tại, I phụ thuộc vào tích lũy các sai số quá khứ, và D dự đoán các sai số tương lai, dựa vào tốc độ thay đổi hiện tại. ằng cách điều chỉnh 3 hằng số trong giải thuật của bộ điều khiển PID, bộ điều khiển có thể dùng trong những thiết kế có yêu cầu đặc biệt. Đáp ứng của bộ điều khiển có thể được mô tả dưới dạng độ nhạy sai số của bộ điều khiển, giá trị mà bộ điều khiển vọt lố điểm đặt và giá trị dao động của hệ thống. Lưu ý là công dụng của giải thuật PID trong điều khiển không đảm bảo tính tối ưu hoặc ổn định cho hệ thống. Vài ứng dụng có thể yêu cầu chỉ sử dụng một hoặc hai khâu tùy theo hệ thống. Điều này đạt được bằng cách thiết đặt đội lợi của các đầu ra không mong muốn về 0. Một bộ điều khiển PID sẽ được gọi là bộ điều khiển PI, PD, P hoặc I nếu vắng mặt các tác động bị khuyết. ộ điều khiển PI khá phổ biến, do đáp ứng vi phân khá nhạy đối với các nhiễu đo lường, trái lại nếu thiếu giá trị tích phân có thể khiến hệ thống không đạt được giá trị mong muốn. b) Lí thuyết điều khiển PID Phần này chỉ mô tả dạng song song hoặc không tương tác của bộ điều khiển PID. Xin xem thêm "Alternative nomenclature and PID forms" cho những dạng khác. Sơ đồ điều khiển PID được đặt tên theo ba khâu hiệu chỉnh của nó, tổng của ba khâu này tạo thành bởi các biến điều khiển (MV). Ta có: MV (t )  Pout  I out  Dout

trong đó: Pout , I out và Dout là các thành phần đầu ra từ ba khâu của bộ điều khiển PID, được xác định như dưới đây. 39

Khâu tỉ lệ Khâu tỉ lệ (đôi khi còn được gọi là độ lợi) làm thay đổi giá trị đầu ra, tỉ lệ với giá trị sai số hiện tại. Đáp ứng tỉ lệ có thể được điều chỉnh bằng cách nhân sai số đó với một hằng số Kp, được gọi là hệ số tỉ lệ. Khâu tỉ lệ được cho bởi: trong đó:

Pout  K p e(t )

Pout : thừa số tỉ lệ của đầu ra Kp

: Hệ số tỉ lệ, thông số điều chỉnh e: sai số  t: thời gian hay thời gian tức thời (hiện tại) Hệ số của khâu tỉ lệ lớn là do thay đổi lớn ở đầu ra mà sai số thay đổi nhỏ. Nếu hệ số của khâu tỉ lệ quá cao, hệ thống sẽ không ổn định (xem phần điều chỉnh vòng). Ngược lại, hệ số nhỏ là do đáp ứng đầu ra nhỏ trong khi sai số đầu vào lớn, và làm cho bộ điều khiển kém nhạy, hoặc đáp ứng chậm. Nếu Hệ số của khâu tỉ lệ quá thấp, tác động điều khiển có thể sẽ quá bé khi đáp ứng với các nhiễu của hệ thống Hệ số của khâu tỉ lệ lớn là do thay đổi lớn ở đầu ra mà sai số thay đổi nhỏ. Nếu hệ số của khâu tỉ lệ quá cao, hệ thống sẽ không ổn định (xem phần điều chỉnh vòng). Ngược lại, hệ số nhỏ là do đáp ứng đầu ra nhỏ trong khi sai số đầu vào lớn, và làm cho bộ điều khiển kém nhạy, hoặc đáp ứng chậm. Nếu Hệ số của khâu tỉ lệ quá thấp, tác động điều khiển có thể sẽ quá bé khi đáp ứng với các nhiễu của hệ thống.

40

Hình 5.1 Đồ thị PV theo thờ gian tương ứng ba giá trị K p ( K d K p và Ki không đổi ) Drop(độ trượt) Nếu không có nhiễu, điều khiển tỉ lệ thuần túy sẽ không xác lập tại giá trị mong muốn của nó, nhưng nó vẫn duy trì một (độ trượt) sai số ổn định trạng thái, là một hàm của độ lợi tỉ lệ và độ lợi quá trình. Đặc biệt, nếu độ lợi quá trình-trong khoảng thời gian dài bị trôi do thiếu điều khiển, như việc làm mát một lò nung tới nhiệt độ phòng-được ký hiệu G và giả sử sai số xấp xỉ là hằng số, khi đó droop-độ trượt xảy ra khi độ lợi không đổi này bằng thừa số

P G  K pe e  G  Kp tỉ lệ của đầu ra,  out  với sai số là tuyến tính,   do đó  . Khi thừa số tỉ lệ, đẩy vào thông số tới giá trị đặt, được bù chính xác bởi độ lợi quá trình, nó sẽ kéo thông số ra khỏi giá trị đặt. Nếu độ lợi quá trình giảm, khi làm lạnh, thì trạng thái dừng sẽ nằm dưới điểm đặt, ta gọi là "droop-độ trượt". Chỉ các thành phần dịch chuyển (trung bình dài hạn, thành phần tần số không) của độ lợi quá trình mới tác động tới độ trượt-các dao động đều hoặc ngẫu nhiên trên hoặc dưới thành phần dịch chuyển sẽ bị triệt tiêu. Độ lợi quá trình có thể thay đổi theo thời gian hoặc theo các thay đổi bên ngoài, ví dụ như nếu nhiệt độ phòng thay đổi, việc làm lạnh sẽ nhanh hơn hoặc chậm hơn. 41

Độ trượt tỉ lệ thuận với độ lợi quá trình và tỉ lệ nghịch với độ lợi tỉ lệ, và là một khiếm khuyết không thể tránh được của điều khiển tỉ lệ thuần túy. Độ trượt có thể được giảm bớt bằng cách thêm một thừa số độ lệch (cho điểm đặt trên giá trị mong muốn thực tế), hoặc sửa đổi bằng cách thêm một khâu tích phân (trong bộ điều khiển PI hoặc PID), sẽ tính toán độ lệch thêm vào một cách hữu hiệu. Bất chấp độ trượt, cả lý thuyết điều chỉnh lẫn thực tế công nghiệp chỉ ra rằng khâu tỉ lệ là cần thiết trong việc tham gia vào quá trình điều khiển. Khâu tích phân Phân phối của khâu tích phân (đôi khi còn gọi là reset) tỉ lệ thuận với cả biên độ sai số lẫn quảng thời gian xảy ra sai số. Tổng sai số tức thời theo thời gian (tích phân sai số) cho ta tích lũy bù đã được hiệu chỉnh trước đó. Tích lũy sai số sau đó được nhân với độ lợi tích phân và cộng với tín hiệu đầu ra của bộ điều khiển. Biên độ phân phối của khâu tích phân trên tất cả tác động điều chỉnh được xác định bởi độ lợi tích phân,  Ki . Thừa số tích phân được cho bởi: t

I out  Ki  e( )d 0

trong đó

I out : thừa số tích phân của đầu ra

K i : độ lợi tích phân, 1 thông số điều chỉnh e: sai số  t: thời gian hoặc thời gian tức thời (hiện tại)



: một biến tích phân trung gian

42

Khâu tích phân (khi cộng thêm khâu tỉ lệ) sẽ tăng tốc chuyển động của quá trình tới điểm đặt và khử số dư sai số ổn định với một tỉ lệ chỉ phụ thuộc vào bộ điều khiển. Tuy nhiên, vì khâu tích phân là đáp ứng của sai số tích lũy trong quá khứ, nó có thể khiến giá trị hiện tại vọt lố qua giá trị đặt (ngang qua điểm đặt và tạo ra một độ lệch với các hướng khác). Để tìm hiểu thêm các đặc điểm của việc điều chỉnh độ lợi tích phân và độ ổn của bộ điều khiển, xin xem phần điều chỉnh vòng lặp.

Hình 5.2 Đồ thị PV theo thờ gian tương ứng ba giá trị Ki ( K p và K d không đổi ) Khâu vi phân Tốc độ thay đổi của sai số qua trình được tính toán bằng cách xác định độ dốc của sai số theo thời gian (tức là đạo hàm bậc một theo thời gian) và nhân 43

K

tốc độ này với độ lợi tỉ lệ  d . Biên độ của phân phối khâu vi phân (đôi khi được gọi là tốc độ) trên tất cả các hành vi điều khiển được giới hạn bởi độ lợi

K

vi phân,  d . Thừa số vi phân được cho bởi:

Dout  K d

d e(t ) dt

trong đó

Dout : thừa số vi phân của đầu ra K d : Độ lợi vi phân, một thông số điều chỉnh e: Sai số  t: thời gian hoặc thời gian tức thời (hiện tại) Khâu vi phân làm chậm tốc độ thay đổi của đầu ra bộ điều khiển và đặc tính này là đang chú ý nhất để đạt tới điểm đặt của bộ điều khiển. Từ đó, điều khiển vi phân được sử dụng để làm giảm biên độ vọt lố được tạo ra bởi thành phần tích phân và tăng cường độ ổn định của bộ điều khiển hỗn hợp. Tuy nhiên, phép vi phân của một tín hiệu sẽ khuếch đại nhiễu và do đó khâu này sẽ nhạy hơn đối với nhiễu trong sai số, và có thể khiến quá trình trở nên không ổn định nếu nhiễu và độ lợi vi phân đủ lớn. Do đó một xấp xỉ của bộ vi sai với băng thông giới hạn thường được sử dụng hơn. Chẳng hạn như mạch bù sớm pha.

44

Hình 5.3 Đồ thị PV theo thờ gian tương ứng ba giá trị K d ( K p và Ki không đổi )

Kết luận Khâu tỉ lệ, tích phân, vi phân được cộng lại với nhau để tính toán đầu ra của bộ điều khiển PID. Định nghĩa rằng u(t) là đầu ra của bộ điều khiển, biểu thức cuối cùng của giải thuật PID là: t

u(t)  MV(t)  K p e(t )  Ki  e( ) d  K d 0

d e(t ) dt

trong đó các thông số điều chỉnh là:

K

Độ lợi tỉ lệ,  p giá trị càng lớn thì đáp ứng càng nhanh do đó sai số càng lớn, bù khâu tỉ lệ càng lớn. Một giá trị độ lợi tỉ lệ quá lớn sẽ dẫn đến quá trình mất ổn định và dao động. 45

K

Độ lợi tích phân,  i giá trị càng lớn kéo theo sai số ổn định bị khử càng nhanh. Đổi lại là độ vọt lố càng lớn: bất kỳ sai số âm nào được tích phân trong suốt đáp ứng quá độ phải được triệt tiêu tích phân bằng sai số dương trước khi tiến tới trạng thái ổn định.

K

Độ lợi vi phân,  d giá trị càng lớn càng giảm độ vọt lố, nhưng lại làm chậm đáp ứng quá độ và có thể dẫn đến mất ổn định do khuếch đại nhiễu tín hiệu trong phép vi phân sai số.

Giải thuật lập trình

Vi xử lý thực hiện giải thuật PID

Modules công suất

46

Động cơ DC, ENCODER phản hổi

Các bước thực hiện giải thuật PID trong chương trình: -

Sai_số_trước_2 = Sai_số_trước_1 Sai_số_trước_1 = Sai_số Sai_số = Giá_trị_đặt – Giá_trị_cảm_biến_đọc_về Trị_điều_khiển_trước = Trị_điều_khiển Trị_điều_khiển = Trị_điều_khiển_trước + Kp*(Sai_số Sai_số_trước_1) + + Ki*T/2*(Sai_số + Sai_số_trước_1) + + Kd/T*(Sai_số - 2*Sai_số_trước_1 + Sai_số_trước_2) - Các thông số Kp, Ki, Kd được lựa chọn sau nhiều lần thử nghiệm trong tình trạng mang tải để đảm bảo bộ PID có được đáp ứng tốt nhất

47

5.2 Chương trình điều khiển 5.2.1 Sơ đồ mạch điện

Hình 5.4 Sơ đồ mạch dò line

Hình 5.2: Sơ đồ mạch điện 48

Bắt đầu

Chạy các chương trình khởi tạo (khởi tạo timer điều khiển động cơ, các cổng I/O, bộ ADC,… )

Lấy mẫu nền và gán các giá trị ADC của line ( black_line1 … black_line6) (hàm check_nen())

Chạy động cơ tiến vào line, khởi tạo timer 2 để tính toán các thông số bộ điều khiển PID

Tắt chân led tương ứng VD: line[0] < black_line1 -> led1 off

Sai

Nếu giá trị ADC (line[0]… line[5] ) > black_line1 …

Đúng

49

Bật chân led tương ứng

Đúng

Gán lỗi tương ứng (Err)

Tính toán giá trị điều khiển và cấp tín hiệu điều khiển động cơ

Phần kiểm tra trạng thái LED: - Mạch dò line có 6 quang trở, trên mạch điều khiển có 6 bóng led tương ứng với trạng thái của 6 quang trở đó 50

- Khi vào vạch đen, quang trở nào gặp vạch sẽ làm sáng led tương ứng. 1 là chạm vạch, 0 là ngoài vạch. ví dụ ta có bảng sau: Err

1

2

3

4

5

6

0

0

0

1

1

0

0

1

0

0

1

0

0

0

2

0

1

1

0

0

0

4

0

1

0

0

0

0

6

1

1

0

0

0

0

8

1

0

0

0

0

0

Err

1

2

3

4

5

6

0

0

0

1

1

0

0

-1

0

0

0

1

0

0

-2

0

0

0

1

1

0

-4

0

0

0

0

1

0

-6

0

0

0

0

1

1

-8

0

0

0

0

0

1

51

5.2.2 Code lập trình điều khiển a) Code ADC_line

#ifndef _ADC_LINE_DETECT__ #define _ADC_LINE_DETECT__

#include "stm32f10x.h" #include "stm32f10x_adc.h" #include "stm32f10x_gpio.h" #include "stm32f10x_exti.h"

#define

line1_pin_IN

GPIO_Pin_0 //ADC channel 1

#define channel 2

line2_pin_IN

GPIO_Pin_1

#define

line3_pin_IN

GPIO_Pin_2 //ADC channel 3

#define

line4_pin_IN

GPIO_Pin_3 //ADC channel 4

#define

line5_pin_IN

GPIO_Pin_4 //ADC channel 5

#define

line6_pin_IN

GPIO_Pin_5 //ADC channel 6

#define

line_ADC_IN_port

GPIOA

#define

line1_pin_OUT

GPIO_Pin_11 //ADC channel 1

//ADC

52

#define channel 2

line2_pin_OUT

GPIO_Pin_10

//ADC

#define

line3_pin_OUT

GPIO_Pin_1 //ADC channel 3

#define

line4_pin_OUT

GPIO_Pin_0 //ADC channel 4

#define

line5_pin_OUT

GPIO_Pin_12 //ADC channel 5

#define

line6_pin_OUT

GPIO_Pin_13 //ADC channel 6

#define

line_Digital_OUT_port

GPIOB

#define

ADC1_DR_Address

((uint32_t)0x4001244C)

void ADC_IN_Init(void); void Digital_OUT_Init(void); void Line_detect_Init(void); void on_LED(int i); void off_LED(int i); void laymaunen(uint32_t time); void laymauline(uint32_t time); void check_nen(void); void adc_to_digital(void); void reset_val(void); #endif

b) Code Motor 53

#include "motor.h" #include "main.h"

void setup_motor(void){

RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOB,ENABLE); RCC_APB2PeriphClockCmd(RCC_APB2Periph_AFIO,ENABLE);

GPIO_PinRemapConfig(GPIO_Remap_SWJ_JTAGDisable,ENABLE); //GPIO_PinRemapConfig(GPIO_Remap_SWJ_NoJTRST,ENA BLE);

//dir pin GPIO_InitTypeDef dir_config; dir_config.GPIO_Mode = GPIO_Mode_Out_PP; dir_config.GPIO_Pin=in1|in2|in3|in4; dir_config.GPIO_Speed = GPIO_Speed_10MHz; GPIO_Init(GPIOB,&dir_config); //pwm pin GPIO_InitTypeDef pwm_config; pwm_config.GPIO_Mode = GPIO_Mode_AF_PP; pwm_config.GPIO_Pin = pwm1|pwm2; pwm_config.GPIO_Speed = GPIO_Speed_50MHz; 54

GPIO_Init(GPIOB,&pwm_config);

}

void TIM_motor_configuration(void){ //Timer config 1kHz pwm // TIM4 is connected to APB1 bus that have default clock 72MHz // The frequency of TIM4 is 72MHz // TIM4 has 4 channels: Cn1:PA0, Cn2: PA1, Cn3:PA2, Cn4: PA3 /*setup clock*/ RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM4,ENABLE); /*Timer struct*/ TIM_TimeBaseInitTypeDef PWM_Timer4_Struct; PWM_Timer4_Struct.TIM_ClockDivision = TIM_CKD_DIV1; PWM_Timer4_Struct.TIM_CounterMode = TIM_CounterMode_Up; /*Period: cycle of Pulse */ // TIM_Period determine the PWM frequency by this equation: // timer_clock = TIM_Frequency / (TIM_Prescaler +1)

55

// setup TIM_Prescaler = 9 => timer_clock = 72MHz/(9+1) = 7.2MHz // PWM_frequency = timer_clock / (TIM_Period + 1) // If we want 1kHz PWM we can calculate: // TIM_Period = (timer_clock / PWM_frequency) - 1 // TIM_Period = (7.2MHz / 1kHz) - 1 = 7199 // => PWM_Frequency = TIM_Frequency/[(TIM_Prescaler +1) (TIM_Period +1)] /*TIM_Period can be from 0 to 65535*/ // PWM_Timer4_Struct.TIM_Period = 7199; //1kHz // PWM_Timer4_Struct.TIM_Period = 4199; //1.5kHz // PWM_Timer4_Struct.TIM_Period = 3599; //2kHz PWM_Timer4_Struct.TIM_Period = 255; // PWM_Timer4_Struct.TIM_Period = 9999; /*Frequency devider*/ /*TIM_Prescaler can be from 0 to 65535*/ PWM_Timer4_Struct.TIM_Prescaler = 280; /*the number begin the since the first loop. eg repetitionCounter =1 : First loop: 0,1,2,3..... Second loop: 1,2,3.... */ PWM_Timer4_Struct.TIM_RepetitionCounter = 0; TIM_TimeBaseInit(TIM4,&PWM_Timer4_Struct); TIM_Cmd(TIM4,ENABLE);

56

//tim OC config TIM_OCInitTypeDef PWM_OC_Struct; PWM_OC_Struct.TIM_OCMode = TIM_OCMode_PWM1; /*First pulse is high or low*/ PWM_OC_Struct.TIM_OCPolarity = TIM_OCPolarity_High; /*Enable output state*/ PWM_OC_Struct.TIM_OutputState = TIM_OutputState_Enable; /*% Pulse */ // Duty cycle calculation equation: // TIM_Pulse = (((TIM_Period + 1) * duty_cycle) / 100) - 1 // Ex. 25% duty cycle: // TIM_Pulse = (((7199 + 1) * 25) / 100) - 1 = 1799 // TIM_Pulse = (((7199 + 1) * 75) / 100) - 1 = 5399 // We initialize PWM value with duty cycle of 0% PWM_OC_Struct.TIM_Pulse = 0; TIM_ARRPreloadConfig(TIM4,ENABLE); TIM_Cmd(TIM4,ENABLE); TIM_OC2Init(TIM4,&PWM_OC_Struct); TIM_OC2PreloadConfig(TIM4,TIM_OCPreload_Enable);

TIM_OC3Init(TIM4,&PWM_OC_Struct); TIM_OC3PreloadConfig(TIM4,TIM_OCPreload_Enable); }

57

void motor_Run(uint16_t number, uint16_t dirrect, uint32_t speed){ switch(number){ case 1: switch(dirrect){ case 1: GPIO_SetBits(dir_port,in1); GPIO_ResetBits(dir_port,in2); TIM4->CCR2 = speed; break; case 0: GPIO_SetBits(dir_port,in2); GPIO_ResetBits(dir_port,in1); TIM4->CCR2= speed; break; } break; case 2: switch(dirrect){ case 1: GPIO_SetBits(dir_port,in3); GPIO_ResetBits(dir_port,in4); TIM4->CCR3 = speed; break; case 0: GPIO_SetBits(dir_port,in4); 58

GPIO_ResetBits(dir_port,in3); TIM4->CCR3 = speed; break; } break; }

}

void tim2_Init(void){ RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM2,ENABLE); TIM_TimeBaseInitTypeDef tim2_struct; tim2_struct.TIM_ClockDivision = TIM_CKD_DIV1; tim2_struct.TIM_CounterMode = TIM_CounterMode_Up; tim2_struct.TIM_Period = 7199; tim2_struct.TIM_Prescaler =99; tim2_struct.TIM_RepetitionCounter = 0; TIM_TimeBaseInit(TIM2,&tim2_struct); TIM_Cmd(TIM2,ENABLE); NVIC_InitTypeDef tim2_nvic; tim2_nvic.NVIC_IRQChannel = TIM2_IRQn; tim2_nvic.NVIC_IRQChannelCmd = ENABLE; tim2_nvic.NVIC_IRQChannelPreemptionPriority = 0xff; tim2_nvic.NVIC_IRQChannelSubPriority = 0xff; NVIC_Init(&tim2_nvic); 59

TIM_ITConfig(TIM2,TIM_IT_Update,ENABLE); } void TIM2_IRQHandler(void){ if (TIM_GetITStatus(TIM2,TIM_IT_Update) != RESET){ TIM_ClearITPendingBit(TIM2,TIM_IT_Update); // GPIO_WriteBit(GPIOC,GPIO_Pin_13,! GPIO_ReadInputDataBit(GPIOC,GPIO_Pin_13)); line_follow(); } } c) Code main #include "main.h" //ham delay 1 cach tuong doi uint32_t tick; #define Inv_Sampling_Time #define Sampling_Time

100 10

int Err,preErr; float uint32_t uint32_t uint16_t

pwma,pwmb; period = 255; speed = 50; min_speed = 30;

60

//float kp = 5.52 ,ki=0.02,kd=0.080; float kp = 5; float ki=0; float kd=0.35; float P=0,I=0,D=0; float x=0; extern uint32_t black_line[6]; void delay_us(uint32_t time){ tick = time*3.5; while(tick--){ } } void delay_ms(uint32_t time){ delay_us(1000*time); } void check_Err(void){ if ( GPIO_ReadInputDataBit(GPIOB,line1_pin_OUT) == 0 && GPIO_ReadInputDataBit(GPIOB,line2_pin_OUT) == 0 && GPIO_ReadInputDataBit(GPIOB,line3_pin_OUT) == 0 && GPIO_ReadInputDataBit(GPIOB,line4_pin_OUT) == 0 && GPIO_ReadInputDataBit(GPIOB,line5_pin_OUT) == 0 && GPIO_ReadInputDataBit(GPIOB,line6_pin_OUT) == 0){ if (preErr=0 && preErr != 255) Err = 6; } if ( GPIO_ReadInputDataBit(GPIOB,line4_pin_OUT) == 1 && GPIO_ReadInputDataBit(GPIOB,line3_pin_OUT) ==0 && GPIO_ReadInputDataBit(GPIOB,line5_pin_OUT) ==0 ) Err = 1; if ( GPIO_ReadInputDataBit(GPIOB,line4_pin_OUT) == 1 && GPIO_ReadInputDataBit(GPIOB,line5_pin_OUT) ==1) Err = 2; if ( GPIO_ReadInputDataBit(GPIOB,line5_pin_OUT) == 1 && GPIO_ReadInputDataBit(GPIOB,line6_pin_OUT) ==0 && GPIO_ReadInputDataBit(GPIOB,line4_pin_OUT) ==0 ) Err = 4; if ( GPIO_ReadInputDataBit(GPIOB,line5_pin_OUT) == 1 && GPIO_ReadInputDataBit(GPIOB,line6_pin_OUT) ==1 ) Err = 6; if ( GPIO_ReadInputDataBit(GPIOB,line6_pin_OUT) == 1 && GPIO_ReadInputDataBit(GPIOB,line5_pin_OUT) ==0 ) Err = 8; if ( GPIO_ReadInputDataBit(GPIOB,line3_pin_OUT) == 1 && GPIO_ReadInputDataBit(GPIOB,line4_pin_OUT) ==1 ) Err = 0; if ( GPIO_ReadInputDataBit(GPIOB,line3_pin_OUT) == 1 && GPIO_ReadInputDataBit(GPIOB,line4_pin_OUT) ==0 && GPIO_ReadInputDataBit(GPIOB,line2_pin_OUT) ==0 ) Err = -1; if ( GPIO_ReadInputDataBit(GPIOB,line3_pin_OUT) == 1 && GPIO_ReadInputDataBit(GPIOB,line2_pin_OUT) ==1) 62

Err = -2; if ( GPIO_ReadInputDataBit(GPIOB,line2_pin_OUT) == 1 && GPIO_ReadInputDataBit(GPIOB,line3_pin_OUT) ==0 && GPIO_ReadInputDataBit(GPIOB,line1_pin_OUT) ==0 ) Err = -4; if ( GPIO_ReadInputDataBit(GPIOB,line2_pin_OUT) == 1 && GPIO_ReadInputDataBit(GPIOB,line1_pin_OUT) ==1 ) Err = -6; if ( GPIO_ReadInputDataBit(GPIOB,line1_pin_OUT) == 1 && GPIO_ReadInputDataBit(GPIOB,line2_pin_OUT) ==0 ) Err = -8;

if ( GPIO_ReadInputDataBit(GPIOB,line1_pin_OUT) == 1 && GPIO_ReadInputDataBit(GPIOB,line2_pin_OUT) ==1 && GPIO_ReadInputDataBit(GPIOB,line3_pin_OUT) ==1 && GPIO_ReadInputDataBit(GPIOB,line4_pin_OUT) ==1 && GPIO_ReadInputDataBit(GPIOB,line5_pin_OUT) ==1 && GPIO_ReadInputDataBit(GPIOB,line6_pin_OUT) ==1) { Err = 255; } } void line_follow(void){

if (Err == 255){ TIM4 -> CCR2 =0; TIM4 -> CCR3 =0; 63

} if (Err != 255){ P=kp*Err; D=kd*(Err-preErr)*Inv_Sampling_Time; I+=ki*Sampling_Time*(Err+preErr)/2000; x=(P+I+D); if(x>speed ) x=speed; pwmb = speed - x; if (pwmb >255) pwmb =255; if (pwmb 255) pwma =255; if (pwma