ĐIÈU KHIỂN TÓC ĐỘ ĐỌNG CƠ DC DÙNG BỘ ĐIỀU KHIỂN PID

TỔNG LIÊN ĐOÀN LAO ĐỘNG VIỆT NAM TRƯỜNG ĐẠI HỌC TÔN ĐỨC THẮNG KHOA ĐIỆN – ĐIỆN TỬ BỘ MÔN ĐIỀU KHIỂN TỰ ĐỘNG  LUẬN VĂN TỐT NGHIỆP Đề tài: ĐIỀU KHIỂN TỐC ĐỘ ĐỘNG CƠ DC DÙNG BỘ ĐIỀU KHIỂN PID SVTH : Trần Đức Việt Ngữ MSSV : 610222D GVHD : TS Huỳnh Thái Hoàng TP HCM, Tháng 7/2008 Điều Khiển Tốc Độ Động Cơ DC Dùng PID GVHD : TS Huỳnh Thái Hoàng NHẬN XÉT CỦA GIÁO VIÊN SVTH : Trần Đức Việt Ngữ_MSSV: 610222 D Điều Khiển Tốc Độ Động Cơ DC Dùng PID GVHD : TS Huỳnh Thái Hoàng LỜI CẢM TẠ Cuốn luận văn tốt nghiệp hoàn thành tốt đẹp theo thời gian quy định nhà trường khoa Việc đạt kết không nỗ lực em mà cịn có quan tâm nhiều quý thầy cô bạn sinh viên Em xin chân thành cám ơn dạy dỗ góp ý q thầy khoa điện tử, đặc biệt xin chân thành cảm ơn bảo tận tình thầy hướng dẫn Huỳnh Thái Hồng q trình làm luận văn Xin cám ơn bạn sinh viên khoa giúp đỡ em nhiều mặt: phương tiện, sách vở, ý kiến … SVTH : Trần Đức Việt Ngữ_MSSV: 610222 D Điều Khiển Tốc Độ Động Cơ DC Dùng PID GVHD : TS Huỳnh Thái Hoàng MỤC LỤC CHƯƠNG 1: GIỚI THIỆU VỀ ĐỘNG CƠ DC SERVO 1.1.Tổng quan động servo : - 1.1.1 Nguyên lý làm việc: - 1.1.2 Cấu tạo 1.1.3 Servo điều biến độ rộng xung 1.2 Đo Tốc Độ Động Cơ - 1.3 Thông Số Động Cơ - 1.4 Lưu Đồ Giải Thuật : 10 1.5 Sơ Đồ Nguyên Lý - 11 1.6 Kết luận 11 CHƯƠNG : TÌM HIỂU CÁC KHỐI TRONG HỆ THỐNG 12 2.1 Sơ Đồ Khối Hệ Thống : - 12 2.2 Giới Thiệu Vi Điều Khiển PIC 18 F4431 - 13 2.2.1 TÍNH NĂNG 13 Modun điều khiển PWM 14 bit - 13 Modun motion feedback 13 Bộ chuyển đổi ADC 10 bit 200ksps, tốc độ cao - 14 Cấu trúc dao động linh hoạt 14 Những điểm mạnh ngoại vi 14 2.2.2 CẤU HÌNH CÁC BỘ DAO ĐỘNG 15 Các loại dao động 15 Bộ dao động tinh thể/cộng hưởng gốm - 15 Khối dao động nội - 16 2.2.3 I/O PORTS - 17 PORTA, TRISA ghi LATA - 17 PORTB, TRISB ghi LATB - 18 PORT C, TRIS C ghi LATC 19 PORTD, TRISD ghi LATD 20 PORT E, TRIS E ghi LATE - 20 2.2.4 TIMER 21 Modun timer - 21 Modun timer1 23 2.2.5 NGẮT 25 Thanh ghi INTCON 27 SVTH : Trần Đức Việt Ngữ_MSSV: 610222 D Trang Điều Khiển Tốc Độ Động Cơ DC Dùng PID GVHD : TS Huỳnh Thái Hoàng Thanh ghi PIR - 28 Thanh ghi PIE - 29 Thanh ghi IPR - 30 2.2.6 MODUN PWM 32 Tính 32 Chế độ FREE-RUNNING - 36 Chế độ SINGLE-SHOT MODE - 36 Chế độ CONTINUOUS UP/DOWN COUNT 36 2.3 Khối Hiển Thị LCD Loại TC1602A 37 2.3.1 Lý Thuyết - 37 3.2.2 Giải thuật hiển thị LCD - 39 2.3.3 Sơ Đồ Nguyên Lý 40 2.3.4 Chương trình LCD - 40 Mạch Kích Và Mạch Động Lực Cầu H 41 Khối Bàn Phím : - 41 5.1 Sơ Đồ Nguyên Lý : 41 5.2 Lưu Đồ giải Thuật : 42 CHƯƠNG : SƠ ĐỒ NGUYÊN LÝ & LƯU ĐỒ GIẢI THUẬT HỆ THỐNG 45 3.1 Sơ Đồ Nguyên Lý hệ Thống - 45 3.2 Lưu Đồ Giải thuật Chương Trình Chính 46 CHƯƠNG : GIỚI THIỆU THUẬT TÓAN PID 47 4.1 Lý thuyết PID -47 4.1.1 Luật điều khiển PID (PID control law) -47 4.1.2 Lưa chọn cấu trúc tham số luật điều khiển phương pháp thực nghiệm. -48 4.1.3 Hiệu chỉnh PI phương pháp thử sai. 50 4.2 Lưu đồ Giải Thuật PID -51 CHƯƠNG : LẬP TRÌNH DIỀU KHIỂN -52 SVTH : Trần Đức Việt Ngữ_MSSV: 610222 D Trang Điều Khiển Tốc Độ Động Cơ DC Dùng PID CHƯƠNG 1: GVHD : TS Huỳnh Thái Hoàng GIỚI THIỆU VỀ ĐỘNG CƠ DC SERVO 1.1.Tổng quan động servo : Cùng với phát triển nhanh chóng khoa học công nghệ, kỹ thuật điều khiển tự động ứng dụng rộng rãi đóng vai trị quan trọng nhiều lĩnh vực, theo địi hỏi chất lượng, tốc độ điều khiển ngày cao Động DC đối tượng điều khiển thường gặp thực tế, ứng dụng rộng rãi công nghiệp, dân dụng, quốc phòng Thiết kế điều khiển PID số tốn quen thuộc nhờ tính đơn giản hiệu Vấn đề điều khiển thường bị giới hạn tốc độ, độ xác (do độ rộng bit liệu giới hạn) sử Động DC động bước vốn hệ hồi tiếp vòng hở - ta cấp điện để động quay chúng quay ta khơng biết, kể động bước động quay góc xác định tùy vào số xung nhận Việc thiết lập hệ thống điều khiển để xác định ngăn cản chuyển động quay động làm động không quay không dễ dàng Mặt khác, động servo thiết kế cho hệ thống hồi tiếp vịng kín Tín hiệu động nối với mạch điều khiển Khi động quay, vận tốc vị trí hồi tiếp mạch điều khiển Nếu có bầt kỳ lý ngăn cản chuyển động quay động cơ, cấu hồi tiếp nhận thấy tín hiệu chưa đạt vị trí mong muốn Mạch điều khiển tiếp tục chỉnh sai lệch cho động đạt điểm xác Động servo có nhiều kiểu dáng kích thước, sử dụng nhiếu máy khác nhau, từ máy tiện điều khiển máy tính mơ hình máy bay xe Ứng dụng động servo robot, loại với động dùng mơ hình máy bay xe Trong thực tế, thân động servo điều khiển vơ tuyến, nối với máy thu vơ tuyến máy bay hay xe Động servo nhận tín hiệu từ máy thu Như có nghĩa ta không cần phải điều khiển robot tín hiệu vơ tuyến cách sử dụng động servo, trừ ta muốn Ta điều khiển động SVTH : Trần Đức Việt Ngữ_MSSV: 610222 D Trang Điều Khiển Tốc Độ Động Cơ DC Dùng PID GVHD : TS Huỳnh Thái Hoàng servo máy tính, vi xử lý hay chí mạch điện tử đơn giản dùng IC 555 1.1.1 Nguyên lý làm việc: Từ trước đến tổng hợp hệ truyền động nhiều thông số người ta thường phân hệ thành cấu trúc nhiều mạch vòng có điều chỉnh nối theo cấp Do việc xác định tham số đối tượng điều khiển (mà nhà sản xuất cung cấp) tất + I- U- - + M - I- EL- U- M L- R- DC Motor Rw M Mc Thông thường đối động điện chiều nhà sản xuất cung cấp thông số bản: Uưđm, Iưđm, Pđm,  đm Tuy nhiên để tổng hợp mô hệ cần phải biết thêm số tham số khác như: Điện trở phần ứng (Rư), Điện cảm phần ứng (Lư), Mơmen qn tính rôto (J), Hằng số mômen (Km), Mômen tổn hao theo tốc độ (B  ) Đây tham số động nên để xác định cần phải thí nghiệm nhiều lần lấy giá trị trung bình Có nhiều phương pháp thực nghiệm hay công thức lý thuyết gần để tính chúng Ở tác giả kết hợp hai phương pháp xác định được: - Hằng số mômen Km=0,138(Nm/A) - Mômen tổn hao theo tốc độ B  : B = 3,38,10-5(Nm/rad) Với B tổng suy giảm tương đương (the total equivalent damping) - Điện trở phần ứng Rư= 5,9() - Điện cảm phần ứng Lư= 3,54(mH.) - Mơmen qn tính rôto J5,10-5kgm2  U + I M + R(1+Ts) - B+Js Km - Mc E Km Hằng số thời gian điện số thời gian cơ: Tư=0,6ms; Tc=15,5ms SVTH : Trần Đức Việt Ngữ_MSSV: 610222 D Trang Điều Khiển Tốc Độ Động Cơ DC Dùng PID GVHD : TS Huỳnh Thái Hoàng 1.1.2 Cấu tạo Motor 2.Electronics Board 3.Positive Power Wire (Red) 4.Signal Wire (Yellow or White) 5.Negative or Ground Wire (Black) 6.Potentiometer 7.Output Shaft/Gear 8.Servo Attachment Horn/Wheel/Arm 9.Servo Case 10.Integrated Control Chip 1.1.3 Servo điều biến độ rộng xung Trục động servo R/C định vị nhờ vào kỹ thuật gọi biến độ rộng xung (PWM) Trong hệ thống này, servo đáp ứng dãy xung số ổn định Cụ thể SVTH : Trần Đức Việt Ngữ_MSSV: 610222 D Trang Điều Khiển Tốc Độ Động Cơ DC Dùng PID GVHD : TS Huỳnh Thái Hoàng hơn, mạch điều khiển đáp ứng tín hiệu số có xung biến đổi từ – ms Các xung gởi 50 lần/giây Chú ý số xung giây điều khiển servo mà chiều dài xung Servo đòi hỏi khoảng 30 – 60 xung/giây Nếu số qua thấp, độ xác cơng suất để trì servo giảm Với độ dài xung ms, servo điều khiển quay theo chiều (giả sử chiều kim đồng hồ Hình Hình 3: Điều khiển vị trí trục động cách điều chế độ rộng xung Với độ dài xung xung ms, servo quay theo chiều ngược lại Kỹ thuật gọi tỉ lệ số - chuyển động servo tỉ lệ với tín hiệu số điều khiển Công suất cung cấp cho động bên servo tỉ lệ với độ lệch vị trí trục với vị trí cần đến Nếu servo gần vị trí đích, động truyền động với tốc độ thấp Điều đảm bảo động không vượt điểm định đến Nhưng servo xa vị trí đích truyền động với vận tốc tối đa để đến đích nhanh tốt Khi trục đến vị trí mong muốn, động giảm tốc Quá trình tưởng chừng phức tạp diễn khoảng thời gian ngắn - servo trung bình cú th quay 60o vũng ẳ - ẵ giõy Vì độ dài xung thay đổi tùy theo hãng chế tạo nên ta phải chọn servo máy thu vô tuyến thuộc hãng để đảm bảo tương thích Đối với robot, ta phải làm vài thí nghiệm để xác định độ dài xung tối ưu 1.2 Đo Tốc Độ Động Cơ Encoder: thường dùng để đo góc quay, có loại encoder encoder gia tăng encoder xung tuyệt đối Cả hai dựa nguyên tắc cảm biến ánh sáng với đĩa có khắc vạch sáng tối quay nguồn sáng phototransistor Đối với động sevor thường có tích hợp sẳn encoder lắp trực tiếp vào đuôi động Encoder loại cho hai xung, xung A xung B lệch 90 , độ phân giải tùy thuộc vào loại động Xung A SVTH : Trần Đức Việt Ngữ_MSSV: 610222 D Xung B Trang Điều Khiển Tốc Độ Động Cơ DC Dùng PID GVHD : TS Huỳnh Thái Hoàng Quay thuận A sớm pha B 90 Quay ngược A trể pha B 90  vòng    phút  Đo tốc độ sử dụng encoder thường dùng đơn vị   vòng  xung 60  =  phút  ĐPGencoder Tgiây V  Với Tgiây thời gian lấy mẫu (tính giây)   vịng  60      phút   ĐPGencoder.Tgiây 60 = ĐPGencoder Để xung = V   Tgiây Với thời gian lấy mẫu   =1   60 (giây) số xung đọc số vòng ĐPGencoder quay động phút Nguyên nhân gây sai số sử dụng encoder: Tần số xung encoder tỷ lệ thuận với tốc độ độ phân giải encoder, tần số xung lớn làm đếm đọc thiếu xung Nhiễu đường truyền, để khắc phục thường sử dụng encoder ngõ xung vi sai A,A\,B,B\ sử dụng encoder ngõ dòng Hiện tượng rung đọc encoder: gây sai số lớn không mong muốn, nguyên nhân rung khí encoder hoạt động SVTH : Trần Đức Việt Ngữ_MSSV: 610222 D Trang Điều Khiển Tốc Độ Động Cơ DC Dùng PID CHƯƠNG : GVHD : TS Huỳnh Thái Hồng GIỚI THIỆU THUẬT TĨAN PID 4.1 LÝ THUYẾT VỀ PID 4.1.1 Luật điều khiển PID (PID control law) Khác với luật điều khiển khác, luật PID cài vào thiết bị điều khiển ta điều khiển động Một điều khiển có khả làm thay đổi chất lượng trình động chất lượng hệ thống chế độ xác lập Bộ điều khiển PID giúp cho hệ thống tác động nhanh nhờ khả bù số qn tính lớn hệ Bộ điều khiển có khả triệt tiêu sai lệch tĩnh nhờ thành phần khâu tích phân cài đặt điều khiển Cấu trúc hệ thống sử dụng điều khiển PID biểu diễn H1 Bộ điều khiển PID cài đặt luật điều khiển đơn giản u(t) = kPe(t) + TI t de(t )  e( )d + TD d (t ) o thiết bị điều khiển tương tự u(k) = kPe(k) + uI(k-1) + T T e(k) + D e(k )  e(k  1) TI T cho thiết bị điều khiển số Hình H2a H2b biểu diễn cấu trúc luật điều khiển PID liên tục luật điều khiển PID số (S ) (S ) V    E   W PID (s) (S ) U   Đối tượng Y (S )  điều khiển  H1: Cấu trúc hệ thống điều khiển tự động Luật điều khiển gồm ba thông số là: hệ số khuếch đại kP luật điều khiển tỷ lệ, số thời gian TI luật điều khiển tích phân số thời gian TD luật điều khiển vi phân, điều khiển PID số có thêm tham số thời gian trích mẫu (Sample Time) T Phương thức làm việc điều khiển PID so sánh với nhóm nghiên cứu (Researchteam) gồm ba thành viên, người ln giữ thăng định luật điều khiển tỷ lệ, ln dự đốn tương lai nên đưa định mang tính chất vượt trước người cịn lại ông chăm chỉ, làm việc tưởng chừng khơng thể làm tốt nữa, thành phần tích phân điều khiển PID Tính huyền diệu điều khiển PID kết hợp hài hòa ba luật điều khiển tỷ lệ, vi phân tích phân Luật điều khiển vi phân gọi luật điều khiển vượt trước đầu luật điều khiển tỷ lệ với đạo hàm sai lệch e(t) Luật điều khiển tích phân cịn gọi luật điều khiển chậm sau đầu luật điều khiển tỷ lệ với sai lệch tích lũy theo thời gian SVTH : Trần Đức Việt Ngữ_MSSV: 610222 D Trang 47 Điều Khiển Tốc Độ Động Cơ DC Dùng PID GVHD : TS Huỳnh Thái Hoàng Một luật điều khiển luật điều khiển thích hợp với nhiều đối tượng công nghiệp khác luật PID luật điều khiển Thường luật điều khiển PID thiết kế cho đối tượng tuyến tính có mơ hình xác Nhưng thực tế, việc xác định mơ hình tốn học đối tượng khó, nên xác định xác lại khó khăn 4.1.2 Lưa chọn cấu trúc tham số luật điều khiển phương pháp thực nghiệm Luật điều khiển thiết kế theo phương pháp thực nghiệm dựa sở hàm độ h(t) Như vậy, muốn thiết kế luật điều khiển đòi hỏi phải xác định hàm độ h(t) Điều hoàn toàn đơn giản kỹ sư trường Công cụ thu thập liệu cần thiết máy ghi trình, hay máy tính cá nhân PC để giám sát a Phương pháp thời gian tổng Kuhn Đây phương pháp thiết kế sở thực nghiệm đơn giản Phương pháp thời gian tổng Kuhn thích hợp cho lớp đối tượng có đặc tính q độ khơng có độ q điều chỉnh, thường gọi động học hình chữ S Từ hàm độ thực nghiệm h(t), phải xác định hai tham số quan trọng đối tượng hệ số khuếch đại kđt số thời gian tổng T  Hằng số thời gian tính công thức: T= A K đt A, K đt xác định từ hình sau: k đt h(t) A t Sau xác định xong hai tham số K đt T  đối tượng, lựa chọn cấu trúc tham số luật điều khiển theo bảng thiết kế Kuhn, Bảng B1 B2 Luật điều khiển Luật PI kp Ti Td 0.5 K đt 0.5 T  - Luật PID K đt 0.667 T  0.167 T  B1: cấu trúc tham số luật điều khiển ưu tiên chế độ lọc nhiễu SVTH : Trần Đức Việt Ngữ_MSSV: 610222 D Trang 48 Điều Khiển Tốc Độ Động Cơ DC Dùng PID GVHD : TS Huỳnh Thái Hoàng Luật điều khiển Luật PI kp Ti Td K đt 0.7 T  - Luật PID K đt 0.8 T  0.194 T  B2: cấu trúc tham số luật điều khiển ưu tiên chế độ tác động nhanh b Xác định thông số điều khiển PID dựa vào phương pháp thực nghiệm Zeigler-Nichols Zeigler Nichols đưa hai cách chọn thông số điều khiển PID tùy theo đặc điểm đối tượng Cách 1: dựa vào đáp ứng hệ hở, áp dụng cho đối tượng có đáp ứng tín hiệu vào hàm nấc có dạng chữ S, ví dụ nhiệt độ lị nhiệt, tốc độ động (Hình_1) r(t) c(t) Đối tượng c(t) K T1 t T2 Hình_1: Đáp ứng nấc hệ hở có dạng S Thông số điều khiển P, PI, PID chọn sau: Thông số Bộ ĐK KP TI SVTH : Trần Đức Việt Ngữ_MSSV: 610222 D TD Trang 49 Điều Khiển Tốc Độ Động Cơ DC Dùng PID GVHD : TS Huỳnh Thái Hoàng P T / T1 K   PI 0.9T / T1 K  T /0.3 PID 1.2T / T1 K  2T 0.5T 4.1.3 Hiệu chỉnh PI phương pháp thử sai Bước 1: đặt K I =0, K p = K p Bước 2: tăng dần K p dao động xuất hiện, chọn K p =50% gía trị vừa tìm Bước 3: tăng dần K I bắt đầu có vọt lố SVTH : Trần Đức Việt Ngữ_MSSV: 610222 D Trang 50 Điều Khiển Tốc Độ Động Cơ DC Dùng PID GVHD : TS Huỳnh Thái Hoàng 4.2 Lưu đồ Giải Thuật PID start Chờ lấy mẫu Tính tốc độ từ mẫu thu Eroor = velocity_set - velocity Uk =Kp*saiso + U(i-1) + Ki*saiso +[saiso – (saiso-1)]*Kd Saiso-1 =Saiso U(i-1) = Ui Bão hoà PWM SVTH : Trần Đức Việt Ngữ_MSSV: 610222 D Trang 51 Điều Khiển Tốc Độ Động Cơ DC Dùng PID CHƯƠNG : GVHD : TS Huỳnh Thái Hồng LẬP TRÌNH DIỀU KHIỂN #include #device pic18f4431*=16 ADC=10 #include #use delay(clock=8000000) //###################################################################### ##### //################################# CAC KHAI BAO ############################ //###################################################################### ##### // khai bao chan port #byte portd=0xf83 //dia chi portd #byte portc=0xf82 //dia chi portc #byte portb=0xf81 //dia chi portb #byte porta=0xf80 //dia chi porta // khai bao ghi dieu khien timer0 #byte t0con=0xfd5 //dia chi ghi dieu khien timer0 #bit tmr0on=t0con.7 //bit dieu khien on/off timer0 #bit t016bit=t0con.6 //bit dieu khien timer0 16bit #bit t0cs=t0con.5 //bit lua chon nguon xung clock timer0 #bit t0se=t0con.4 //bit lua chon canh xung timer0 #byte tmr0l=0xfd6 //byte thap cua timer0 #byte tmr0h=0xfd7 //byte cao cua timer0 // khai bao ghi dieu khien timer1 #byte t1con=0xfcd //dia chi ghi dieu khien timer1 #bit rd16=t1con.7 //bit cho phep che doc ghi 16bit #bit tmr1cs=t1con.1 //bit lua chon nguon xung clock timer1 #bit tmr1on=t1con.0 //Timer1 On bit #byte tmr1h=0xfcf #byte tmr1l=0xfce #byte pie1=0xf9d #bit tmr1ie=pie1.0 //TMR1 Overflow Interrupt Enable bit #byte pir1=0xf9e #bit tmr1if=pir1.0 //TMR1 Overflow Interrupt Flag bit // khai bao ghi dieu khien pwm #byte ptcon0=0xf7f //thanh ghi ptcon0 #byte ptcon1=0xf7e //thanh ghi ptcon1 #byte ptmrl=0xf7d // #byte ptmrh=0xf7c // #byte ptperl=0xf7b // SVTH : Trần Đức Việt Ngữ_MSSV: 610222 D Trang 52 Điều Khiển Tốc Độ Động Cơ DC Dùng PID GVHD : TS Huỳnh Thái Hoàng #byte ptperh=0xf7a // #byte pdc0l=0xf79 // #byte pdc0h=0xf78 // #byte pwmcon0=0xf6f // #byte pwmcon1=0xf6e // // khai bao ghi ngat #byte intcon=0xff2 //thanh ghi ngat intcon #bit int_global=intcon.7 //bit cho phep ngat toan cuc #bit tmr0ie=intcon.5 //bit cho phep ngat tran timer0 #bit tmr0if=intcon.2 //bit co ngat tran timer0 #byte intcon2=0xff1 //thanh ghi ngat intcon2 #bit intedg1=intcon2.5 //bit lua chon canh ngat ngoai1 #byte intcon3=0xff0 //thanh ghi ngat intcon3 #bit int1ie=intcon3.3 //bit cho phep ngat ngoai1 #bit int1if=intcon3.0 //bit co ngat ngoai1 // khai bao ghi dieu khien dao dong #byte soccon=0xfd3 //thanh ghi dieu khien dao dong // khai bao chan lcd #byte data=0xf83 //du lieu noi voi portd #bit rs=portc.0 #bit rw=portc.1 #bit enable=portc.2 #bit wait=portd.0 //###################################################################### ##### //################################### HAM CON ############################### //###################################################################### ##### // ham cho lcd thuc thi lenh char data_lcd; void wait_lcd() { cho_lcd: output_high(pin_c2); output_low(pin_c0); output_high(pin_c1); output_d(0xff); delay_us(5); if(input(pin_d7)==1) { SVTH : Trần Đức Việt Ngữ_MSSV: 610222 D Trang 53 Điều Khiển Tốc Độ Động Cơ DC Dùng PID GVHD : TS Huỳnh Thái Hoàng goto cho_lcd; } output_low(pin_c2); output_low(pin_c1); } // ham khoi dong lcd void init_lcd() { output_high(pin_c2); output_low(pin_c0); output_d(0x38); output_low(pin_c2); wait_lcd(); output_high(pin_c2); output_low(pin_c0); output_d(0x0e); output_low(pin_c2); wait_lcd(); output_high(pin_c2); output_low(pin_c0); output_d(0x06); output_low(pin_c2); wait_lcd(); } // ham xoa man hinh lcd void clear_lcd() { output_high(pin_c2); output_low(pin_c0); output_d(0x01); output_low(pin_c2); wait_lcd(); } // ham set vi tri tro int8 cursor; int8 set_cursor(int8 cursor) { data=0x80+cursor; output_high(pin_c2); output_low(pin_c0); output_d(data); output_low(pin_c2); wait_lcd(); SVTH : Trần Đức Việt Ngữ_MSSV: 610222 D Trang 54 Điều Khiển Tốc Độ Động Cơ DC Dùng PID GVHD : TS Huỳnh Thái Hoàng } // ham ghi du lieu lcd char write_text(char data_lcd,) { output_high(pin_c2); output_high(pin_c0); output_d(data_lcd); output_low(pin_c2); wait_lcd(); } // ham nap duty_cycle cho pwm int16 duty_cycle; int16 write_duty(int16 duty_cycle) { pdc0h=make8(duty_cycle,1); pdc0l=make8(duty_cycle,0); } // ham doc hang int8 cot,hang,off_set; int1 bit_switch,bit_text,bit0,bit1,bit2,bit3; read_switch() { start_read_switch: cot=0; hang=0; bit_switch=0; output_low(pin_a0); //set switch output_low(pin_a1); output_low(pin_b2); output_low(pin_b3); output_high(pin_b4); output_high(pin_e0); output_high(pin_b6); output_high(pin_b7); delay_us(5); bit0=input(pin_b4); bit1=input(pin_e0); bit2=input(pin_b6); bit3=input(pin_b7); bit_text=bit0&&bit1&&bit2&&bit3; //text switch if(bit_text==1) //no switch push goto start_read_switch; //quit read_switch if(bit0==0) SVTH : Trần Đức Việt Ngữ_MSSV: 610222 D Trang 55 Điều Khiển Tốc Độ Động Cơ DC Dùng PID GVHD : TS Huỳnh Thái Hoàng cot=0; else if(bit1==0) cot=1; else if(bit2==0) cot=2; else cot=3; output_high(pin_a0); output_high(pin_a1); output_high(pin_b2); output_high(pin_b3); output_low(pin_b4); output_low(pin_e0); output_low(pin_b6); output_low(pin_b7); delay_us(5); bit0=input(pin_a0); bit1=input(pin_a1); bit2=input(pin_b2); bit3=input(pin_b3); bit_text=bit0&&bit1&&bit2&&bit3; if(bit_text==1) goto start_read_switch; if(bit0==0) hang=0; else if(bit1==0) hang=1; else if(bit2==0) hang=2; else hang=3; off_set=cot+4*hang; delay_ms(200); return off_set; } int8 v_thousand,v_hundred,v_ten,v_unit,i; int8 sampling; int16 velocity=0,velocity_temp=0,velocity_set; signed int16 error,error_temp=0,Ui_temp=0,Uk; int16 set_velocity() { data_lcd='C'; write_text(data_lcd); SVTH : Trần Đức Việt Ngữ_MSSV: 610222 D Trang 56 Điều Khiển Tốc Độ Động Cơ DC Dùng PID GVHD : TS Huỳnh Thái Hoàng data_lcd='H'; write_text(data_lcd); data_lcd='O'; write_text(data_lcd); data_lcd='N'; write_text(data_lcd); data_lcd=' '; write_text(data_lcd); data_lcd='C'; write_text(data_lcd); data_lcd='H'; write_text(data_lcd); data_lcd='I'; write_text(data_lcd); data_lcd='E'; write_text(data_lcd); data_lcd='U'; write_text(data_lcd); data_lcd=':'; write_text(data_lcd); wait_direct: read_switch(); if(off_set==12) { clear_lcd(); data_lcd=''; write_text(data_lcd); output_low(pin_b0); } else goto wait_direct; data_lcd='V'; write_text(data_lcd); data_lcd='s'; write_text(data_lcd); data_lcd=':'; SVTH : Trần Đức Việt Ngữ_MSSV: 610222 D Trang 57 Điều Khiển Tốc Độ Động Cơ DC Dùng PID // GVHD : TS Huỳnh Thái Hoàng write_text(data_lcd); v_thousand=0; v_hundred=0; v_ten=0; v_unit=0; for(i=1;i9) break; v_thousand=v_hundred; v_hundred=v_ten; v_ten=v_unit; v_unit=off_set; set_cursor(cursor=0x04); data_lcd=v_thousand|0x30; write_text(data_lcd); data_lcd=v_hundred|0x30; write_text(data_lcd); data_lcd=v_ten|0x30; write_text(data_lcd); data_lcd=v_unit|0x30; write_text(data_lcd); } velocity_set=v_thousand*10; velocity_set=(velocity_set+v_hundred)*10; velocity_set=(velocity_set+v_ten)*10; velocity_set=velocity_set+v_unit; set_cursor(cursor=0x09); data_lcd='O'; write_text(data_lcd); data_lcd='k'; write_text(data_lcd); data_lcd='?'; write_text(data_lcd); while(1) { read_switch(); if(off_set==15) break; delay_ms(5); } return velocity_set; SVTH : Trần Đức Việt Ngữ_MSSV: 610222 D Trang 58 Điều Khiển Tốc Độ Động Cơ DC Dùng PID GVHD : TS Huỳnh Thái Hoàng } int16 display_velocity(int16 velocity) { v_unit=velocity%10; v_ten=(velocity/10)%10; v_hundred=(velocity/100)%10; v_thousand=velocity/1000; set_cursor(cursor=0x0c); data_lcd=v_thousand|0x30; write_text(data_lcd); data_lcd=v_hundred|0x30; write_text(data_lcd); data_lcd=v_ten|0x30; write_text(data_lcd); data_lcd=v_unit|0x30; write_text(data_lcd); } void main() { int16 velocity1,velocity2,velocity3,velocity4,velocity5; soccon=0x72; //**************** thiet lap cau hinh cho PWM ***************** ptcon0=0x00; ptcon1=0x80; //1:1 Postscale //PWM time base input clock is FOSC/4 //Free-Running mode //PWM time base is on pwmcon0=0x11; //PWM1 pin is enabled for PWM output //PWM I/O pin pair (PWM0, PWM1) is in the Independent mode pwmcon1=0x00; //0000 = 1:1 Postscale //PWM time base is counting upwards //Period Buffer registers are enabled //pwm phan giai 10bit // ptperh=0x00; ptperl=0xff; //******************* thiet lap cac thong so ban dau ***************** init_lcd(); t0con=0x20; SVTH : Trần Đức Việt Ngữ_MSSV: 610222 D Trang 59 Điều Khiển Tốc Độ Động Cơ DC Dùng PID GVHD : TS Huỳnh Thái Hoàng tmr0h=0; tmr0l=0; sampling=0; set_velocity(); set_cursor(cursor=0x09); data_lcd='V'; write_text(data_lcd); data_lcd='r'; write_text(data_lcd); data_lcd=':'; write_text(data_lcd); start: tmr0on=1; delay_ms(150); tmr0on=0; velocity=2*make16(tmr0h,tmr0l); tmr0h=0; tmr0l=0; error=velocity_set-velocity; Uk=error/5+Ui_temp+error/5; Ui_temp=Ui_temp+error/5; if(Uk>1023) duty_cycle=1023; else if(Uk