<span class="text_page_counter">Trang 1</span><div class="page_container" data-page="1">

<b>ĐẠI HỌC QUỐC GIA HÀ NỘITRƯỜNG ĐẠI HỌC CÔNG NGHỆ</b>

<b>BÁO CÁO BÀI TẬP LỚN</b>

<b>MÔN: ROBOT TRONG CƠNG NGHIỆP</b>

<b>Điều khiển hệ thống cánh tay robot thơng minh dựa trên thuật toán PID</b>

Giảng viên hướng dẫn: TS. Đinh Trần HiệpMã lớp học phần: EMA3083_20

Nhóm sinh viên: Phan Trung KiênĐặng Minh Lân Đỗ Thị Loan

21021330 2102133221021334

</div><span class="text_page_counter">Trang 2</span><div class="page_container" data-page="2">

Chương 1: Giới thiệu hệ thống Chương 2: Nguyên lí hoạt độngChương 3: Kết quả

</div><span class="text_page_counter">Trang 3</span><div class="page_container" data-page="3">

<b>Chương 1: Giới thiệu hệ thống</b>

<b>1.1.Giới thiệu đề tài: </b>

Trong mơn học này, nhóm đã lựa chọn đề tài “Điều khiển hệ thống cánh tayrobot thông minh dựa trên thuật tốn PID”. Đây là một ví dụ điển hình về ứng dụngcác nguyên tắc cơ điện tử trong thực tế. Cụ thể về đề tài này, nhóm sẽ tìm hiểu về việcsử dụng PID hay còn biết đến là Proportional-Integral- Derivative (Tỉ lệ - Tích phân -Đạo hàm) để tính tốn và điều khiển góc quay của cánh tay robot dựa trên dữ liệu từcảm biến MPU6050. MPU6050 cung cấp dữ liệu về sự chuyển động và hướng của đốitượng, rất phù hợp cho việc sử dụng cánh tay robot; thuật toán PID là một phươngpháp điều khiển phản hồi phổ biến và có hiệu quả cao trong các hệ thống tự động.Ngoài ra, việc điều khiển cánh tay robot cũng được điều khiển dựa trên cơ sở lý thuyếtbài tốn động học ngược.

Mục tiêu của đề tài: Tìm hiểu và áp dụng PID để điều khiển góc quay của cánhtay robot dựa trên dữ liệu từ cảm biến kết hợp với bài toán động học ngược để xâydựng một hệ thống điều khiển linh hoạt và chính xác để cánh tay robot có thể dichuyển một cách tốt nhất. Ngồi ra tìm hiểu về các bậc tự do của cánh tay nhằm đưacánh tay đến 1 vị trí mong muốn. Thông qua việc kết hợp sử dụng phương pháp điềukhiển PID và bài toán động học ngược, đề tài này hứa hẹn mang lại những hiểu biết vềviệc tính tốn và điều khiển góc quay của cánh tay robot hay thậm chí là cả một conrobot trong thực tế.

<b>1.2 Mơ tả hệ thống:1.2.1 Khâu cơ khí:</b>

Ở trong báo cáo này sử dụng cánh tay robot được làm từ mica qua quá trình cắtlaser và được lắp ghép bởi ốc vít 3M. Ta thu được 1 robot với kích thước 14x9x18 cm.Độ dài trục 1 và trục 2 lần lượt là 85mm và 135mm. Robot gồm 4 động cơ, tuy nhiêncó 1 động cơ đề điều chỉnh độ rộng khớp gắp nên cánh tay robot có 3 bậc tự do. Mỗiđộng cơ sẽ tương ứng một vị trí khớp để điều khiển các trục xoay. Mỗi khớp điềukhiển một trục xoay cho phép cánh tay robot xoay quanh trục chính. Trên thực tế cónhiều cơ cấu truyền động khác nhau từ động cơ đến trục xoay. Ở trong báo cáo này,nhóm đã sử dụng cánh tay robot với các cách truyền động khác nhau. Ở khớp đầu tiênchính là trục chính, động cơ sẽ được gắn trực tiếp vào đế của robot. Ở khớp thứ 2 sửdụng bánh răng truyền chuyển động từ động cơ đến trục xoay để điều khiển trục xoay1. Ở khớp thứ 3 cần có 1 thanh truyền động từ động cơ đến trục xoay 2. Động cơ thứ 4truyền chuyển động cho độ rộng của khớp gắp qua bánh răng.

</div><span class="text_page_counter">Trang 4</span><div class="page_container" data-page="4">

Hình 2.1 Hình ảnh cánh tay robot

<b>1.2.2. Khâu cảm biến</b>

Trong hệ thống sử dụng cảm biến MPU6050 để thực hiện phản hồi góc đo chohệ thống, cập nhật đầu vào cho thuật toán PID. Cảm biến gia tốc MPU6050 là mộtmodule cảm biến tích hợp, kết hợp cảm biến gia tốc kế và con quay hồi chuyển(gyroscope). Nó được sử dụng để đo và theo dõi chuyển động, gia tốc và góc quay củamột đối tượng.

Hình 2.2 : Hình ảnh module cảm biến MPU6050

Cảm biến góc quay MPU6050: là một module cảm biến đa chức năng thườngđược sử dụng trong các ứng dụng điều khiển và định vị trong robot và các thiết bị diđộng khác. Nó cịn là một cảm biến linh hoạt và mạnh mẽ, cung cấp thông tin cần thiếtđể điều khiển và theo dõi chuyển động của các thiết bị điện tử một cách chính xác vàhiệu quả. MPU6050 kết hợp một bộ gia tốc kế và một bộ góc quay trong cùng mộtchip. Bộ gia tốc kế đo gia tốc tuyến tính của thiết bị theo các trục XYZ. Bộ góc quayđo tốc độ góc của thiết bị xung quanh các trục XYZ. Từ đó nó có thể tính được gócxoay theo 3 trục. MPU6050 có độ chính xác khá cao, giúp xác định vị trí và hướngchuyển động của thiết bị một cách chính xác. Cảm biến này thường có độ nhạy và độphân giải cấp cao, giúp nhận biết cảnh báo nhỏ nhất trong chuyển động. MPU6050

</div><span class="text_page_counter">Trang 5</span><div class="page_container" data-page="5">

thường được giao tiếp thơng qua giao diện I2C (Inter-Integrated Circuit), cho phép nókết nối với các vi điều khiển và các thiết bị khác dễ dàng. Module cũng cung cấp cácchân nối điện thông dụng cho dễ dàng kết nối với các mạch khác. Trong robot, nó cóthể được sử dụng để đo góc quay của các cánh tay hoặc chân robot, giúp điều khiểnchính xác hành động của chúng.

Ở trong báo cáo tập trung sử dụng khả năng đo góc xoay quanh trục Z của cảmbiến nhằm xác định góc xoay của cánh tay robot. Từ những giá trị của cảm biến để làmđầu vào cho thuật toán PID. Sử dụng PID để điều khiển động cơ quay đến một vị trísetpoint cho trước.

<b>1.2.3. Bài tốn động học ngược:</b>

Ban đầu, cho trước vị trí và hướng của khâu thao tác cuối, thường là một bộ giátrị của 6 thông số gồm 3 thông số định vị và 3 thông số định hướng. Nếu các hệ tọa độdùng để định vị và định hướng khâu cuối được lập theo phương pháp DH, thì từ bộ 2giá trị trên, ta sẽ xác định được giá trị của tất cả các phần tử trong ma trận thuần nhấtbiến đổi từ hệ tọa độ gốc sang hệ tọa độ cuối.

Hình 2.3 Mơ phỏng cánh tay robot

Đối với hệ tọa độ khâu cố định O<small>0</small>X<small>0</small>Y<small>0</small>Z<small>0</small>, trục <i>X</i><small>0</small> có thể được chọn bất kỳnhưng thường là phù hợp với vị trí và hình dáng của khâu cố định (khâu 0). Đối với hệtọa độ khâu thao tác cuối <i>OnXnYnZn </i>, trục <i>Zn</i> thường được chọn tùy thuộc vào thiết bịcụ thể sẽ lắp lên đầu công tác của Robot, gốc hệ tọa độ khâu thao tác cuối <i>On</i> thườngđược đặt tại điểm tương tác với môi trường và được gọi là điểm tác động cuối.

Cụ thể ở trong hệ thống này nhóm đã chọn được góc tọa độ như hình trên với 3gốc tọa độ bao gồm : gốc tọa độ tham chiếu O<small>0</small>X<small>0</small>Y<small>0</small>Z<small>0</small>, hệ tọa độ ứng với khớp 2O<small>1</small>X<small>1</small>Y<small>1</small>Z<small>1 </small>và hệ trục tọa độ tương ứng với khớp 3 là O<small>2</small>X<small>2</small>Y<small>2</small>Z<small>2. </small>Ngồi ra có l<small>1</small>,l<small>2 </small>tươngứng với chiều dài trục 1,và trục 2, l1 = 85mm, l2 = 135 mm.

Sau khi đã thiết lập được hệ trục tọa độ, nhiệm vụ tiếp theo là xác định thông sốcủa phép biến đổi giữa các hệ trục tọa độ trung gian, hay còn gọi là bộ thơng số

Denavit-Hatenberg (bộ thơng số D-H).Trong đó các hệ số :

<i>θii là góc giữa trục Xi−1 và trục Xi ;</i>

<i>di là khoảng cách giữa trục Xi−1 và trục Xi ;ai là khoảng cách giữa trục Zi−1 và trục Zi ;αii là góc giữa trục Zi−1 và trục Zi .</i>

</div><span class="text_page_counter">Trang 6</span><div class="page_container" data-page="6">

Từ các thơng số đề bài ta có bảng D-H cho hệ thống sau:

Từ các phép biến đổi cơ bản nêu trên, ta tính được ma trận thuần nhất biến đổi từhệ tọa độ O<small>i-1</small>X<small>i-1</small>Y<small>i-1</small>Z<small>i-1</small> sang hệ O<small>i</small>X<small>i</small>Y<small>i</small>Z<small>i</small> tọa độ Ma trận này bằng tích các ma trậnthuần nhất của các phép biến đổi cơ bản theo đúng thứ tự:

</div><span class="text_page_counter">Trang 7</span><div class="page_container" data-page="7">

Ma trận D-H cho hệ 1 → 2

Ta có ma trận thuần nhất khi chuyển hệ trục tọa độ từ 0 →2 = *

Các hệ số của ma trận thuần nhất trên lần lượt là:

Trong đó với hệ thống này nhóm cần xác định tọa độ của tay gắp trong mặt phẳng hệ tọa độ OXY nên cần quan tâm đến 2 giá trị Px và Py. Trong đó Px, Py lần lượt là tọa độ của tay gắp trong mặp phẳng OXY. Đồng nhất hệ số ta được:

<i>P_x</i>=<i>L</i>₁<i>cos ⁡(θ</i>¿¿<i>1)+L</i>₂<i>cos ⁡(θ</i>₁+<i>θ</i>₂)¿

<i>P_y</i>=<i>L</i>₁<i>sin(θ</i>¿ ¿<i>1)+L</i>₂<i>sin ⁡(θ</i>₁+<i>θ</i>₂)¿ <i>p_x</i><small>2</small>

+<i>2 L</i>₁<i>L</i>₂<i>cos θ</i><small>2 </small>

<i>cos ⁡(θ</i>¿¿2)=<i>^p^x</i><small>2</small>

<i>2 L</i>₁<i>L</i>₂ ^¿<i>θ</i>₂=cos⁻¹ <i>^p^x</i>

<i>2 L</i>₁<i>L</i>₂

<i>L</i>₂<i>sin θ</i>₂<i>L</i>_{1 +}_¿<i>_L</i>₂<i>_{cos θ}</i>₂¿

Ở trong hệ thống này nhóm đã thực nghiệm với một số tọa độ của tay gắp khác nhau, cụ thể nhóm đã tiến hành đặt vị trí tay gắp ở tọa độ XY (120,0). Từ đó áp dụng cơng thức ta thu được.

</div><span class="text_page_counter">Trang 8</span><div class="page_container" data-page="8">

2∗85∗135 <i>^θ</i><small>1</small>=<i>atan2</i> ⁰

12 0⁺<i>^atan2</i>

<i>135 sinθ</i>₂<i>85+135∗cos θ</i>₂

Từ đó nhóm đã tính được <i>θ</i><small>1 = 80,45 và </small><i>θ</i>₂=¿118,7.

Trong q trình lắp ráp và thử nhóm nhận thấy động cơ khớp 2 theo phương nằmngang đang là vị trí góc 135º, động cơ khớp 3 theo phương ngang đang là góc 215º. Từhai góc tính được của thuật tốn tính tốn được sẽ chuyển đổi sang vị trí góc của haiđộng cơ. Cụ thể vị trí góc của động cơ 1 sẽ là 135-<i>θ</i><small>1 Tuy nhiên do trục 2 không được</small>điều khiển trực tiếp bới khớp 2 mà được điều khiển qua 1 thanh truyền động. Có thanhnày ln song song với trục 2 nên từ đó ta tính được vị trí góc của động cơ khớp 3 là215-(<i>θ</i><small>2-</small><i>θ</i><small>1). Ngoài ra cũng biết rằng chiều dài trục 1 và trục 2 lần lượt là </small><i>l</i><small>1= 85mm và</small>

Hình 3.1 : Mô phỏng cánh tay robot

Hàm atan2() là một hàm lượng giác ngược, ban đầu được xây dựng trong ngơnngữ lập trình Fortran, sau đó được đưa vào nhiều ngơn ngữ lập trình khác và dần trởnên phổ biến trong khoa học và kỹ thuật. Khác với hàm arctan() (hay còn được viết làatan()) chỉ nhận 1 tham số và có miền nghiệm chỉ là nửa vịng trịn lượng giác, hàmatan2() nhận 2 tham số và có miền nghiệm là tồn bộ vịng trịn lượng giác.

Ý nghĩa hình học của hàm atan2() được thể hiện trong Hình 3.23. Giả sử 2 thamsố của hàm lần lượt là y và x, hàm atan2(y,x) sẽ trả về giá trị θ là góc tạo bởi đường

</div><span class="text_page_counter">Trang 9</span><div class="page_container" data-page="9">

nối gốc tọa độ O(0,0) với điểm có tọa độ (x,y) trên vịng trịn lượng giác (gốc tính từtrục Ox, chiều dương ngược chiều kim đồng hồ).

</div><span class="text_page_counter">Trang 10</span><div class="page_container" data-page="10">

<b>Chương 2: Thiết kế hệ thống</b>

<b>2.1. Sơ đồ khối và nguyên lí hoạt động</b>

Hình 3.1. Sơ đồ khối của hệ thống

Sơ đồ khối hệ thống gồm có 3 khối chính là khối điều khiển, bộ xử lí trung tâm(MCU), thiết bị ngoại vi là máy tính. Ở khối điều khiển ở đây gồm khối điều khiểntrục chính sử dụng thuật tốn PID và khối điều khiển tay gắp đến vị trí mong muốn sửdụng bài toán động học ngược. Các khối này được điều khiển với bộ xử lí trung tâm làArduino Uno R3 để trả về các kết quả mong muốn từ người dùng. Đầu tiên ở khối điềukhiển trục chính sử dụng thuật tốn PID có 2 khâu là khâu cảm biến góc để đọc đượcgiá trị vị trí góc mà trục chính đang đứng. Sau khi có kết quả trả về, khâu trục chínhđược sử dụng động cơ DC kết hợp với thuật toán điều khiển PID để điều khiển độngcơ đến vị trí góc mong muốn. Khối điều khiển thứ hai sử dụng bài toán động họcngược để điều khiển các khâu trục khớp và trục gắp của cánh tay ta sử dụng bài toánđộng học ngược để tính ra các vị trí góc mà cánh tay muốn hướng đến đối tượng. Kếtquả các vị trí các góc đo được sẽ được hiển thị trên cửa sổ Serial Monitor trên phầnmềm Arduino.

<b>2.2. Đầu vào, đầu ra và mối liên hệ giữa đầu vào và đầu ra của hệ thống:</b>

Trong đề tài này nhóm đã đặt ra một yêu cầu cho hệ thống thực hiện gắp vật ởmột vị trí biết trước và di chuyển nó đến một nơi xác định trong tầm kiểm soát. Bàitoán được đặt ra bao gồm 2 phần. Phần 1 điều khiển động cơ khớp 2 và khớp 3 để đưatay gắp đến vị trí mong muốn, phần 2 điều khiển động cơ khớp 1 xoay đến vị trí mongmuốn.

Đầu vào là tọa độ X, Y của đầu gắp trên cách tay robot. Và đầu ra là góc quaycủa 2 động cơ so với vị trí tham chiếu. Để làm được điều này một thuật tốn tính tốn

</div><span class="text_page_counter">Trang 11</span><div class="page_container" data-page="11">

chuyển đổi giữa vị trí và góc xoay của động cơ được thiết lập. Thuật toán này đượcxây dựng dựa trên bài toán động học ngược. Tiếp đến là thuật toán để điều khiển độngcơ quay đến những góc theo kết quả từ thuật tốn tính tốn ở trên. Bởi hai động cơservo SG90 quy định sẵn vị trí góc của nó. Trong q trình lắp ráp và thử nhóm nhậnthấy động cơ khớp 2 theo phương nằm ngang đang là vị trí góc 135º, động cơ khớp 3theo phương ngang đang là góc 215º. Từ hai góc tính được của thuật tốn tính tốnđược sẽ chuyển đổi sang vị trí góc của hai động cơ. Cụ thể vị trí góc của động cơ 1 sẽlà 135-<i>θ</i><small>1 và vị trí góc động cơ 2 là 215-(</small><i>θ</i><small>2-</small><i>θ</i><small>1).</small>

Hình 3.2. Mơ phỏng tính tốn

Ở phần tiếp theo, động cơ xoay đến 1 góc với số đo biết trước nên đầu vào sẽ làgóc setpoint, cụ thể trong báo cáo này nhóm đã thực hiện các thực nghiệm với gócsetpoint là 90º. Đầu ra sẽ là một giá trị output để điều khiển băm xung cho động cơ. Ởđây nhóm đã xây dựng thuật tốn dựa trên thuật tốn điều khiển PID. Một cảm biếngóc quay MPU6050 đã được sử dụng để làm đầu vào cho thuật toán. Các hệ số <i>K_p</i>, <i>K_i</i>,

<i>K_d</i> được lựa chọn để thuật tốn hoạt động ổn định và chính xác nhất có thể. Ngồi rathời gian phản hồi của cảm biến cũng cần được quan tâm, nó chính là thời gian lặp của1 vịng PID. Ở đây nhóm đã để thời gian phản hồi là 100 milis. Các hệ số này vơ cùngquan trọng, nó quyết định tính chất hoạt động của hệ thống. Sau đó đầu ra của thuậttoán PID sẽ được đi qua module băm xung L298N để cấp nguồn cho động cơ quay đếnvị trí mong muốn. Nguồn này chính là điện áp có mức dao động từ 5V đến 12V (tínhchất của động cơ - cơ cấu chấp hành). Tốc độ quay của động cơ phụ thuộc và điện ápnguồn đầu vào.

Nói tóm lại, hệ thống được điều khiển với các chức năng lần lượt như sau, Đầutiên cảm biến được kích hoạt, và đợi 3s để cảm biến góc xoay có thể ổn định, sau đó 2động cơ ở khớp 2 và khớp 3 sẽ hoạt động để điều khiển vị trí của đầu gắp cánh tayrobot đến vị trí mong muốn. Tại đây cần delay chương trình khoảng 3s để 2 động cơ 1và động cơ 2 thực hiện xong nhiệm vụ. Sau đó động cơ ở khớp số 1 sẽ quay và điềuchỉnh dần dần đến vị trí mong muốn qua thuật tốn PID. Nhóm cũng hướng tới sau đóđộng cơ ở khớp 2 và khớp 3 hoạt động để đưa đầu gắp của cánh tay đến 1 tọa độ mới.

</div><span class="text_page_counter">Trang 12</span><div class="page_container" data-page="12">

<b>2.3 Bộ xử lý trung tâm</b>

Arduino Uno R3 là một bảng mạch vi điều khiển nguồn mở dựa trên vi điềukhiển Microchip ATmega328 được phát triển bởi Arduino.cc. Bảng mạch được trangbị các bộ chân đầu vào/ đầu ra Digital và Analog có thể giao tiếp với các bảng mạchmở rộng khác nhau.

Hình 3.3 Vi điều khiển trung tâm của hệ thống.

Arduino Uno R3 là bo mạch vi mạch mở và linh hoạt, được thiết kế để dễ dàngsử dụng cho người mới bắt đầu trong lĩnh vực điện tử và lập trình. Arduino Uno R3 cóhình dạng nhỏ gọn, chứa một vi mạch chính với bộ điều khiển ATmega328P củaMicrochip. Cụ thể trong hệ thống này, nhóm sử dụng vi điều khiển này để thực hiệnđiều khiển tồn bộ chương trình và tính tốn cũng như lưu trữ dữ liệu của hệ thống.

Ngồi ra vi mạch này có tốc độ xử lý 16MHz, bộ nhớ flash 32KB và RAM2KB. Với tần số dao động này nhằm đồng bộ thời gian trên vi điều khiển và thời gianthực tế. Ở trong hệ thống này có sử dụng đến thời gian thực với 100milis. Đây chính làthời gian phản hồi của cảm biến hay nó cũng chính là 1 vịng lặp cho thuật tốn PID.Tùy vào từng hệ thống và cấu tạo phần cơ khí mà hệ thống cần có những thời gianphản hồi phù hợp.

Ngồi ra nó có thể được lập trình trực tiếp từ máy tính thơng qua dây nạp codecổng USB. Khơng chỉ nạp code nó cịn là giao tiếp giữa máy tính và vi điều khiển giúpthu thập dữ liệu từ vi điều khiển và hiện lên màn hình. Nó giúp người dùng có thể theodõi được các giá trị cảm biến góc MPU6050 trả về cũng như theo dõi các bước thựchiện của hệ thống xem hệ thống có hoạt động ổn định khơng.

Arduino Uno R3 có một loạt cổng kết nối và chân chức năng, cổng nguồn DC,cổng vào/ra digital và cổng analog. Các chân vào/ra digital có thể được sử dụng để kếtnối với các cảm biến và thiết bị điều khiển khác nhau, trong khi các cổng analog có thểsử dụng cho các cảm biến tín hiệu analog. Ở đây nhóm đã sử dụng chân này để cấpnguồn cũng như giao tiếp với cảm biến MPU6050 qua giao tiếp I2C và module điềukhiển động cơ qua giao tiếp SPI.

<b>2.4 Cơ cấu chấp hành</b>

</div><span class="text_page_counter">Trang 13</span><div class="page_container" data-page="13">

Động cơ Servo SG90 được sử dụng để điều khiển 2 bậc liên quan đến bán kínhvà góc quay trong bài tốn động học ngược. Động cơ Servo SG90 có kích thước nhỏ,là loại được sử dụng nhiều nhất để làm các mơ hình nhỏ hoặc các cơ cấu kéo khơngcần đến lực nặng. Động cơ servo SG90 180º có tốc độ phản ứng nhanh, các bánh răngđược làm bằng nhựa nên cần lưu ý khi nâng tải nặng vì có thể làm hư bánh răng, độngcơ RC Servo MG90 có tích hợp sẵn Driver điều khiển động cơ bên trong nên có thể dễdàng điều khiển góc quay bằng phương pháp điều độ rộng xung PWM. Động cơ servođược thiết kế những hệ thống hồi tiếp vịng kín. Tín hiệu ra của động cơ được nối vớimột mạch điều khiển. Khi động cơ quay, vận tốc và vị trí sẽ được hồi tiếp về mạchđiều khiển này. Nếu có bất kỳ lý do nào ngăn cản chuyển động quay của động cơ, cơcấu hồi tiếp sẽ nhận thấy tín hiệu ra chưa đạt được vị trí mong muốn. Mạch điều khiểntiếp tục chỉnh sai lệch cho động cơ đạt được điểm chính xác. Các động cơ servo điềukhiển bằng liên lạc vô tuyến được gọi là động cơ servo RC (radio-controlled). Trongthực tế, bản thân động cơ servo không phải được điều khiển bằng vơ tuyến, nó chỉ nốivới máy thu vô tuyến trên máy bay hay xe hơi. Động cơ servo nhận tín hiệu từ máy thunày.

Hình 3.4 Hình ảnh các động cơ được sử dụng trong hệ thống.

Trục chính sử dụng động cơ DC Giảm Tốc JGB37-520 DC Geared Motor. Độngcơ này được điều khiển bằng thuật toán PID và mạch cầu H L298N. Động cơ đượcthiết kế với đường kính trục là 6mm, nó phù hợp để gắn vào đế động cơ. Lực kéomoment tối đa là 3.1 Kg.cm hoàn toàn đủ lực để xoay cánh tay robot. Tông chiều dàicủa động cơ là 5cm. Với chiều cao này nhóm đã thiết kế sử dụng cọc đồng 5cm để làmphần giá đỡ cho robot.

<b>2.5. Thuật toán điều khiển của hệ thống:</b>

Ở khối điều khiển trục chính, hệ thống thực hiện thuật toán PID để đưa cánh tayrobot xoay đến vị trí mong muốn:

- Bước 1: Xác định góc cần đạt được (Setpoint), Thiết lập các biến lưu trữ giá trịgóc hiện tại và sai số, thiết lập các hệ số PID: <i>K_p</i>, <i>K_i</i>, <i>K_d</i>.

</div>