Đó chính là một nhu cầu cầnthiết và cấp bách đối với mỗi sinh viên, đề tài này được thực hiện chính là đápứng nhu cầu đó.Để góp phần đáp ứng nhu cầu trên và đóng góp thêm giải pháp thay
PHÂN TÍCH VÀ CHỌN PHƯƠNG ÁN THIẾT KẾ
Phân tích hệ thống đếm và phân loại sản phẩm
2.1.1 Vị trí công nghệ sản xuất
Quá trình đếm và phân loại tự động là một quá trình lao động kỹ thuật phức tạp Mức độ phức tạp khối lượng liên quan chặt chẽ đến quá trình phân loại vì định vị sản phẩm càng chính xác thì sản xuất chúng cũng sẽ nhanh, dễ dàng, ít sửa chữa…
Mục đích nghiên cứu thông qua đề tài là tìm hiểu về ứng dụng đếm và phân loại sản phẩm nên nhiệm vụ nghiên cứu gồm:
-Tìm hiểu cơ chế hoạt động.
-Phân tích sơ đồ nguyên lý.
-Nâng cao khả năng lập trình vi điều khiển.
-Phát triển khả năng tư duy cho sinh viên trong quá trình nghiên cứu.
Chọn phương án thiết kế
2.2.1 Các yêu cầu khi thiết kế hệ thống
Với mục tiêu là mô hình thực hành phục vụ đào tạo nên không thể đáp ứng được đầy đủ yêu cầu trong thực tế cũng như các điều kiện phân loại phức tạp. Tuy nhiên, mô hình thiết kế phải đảm bảo một số yêu cầu kỹ thuật chung như sau:
-Mô hình cơ bản phải phù hợp với nguyên lý phân loại trong thực tế.
-Lắp ráp, đấu nối và vận hành điều khiển dễ dàng.
-Sử dụng các vật tư, thiết kế, linh kiện thông dụng để dễ dàng thay thế sữa chữa.
-Đảm bảo tính thẩm mỹ và gọn gàng Các cơ cấu truyền động, kết nối phải đảm bảo cứng vững và tuổi thọ cao.
2.2.2 Chọn phương pháp thiết kế
Trên yêu cầu kỹ thuật đã phân tích ở trên chúng em lựa chọn phương án thiết kế của mô hình đếm và phân loại sản phẩm theo màu sắc như sau:
Cảm biến màu sắc Cánh tay robot Động cơ Servo
Cảm biến hồng ngoại Động cơ DC
Hình 2.1: Sơ đồ nguyên lý hoạt động của mô hình
Khi khởi động hệ thống băng tải hoạt động, sản phẩm cần phân loại được đưa vào băng tải Sản phẩm sẽ đi qua cảm biến màu sắc để nhận biết các màu sắc. Cảm biến hồng ngoại sẽ phát hiện sản phẩm để đếm và hiện lên màn hình LCD số lượng sản phẩm mỗi màu và tổng số lượng sản phẩm Sau khi qua cảm biến sản phẩm sẽ được động cơ servo quay đến hộp đựng sản phẩm của mỗi màu Khi hộp đựng đủ 3 sản phẩm sẽ được cánh tay robot gắp đến vị trí đã định trước đó và thay hộp mới vào vị trí cũ.
Sơ đồ khối và nguyên lý hoạt động
KHỐI XỬ LÝ ĐỘNG CƠ DC
2.3.2 Nguyên lý hoạt động các khối
-Khối nguồn: cung cấp nguồn cho toàn bộ hoạt động của mạch.
-Khối động cơ DC: dùng để điều khiển băng chuyền hoạt động.
-Khối xử lý: nhận tín hiệu từ khối nhận diện màu sắc và khối phát hiện sản phẩm, tiến hành xử lý và xuất tín hiệu ra khối hiển thị và khối phân loại sản phẩm.
-Khối nhận diện màu sắc: gửi tín hiệu cho khối xử lý và xuất ra khối phân loại sản phẩm.
-Khối phát hiện sản phẩm: gửi tín hiệu cho khối xử lý và xuất ra khối hiển thị. -Khối hiển thị: nhận tín hiệu từ khối xử lý và hiện lên màn hình.
-Khối phân loại sản phẩm: nhận tín hiệu từ khối xử lý và phân loại sản phẩm.-Khối vận chuyển: nhận tín từ khối xử lý khi hộp chứa sản phẩm đầy và vận chuyển đi, rồi thay hộp mới vào.
Sơ đồ mạch
TÍNH TOÁN THIẾT KẾ HỆ THỐNG
Tính toán thiết kế
3.1.1 Sản phẩm được phân loại
Hình 3.1: Sản phẩm phân loại
Thông số Đơn vị đo Đường kính 48mm
Bảng 3.1: Thông số sản phẩm
Hình 3.2: Hộp đựng sản phẩm
Thông số Kích thước Đường kính ngoài 60 mm Đường kính trong 58 mm
Chiều cao 50 mm Động sâu 48 mm
Bảng 3.2: Thông số hộp đựng sản phẩm
3.1.3.1 Phân loại lựa chọn băng tải
- Băng tải có nhiều loại mỗi loại dùng để tải những vật liệu khác nhau.Tùy vào mục đích sử dụng và vật cần tải mà ta chọn băng tải cho phù hợp.
Phạm vi ứng dụng Vận chuyển từng chi tiết giữa các nguyên công hoặc vận chuyển thùng chứa trong gia công cơ và lắp ráp.
Vận chuyển chi tiết trên vệ tinh trong gia công chuẩn bị phôi và trong lắp ráp.
Vận chuyển các chi tiết lớn giữa các bộ phận trên khoảng cách > 50 Kg
Vận chuyển chi tiết trên các vệ tinh giữa các nguyên công với khoảng cách < 50 Kg
* Lựa chọn băng tải: Đối với sản phẩm là chất lỏng ta dùng băng tải kênh. Đối với sản phẩm rời rạc thì ta dùng băng tải con lăn hoặc băng tải đai con lăn hoặc băng tải dây đai.
* Hệ thống băng tải được sử dụng nhiều trong các nhà máy sản xuất - chế biến – lắp ráp linh kiện các khu công nghiệp mỏ khai khoáng nhằm đáp ứng nhu cầu sản xuất công nghiệp cũng như vận chuyển vật liệu nặng mà sức người không thể đáp ứng kịp thời Góp phần nâng cao năng lực cạnh tranh của doanh nghiệp nhằm giải quyết được bài toán giảm chi phí nhân công mà vẫn gia tăng được số lượng thành phần cao nhất đồng thời đảm bảo được số lượng thành phẩm cao nhất đồng thời đảm bảo được chất lượng và thời gian giao hàng của từng đơn hàng.
– Được cấu tạo đơn giản, linh hoạt dễ dàng sử dụng mang lại hiệu quả cao trong quá trình sản xuất.
– Năng suất vận hành cao, không gây tiếng ồn khi làm việc.
– Số lượng vận chuyển lớn.
– Không tiêu hao nhiều điện năng trong quá trình vận hành.
– Dễ dàng sửa chữa, bảo trì khi gặp sự cố.
– Vốn đầu tư ban đầu lớn.
– Một số hệ thống khó di chuyển, không vận chuyển được các sản phẩm quá kích cỡ.
– Đối với các sản phẩm hạt, vụn, có thể bị hao hụt, rơi vật liệu trong quá trình vận chuyển.
– Khi vận chuyển xa và địa hình không thẳng đòi hỏi phải có nhiều hệ thống kết hợp lại với nhau.
3.1.3.2 Cấu tạo, nguyên lí hoạt động của băng tải:
- Băng tải 1 là băng chuyền dùng để vận chuyển chitiếtlắp1đến đường dẫn phôi, sau đó tay đẩy sẽ đẩy chi tiết đến bàn lắp ráp sản phẩm
- Sơ đồ nguyên lý của băng tải 1:
Hình 3.4: Sơ đồ nguyên lý băng tải
- Hệ truyền động hệ thống băng tải gồm có: Động cơ
3.1.3.3 Tính toán động học cho băng tải
Chọn đường kính các bánh dẫn và bị dẫn: D = D1 = D2 = 60mm
Chọn khoảng cách trục: a T0mm
Bề rộng của đai B = 80mm
Khối lượng của chi tiết có trên băng tải 1 là m= 10g
Tổng khối lượng của chi tiết có trên băng tải là M0g
Trọng lượng của các vật P = Mg = 30*9.81)4 g
Ta có Fk > Fms =(∑m + mb )*g *f
Trong đó: f là hệ số ma sát trượt trên tấm đỡ f= 0.15
Tính gần đúng ta có: Fk*(∑m + mb)*f*(0.3+0.1)*0.15=0.9[N]
*Số vòng quay trục tang băng tải:
*Tính tỉ số truyền của động cơ
= = n = =6.22 i 3.1.3.4 Tính toán công suất chọn động cơ cho băng tải
Với đặc điểm yêu cầu của của băng chuyền là tải trọng nhỏ, giá thành rẽ, dễ điều khiển ta chọn động cơ điện một chiều để dẫn động cho băng tải Còn đối với động cơ bước thì giá thành cao và momen xoắn nhỏ nên không phù hợp với yêu cầu, về động cơ servo thì có vẻ thích hợp để ứng dụng cho các cánh tay/chân robot hoặc ứng dụng để điều khiển các loại bánh lái…Động cơ được chọn yêu cầu phải có moment lớn do yêu cầu làm của băng tải có tải trọng Và băng tải chuyển động với vận tốc nhỏ nên ta chọn động cơ có tốc độ thấp nhưng phải đảm bảo yêu cầu về tốc độ cũng như tải Vì vậy chúng ta chọn loại động cơ
*Tính toán chọn động cơ
Dựa vào bảng ta chọn được hiệu suất của các loại bộ truyền và ổ như sau:
Tên gọi Hiệu suất η Được che kín Để hở
Bộ truyền bánh răng trụ
Bộ truyền bánh răng côn
Bộ truyền bánh ma sát
*Tính công suất kéo sản phẩm
*Tính công suất động cơ
*Ta chọn động cơ có công suất lớn hơn Nđc tính được
Với yêu cầu của đồ án, chúng em đã chọn cánh tay robot 4 bậc tự do Dùng động cơ servo làm cơ cấu vận hành
Hình 3.6: Cánh tay robot 4 bậc tự do
Thông số kỹ thuật của động cơ servo dùng trong cánh tay robot:
Servo MG996R (nâng cấp MG995) có momen xoắn lớn
Đây là bản nâng cấp từ servo MG995 về tốc độ, lực kéo và độ chính xác
Phù hợp với máy bay cánh quạt loại 50 -90 methanol và máy bay cánh cố định xăng 26cc-50cc
So với MG946R, MG996R nhanh hơn, nhưng hơi nhỏ hơn.
Kích thước sản phẩm: 40.7 * 19.7 * 42.9mm
Lực kéo: 9.4kg / cm (4.8V), 11kg / cm (6V)
Tốc độ xoay: 0.17 giây / 60 độ (4.8 v) 0.14 giây / 60 độ (6 v)
Vật liệu bánh răng: Kim loại
*Tính toán thông số động học:
Lực quay tối thiểu để robot hoạt động :
Fđ1 > Fms1 Fđ1 > m1.g.f = 0,55*9,81*0,25 = 1,39 N Momen xoắn :
Fđ2 > Fms2 Fđ1 > m2.g.f = 0,412*9,81*0,25 = 1 N Momen xoắn :
Fđ3 > Fms3 Fđ3 > m3.g.f = 0,189*9,81*0,25 = 0,46 N Momen xoắn :
Fđ4 > Fms4 Fđ4 > m4.g.f = 0,195*9,81*0,25 = 0,48 N Momen xoắn :
Arduino được khởi động vào năm 2005 như là một dự án dành cho sinh viên trại Interaction Design Institute Ivrea (Viện thiết kế tương tác Ivrea) tại Ivrea, Italy Cái tên "Arduino" đến từ một quán bar tại Ivrea, nơi một vài nhà sáng lập của dự án này thường xuyên gặp mặt.
Các thiết bị dựa trên nền tảng Arduino được lập trình bằng ngôn ngữ riêng Ngôn ngữ này dựa trên ngôn ngữ Wiring được viết cho phần cứng nói chung trên một môi trường phát triển tích hợp (IDE) chạy trên các máy tính cá nhân
Và Wiring lại là một biến thể của C/C++ Một số người gọi nó là Wiring, một số khác thì gọi là C hay C/C++.
Sau khi nền tảng Wiring hoàn thành, các nhà nghiên cứu đã làm việc với nhau để giúp nó nhẹ hơn, rẻ hơn, và khả dụng đối với cộng đồng mã nguồn mở một trong số các nhà nghiên cứu là David Cuarlielles, đã phổ biến ý tưởng này.
Những nhà thiết kế của Arduino cố gắng mang đến một phương thức dễ dàng, không tốn kém cho những người yêu thích, sinh viên và giới chuyên nghiệp để tạo ra những thiết bị có khả năng tương tác với môi trường thông qua các cảm biến và các cơ cấu chấp hành.
Thông tin thiết kế phần cứng được cung cấp công khai để những ai muốn tự làm một mạch Arduino bằng tay có thể tự mình thực hiện được (mã nguồn mở). Người ta ước tính khoảng giữa năm 2011 có trên 300 ngàn mạch Arduino chính thức đã được sản xuất thương mại, và vào năm 2013 có khoảng 700 ngàn mạch chính thức đã được đưa tới tay người dùng.
Phần cứng Arduino gốc được sản xuất bởi công ty Italy tên là Smart Projects Một vài board dẫn xuất từ Arduino cũng được thiết kế bởi công ty của Mỹ tên làSparkFun Electronics Nhiều phiên bản của Arduino cũng đã được sản xuất phù hợp cho nhiều mục đích sử dụng:
Hình 3.7: Những phiên bản Arduino 3.2.1.2 Sơ lược về Arduino Uno R3
Arduino Uno R3 là một bản mạch vi điều khiển nguồn mở dựa trên vi điều khiển Microchip Atmega328 Bảng mạch được trang bị 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ạch mở rộng khác nhau.Mạch Arduino Uno R3 thích hợp cho những bạn mới tiếp cận và đam mê về điện tử, lập trình Dựa trên nền tảng mở do Arduino.cc cung cấp, các bạn dễ dàng xây dựng cho mình một dự án nhanh nhất.
3.2.1.3 Một vài thông số của Arduino Uno R3
Vi điều khiển Atmega328 họ 8bit Điện áp hoạt động 5v DC
Tần số hoạt động 16MHz
Dòng tiêu thụ khoảng 30mA Điện áp vào khuyên dùng 7-12v DC Điện áp giới hạn vào 6-20v DC
Dòng tối đa trên mỗi chân I/O 30 mA
Bộ nhớ flash 32 KB (ATmega328) với 0.5KB dùng bởi bootloader SRAM 2 KB (ATmega328)
Bảng 3.1: Thông số của Arduino Uno R3
3.2.2.1 Sơ lược về động cơ servo Động cơ servo được thiết kế cho 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ới một mạch vi điều khiển Khi động cơ quay, vận tốc và vi 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 tín hiệu ra chưa đạt được vị trí mong muốn Mạch điều khiển tiếp tục chỉnh sai lệch cho động cơ đạt được điểm chính xác.
Momen xoắn tại nguồn 4.8v 1.8kg/cm
Momen xoắn tại nguồn 6v 2.2kg/cm Điện áp hoạt động 4.8-6v DC Độ dài dây nối 175mm
Bảng 3.2: Thông số kỹ thuật động cơ servo mg90s
Trục của động cơ servo được định vị nhờ vào kỹ thuật gọi là điều biến độ rộng xung (PWM) Trong hệ thống này, servo là đáp ứng của một dãy các xung số ổn định Cụ thể hơn, mạch điều khiển lad đáp ứng của một tín hiệu số có các xung biến đổi từ 1-2 ms Các xung này được gửi đi 50 lần/giây Chú ý rằng không phải số xung trong một giây điều khiển động cơ servo mà là chiều dài của các xung Servo đòi hỏi khoảng 30-60 xung/giây Nếu số này thấp, độ chính xác và công suất duy trì servo sẽ giảm.
Với độ dai xung 1 ms, servo được điều khiển quay theo một chiều (giả sử là chiều kim đồng hồ như (Hình 3.10)
Hình 3.10: Hoạt động của servo
Với độ dài xung 2 ms, servo quay theo chiều ngược lại Kỹ thuật này cong được gọi là tỉ lệ số-chuyển động của dộng cơ servo tỉ lệ với tín hiệu số điều khiển.
Các servo khác nhau ở góc quay được với cùng tín hiệu 1-2 ms (hoặc bất kỳ) được cung cấp Nếu ta cố điều khiển động cơ servo vợt quá những giới hạn cơ học của nó, hiện tượng này kéo dày hơn vài giây sẽ làm bánh răng của động cơ bị phá hủy.
3.2.3 Cảm biến màu sắc TCS3200
3.2.3.1 Sơ lược về cảm biến màu sắc TCS3200
Các phần mềm dùng để thiết kế
Arduino là môi trường phát trển tích hợp mã nguồn mở, cho phép người dùng dễ dàng viết code và tải nod lên board mạch, được viết bằng Java dựa trên ngôn ngữ lập trình và phần mềm mã nguồn khác.
Kể từ tháng 3 năm 2015,Arduino IDE ( Intergrated Devalopment editor – môi trường phát triển thích hợp) đã được phổ biến tai rất niều nơi với giao diện trực quan.
Ngôn ngữ phổ biến cho Arduino là C và C++ Do đó phần mềm phù hợp với những người dùng quen thuộc với những ngôn ngữ này.
Phần mềm gồm những mảng thư viện phong phú như: EEPROM, Firmata, GSM, Servo, TFT, Wifi… Và các mảnh thư viện ngày càng đa dạng nhờ sự đóng góp của cộng đồng Arduino trên toàn thế giới.
Proteus là phần mềm mô phỏng mạch điện tử của Labcenter Electronics, mô phỏng cho hầu hết các linh kiện điện tử thông dụng, đặc biệt hỗ trợ cho tất cả các MCU như PIC, 8051, AVR…
Phần mềm bao gồm 2 mảng chính là ISIS cho phép mô phỏng mạch điện tử vàARES dùng để vẽ mạch in.
Hình 3.28: giao diện chính của proteus
KẾT QUẢ ĐẠT ĐƯỢC
Mô hình đếm và phân loại sản phẩm theo màu
Hình 4.1: Mô hình băng tải
Mô tả: Hình trên là mô hình băng tải được điều khiển bởi một động cơ DC Băng tải có chức năng v loại theo màu sắc ến để đếm và phân
Mô tả: Hình trên là mạch kết nối các chân của Arduino Uno R3 với các chân cắm với các chức năng khác nhau như cảm biến màu sắc TCS3200, cảm biến hồng ngoại, LCD, Servo.
Hình 4.3: Mạch hiển thị LCD
Mô tả: Hình trên là mạch LCD đã được giao tiếp với module LCD I2C để giảm số đường điều khiển của MCU, nhờ đó chúng ta sẽ còn nhiều chân để phục vụ cho những mục đích khác LCD có chức năng hiển thị các thông tin như tổng số sản phẩm đếm được, số lượng sản phẩm của từng màu.
Hình 4.4: Mạch cảm biến màu
Mô tả: Hình trên là mạch có chức năng nhận biết màu sắc của từng sản phẩm để phân loại nhờ vào IC cảm biến màu sắc TCS3200.
Hình 4.5: Mạch cảm biến hồng ngoại
Mô tả: Hình trên là mạch cảm biến hồng ngoại có chức năng đếm số sản phẩm nhờ một led phát hồng ngoại và một led thu, led phát sẽ phát ra ánh sáng có tần số hồng ngoại, khi phát hiện vật cản tín hiệu đầu ra OUT ở mức thấp và đèn led sáng Có thể điều chỉnh khoảng cách bằng biến trở trên mạch.
Mô tả: Hình trên là cánh tay robot đã được liên kết với Arduino Khi hộp đựng
Mô tả: Hình trên là mô năng lại với nhau
Mô tả hoạt động: t nối với các mạch chức
1 Đầu tiên chúng ta phải cấp nguồn cho hệ thống Khi đó LCD, cảm biến màu sắc và cảm biến hồng ngoại sẽ được kích hoạt sáng.
2 Khi băng tải hoạt động, sản phẩm được đưa lên băng tải tự động bằng hệ thống gạt tự động của động cơ servo.
3 Sản phẩm sẽ đi qua cảm biến màu sắc để nhận biết màu,sau đó tiếp tục qua cảm biến hồng ngoại để đém số lượng, đồng thời động cơ Servo sẽ quay đến ô chứa sản phẩm phù hợp với từng màu Sau khi đi hết băng tải, sản phẩm tự động rơi vào ô chứa và phân loại.
4 Màn hình LCD sẽ hiển thị tổng số lượng sản phẩm và số lượng sản phẩm của từng màu.
5 Khi màn hình LCD hiển thi số lượng mỗi màu lên 3 thì cánh tay robot sẽ gắp ô chứa sản phẩm đi và thêm ô chứa mới vào.
- Hệ thống hoạt động chính xác khi tốc độ cấp sản phẩm vào băng tải vừa phải.
- Khi sản phẩm được cấp quá nhanh thì cảm biến màu sắc và động cơ Servo sẽ không xử lý chính xác khiến cho việc phân loại sản phẩm bị sai lệch Đây là một trong những hạn chế của đề tài.
- Ánh sáng môi trường cũng là yếu tố tác động đến độ chính xác của hệ thống.
Code chương trình
Servo topServo; int frequency = 0; int color=0; int tong=0; int RED=0; int GREEN=0; int BLUE=0; int i=0; int r=0; int g=0; int b=0; int colorState=0; void setup() { pinMode(4,OUTPUT); pinMode(0,OUTPUT); digitalWrite(0, LOW); pinMode(1,OUTPUT); digitalWrite(1, LOW); pinMode(5,INPUT_PULLUP);//// nut nhan pinMode(2,INPUT_PULLUP);//// hong ngoai pinMode(S0, OUTPUT); pinMode(S1, OUTPUT); pinMode(S2, OUTPUT); pinMode(S3, OUTPUT); pinMode(sensorOut, INPUT); pinMode(8, OUTPUT);///// module role
/// Setting frequency-scaling to 20% digitalWrite(S0, HIGH); digitalWrite(S1, HIGH); bottomServo.attach(7);//////////// bot servo topServo.attach(3);/////////////// top servo
///////Serial.begin(9600); lcd.init(); lcd.begin(16,2); lcd.backlight(); digitalWrite(8,HIGH);
{ lcd.setCursor(13,1); lcd.print(tong); lcd.setCursor(1,1); lcd.setCursor(5,1); lcd.setCursor(9,1);
// b=0; lcd.print(RED); lcd.print(GREEN); lcd.print(BLUE);
// Setting red filtered photodiodes to be read digitalWrite(S2, LOW); digitalWrite(S3, LOW); frequency = pulseIn(sensorOut, LOW); int R = frequency;
//Serial.print(frequency);//printing RED color frequency
//Serial.print(" "); delay(50); digitalWrite(S2, HIGH); digitalWrite(S3, HIGH); frequency = pulseIn(sensorOut, LOW); int G = frequency;
//Serial.print(frequency);//printing RED color frequency delay(50); digitalWrite(S2, LOW); digitalWrite(S3, HIGH); frequency = pulseIn(sensorOut, LOW); int B = frequency;
// Printing the value on the serial monitor
//Serial.print(frequency);//printing RED color frequency
//Serial.println(" "); delay(50); if(R11 & B6){ color = 1; b = b + 1;
} void loop() { digitalWrite(0,LOW); digitalWrite(1,LOW); digitalWrite(4,LOW); hienthiLCD(); int backk = digitalRead(5); color = readColor(); delay(50); switch (color){ case 1: bottomServo.write(30); break; case 2: bottomServo.write(135); break; case 3: bottomServo.write(80); break; case 0: break;
//////////// if (RED == 3){ delay(2500); digitalWrite(4, HIGH); delay(2000); digitalWrite(4, LOW); bottomServo.write(0); delay(50000); digitalWrite(8,HIGH);
//////////////// if (BLUE ==3){ delay(2500); digitalWrite(0, HIGH); delay(2000); digitalWrite(0, LOW); bottomServo.write(180); digitalWrite(8,LOW); delay(50000); digitalWrite(8,HIGH);
//////////////// if (GREEN == 3){ delay(2500); digitalWrite(1, HIGH); delay(2000); digitalWrite(1, LOW); bottomServo.write(180); digitalWrite(8,LOW); delay(47000);
BLUE = 0; lcd.setCursor(0,1); lcd.print(" 0 0 0 0 ");
/////////////////////////////////////////// điều khiển cánh tay robot
#define Dongco4 26 void remoteServo1(int y)
{ for(int i = servo3.read();i < y; i++) servo3.write(i); delay(20);
//End: Tao dieu khien servo void homeSetup()
{ remoteServo2(90); delay(500); remoteServo3(90); delay(500); remoteServo4(100); delay(500); remoteServo1(0);
//Begin: hanh trinh duy trien A void hanhTrinhVatA()
{ remoteServo4(145); delay(1000); remoteServo3(90); delay(1000); remoteServo1(115); delay(1000); remoteServo2(40); delay(1000); remoteServo3(40); delay(1000);
//kep chat remoteServo4(110); delay(1000); remoteServo2(80); delay(1000); remoteServo1(180); delay(1000);
//tha ra remoteServo2(40); delay(1000); remoteServo4(145); delay(1000); delay(1000); remoteServo3(90); delay(1000); remoteServo1(0); delay(1000);
//lay cai moi remoteServo3(40); delay(1000); remoteServo2(40); delay(1000); remoteServo4(110); delay(1000); remoteServo2(90); delay(1000); remoteServo3(90); delay(1000); remoteServo1(115); delay(1000); remoteServo2(40); delay(1000); remoteServo3(40); delay(1000); remoteServo4(135); delay(1000); homeSetup();
//End: hanh trinh duy trien A
//Begin: hanh trinh duy trien C void hanhTrinhVatC()
{ remoteServo4(145); delay(1000); remoteServo1(53); delay(1000); remoteServo2(20); delay(1000); remoteServo3(80); delay(1000);
//kep chat remoteServo4(110); delay(1000); remoteServo2(80); delay(1000); remoteServo3(90); delay(1000); remoteServo1(180); delay(1000);
//remoteServo1(servo1,60,180); remoteServo2(20); delay(1000); remoteServo3(80); delay(1000); remoteServo4(145); delay(1000); remoteServo2(90); delay(1000); remoteServo3(90); delay(1000); remoteServo1(0); delay(1000);
//lay cai moi remoteServo2(20); delay(1000); remoteServo3(80); delay(1000); remoteServo4(110); delay(1000); remoteServo2(90); delay(1000); remoteServo3(90); delay(1000); remoteServo1(53); delay(1000); delay(1000); remoteServo3(70); delay(1000); remoteServo4(135); delay(1000); homeSetup();
//End: hanh trinh duy trien C
//Begin: hanh trinh duy trien B void hanhTrinhVatB()
//vao vi tri remoteServo4(142); delay(1000); remoteServo1(80); delay(1000); remoteServo3(10); delay(1000); remoteServo2(60); delay(1000);
//kep chat va tha ra remoteServo4(110); delay(1000); remoteServo2(98); delay(1000); delay(1000); remoteServo2(60); delay(1000); remoteServo4(143); delay(1000);
//lay cai moi remoteServo2(98); delay(1000); remoteServo3(90); delay(1000); remoteServo1(0); delay(1000); remoteServo3(10); delay(1000); remoteServo2(60); delay(1000); remoteServo4(110); delay(1000); remoteServo2(98); delay(1000); remoteServo3(10); delay(1000); remoteServo1(80); delay(1000); remoteServo2(60); remoteServo4(142); delay(1000); homeSetup();
//End: hanh trinh duy trien B void setup()
Serial.begin(115200); pinMode(Led, OUTPUT); pinMode(25,INPUT); pinMode(33,INPUT); pinMode(32,INPUT);
ESP32PWM::allocateTimer(3); servo1.setPeriodHertz(50); servo2.setPeriodHertz(50); servo3.setPeriodHertz(50); servo4.setPeriodHertz(50); servo5.setPeriodHertz(50); servo1.attach(Dongco1, minUs, maxUs); servo2.attach(Dongco2, minUs, maxUs); servo4.attach(Dongco4, minUs, maxUs); servo5.attach(Dongco5, minUs, maxUs); homeSetup();
{ int a = digitalRead(25); int b = digitalRead(33); int c = digitalRead(32); delay(2000);
Serial.println(c); if(a == HIGH){ hanhTrinhVatA();
