Nghiên cứu ứng dụng vi điều khiển stm32f4 trong điều khiển tốc độ động cơ điện một chiều

BỘ GIÁO DỤC VÀ ĐÀO TẠO TRƯỜNG ĐẠI HỌC BÁCH KHOA HÀ NỘI - HOÀNG ĐỨC HẠNH NGHIÊN CỨU ỨNG DỤNG VI ĐIỀU KHIỂN STM32F4 TRONG ĐIỀU KHIỂN TỐC ĐỘ ĐỘNG CƠ ĐIỆN MỘT CHIỀU LUẬN VĂN THẠC SĨ KĨ THUẬT ĐIỀU KHIỂN VÀ TỰ ĐỘNG HÓA Hà Nội – Năm 2014 BỘ GIÁO DỤC VÀ ĐÀO TẠO TRƯỜNG ĐẠI HỌC BÁCH KHOA HÀ NỘI - HOÀNG ĐỨC HẠNH NGHIÊN CỨU ỨNG DỤNG VI ĐIỀU KHIỂN STM32F4 TRONG ĐIỀU KHIỂN TỐC ĐỘ ĐỘNG CƠ ĐIỆN MỘT CHIỀU Chuyên ngành: ĐIỀU KHIỂN VÀ TỰ ĐỘNG HÓA LUẬN VĂN THẠC SĨ KĨ THUẬT ĐIỀU KHIỂN VÀ TỰ ĐỘNG HÓA NGƯỜI HƯỚNG DẪN KHOA HỌC: GS.TS PHAN XUÂN MINH Hà Nội – Năm 2014 Hoàng Đức Hạnh - CB120350 LỜI CAM ĐOAN Tên : Hoàng Đức Hạnh Sinh ngày : 09/09/1985 Học viên lớp cao học khóa 2012 - Ngành Điều khiển Tự Động Hóa - Trường Đại Học Bách Khoa Hà Nội Tôi xin cam đoan đề tài “NGHIÊN CỨU ỨNG DỤNG VI ĐIỀU KHIỂN STM32F4 TRONG ĐIỀU KHIỂN TỐC ĐỘ ĐỘNG CƠ ĐIỆN MỘT CHIỀU” GS.TS Phan Xuân Minh hướng dẫn cơng trình nghiên cứu riêng tơi Ngoài tài liệu tham khảo dẫn cuối luận án, đảm bảo không chép cơng trình kết người khác Nếu phát có sai phạm với điều cam đoan trên, tơi xin hồn tồn chịu trách nhiệm Học viên Hoàng Đức Hạnh i Hoàng Đức Hạnh - CB120350 MỤC LỤC LỜI CAM ĐOAN i MỤC LỤC ii DANH MỤC TỪ VIẾT TẮT iv DANH MỤC HÌNH ẢNH v DANH MỤC BẢNG viii MỞ ĐẦU ix CHƢƠNG 1: THIẾT KẾ BỘ ĐIỀU KHIỂN CHO ĐỘNG CƠ DC CÔNG SUẤT 10W 1.1.Nhận dạng động điện chiều 9V-DC Toolbox Identification phần mềm Matlab 1.1.1.Động 9V-DC 1.1.2.Sơ đồ cấu trúc nhận dạng động 1.1.3.Thu thập liệu mơ hình động 1.2.Thiết kế điều khiển 1.2.1.Thiết kế điều khiển PID - Ziegler- Nichols 1.2.2.Thiết kế điều khiển PID - Internal Model Control (IMC) 12 1.2.3.Thiết kế điều khiển Fuzzy-PID 15 1.2.4.Đánh giá, lựa chọn nâng cao chất lượng điều khiển 24 1.3.Tổng kết chương 31 CHƢƠNG 2: TỔNG HỢP BỘ ĐIỀU KHIỂN CHO ĐỘNG CƠ 32 2.1 Cấu hình hệ thống điều khiển 32 2.1.1 Cảm biến (Encoder) 32 2.1.2 Thiết bị chấp hành (ICL298) 34 2.1.3 Vi điều khiển STM32F4 ngoại vi 37 2.2 Viết chương trình nạp cho Kit STM32F4 Discovery 52 2.2.1 Chương trình sử dụng điều khiển PID 53 2.2.2 Chương trình sử dụng điều khiển Fuzzy-PID 55 2.3.Tổng kết chương 57 CHƢƠNG 3: KẾT QUẢ THỰC NGHIỆM 58 3.1 Sơ đồ khối cấu trúc phần cứng 58 3.2 Chương trình giám sát hệ thống 58 3.3 Kết thực nghiệm đáp ứng hệ thống điều khiển ổn định tốc độ động điều khiển PID 60 ii Hoàng Đức Hạnh - CB120350 3.4 Kết thực nghiệm đáp ứng hệ thống điều khiển ổn định tốc độ động điều khiển PID định mờ 61 3.5 Tổng kết chương 63 KẾT LUẬN VÀ HƢỚNG PHÁP TRIỂN 64 TÀI LIỆU THAM KHẢO 65 PHỤ LỤC 66 iii Hoàng Đức Hạnh - CB120350 DANH MỤC TỪ VIẾT TẮT ARM Acorn RISC Machines RISC Reduced Instruction Set Computer PWM Pulse Width Modulation LCD Liquid Crystal Display UART Universal Asynchronous Reveiver/Transmitter ALU Arithmetic Logic Unit iv Hồng Đức Hạnh - CB120350 DANH MỤC HÌNH ẢNH Hình 1.1: Động Lego-NXT Hình 1.2: Mơ hình đối tượng điều khiển Hình 1.3: Sơ đồ cấu trúc nhận dạng động Hình 1.4: Chương trình thu thập liệu Hình 1.5: Cửa sổ tool nhận dạng hệ thống Hình 1.6: Chọn đầu vào cho mơ hình nhận dạng Hình 1.7: Phương thức nhận dạng Hình 1.8: Mơ hình nhận dạng Hình 1.9: Tham số thu từ tool nhận dạng Hình 1.10: Sai lệch mơ hình nhận dạng Hình 1.11: Nhiệm vụ điều khiển PID Hình 1.12: Xác định tham số xấp xỉ đối tượng Hình 1.13: Hộp thoại SISOTool 10 Hình 1.14: Hộp thoại sử dụng phương pháp Ziegler-Nichols 10 Hình 1.15: Mơ hệ thống simulink PIDZiegler-Nichols 11 Hình 1.16: Sơ đồ khối điều khiển PID Ziegler-Nichols 11 Hình 1.17: Đặc tính tốc độ động sử dụng điều khiển PID Ziegler-Nichols 12 Hình 1.18: Hộp thoại sử dụng phương pháp IMC 13 Hình 1.19: Mơ hình hệ thống sử dụng điều khiển PID_IMC 14 Hình 1.20: Sơ đồ khối điều khiển PID_IMC 14 Hình 1.21: Đặc tính tốc độ động sử dụng điều khiển PID_IMC 14 Hình 1.22: Các dạng hàm phụ thuộc tập mờ 15 Hình 1.23: Các hàm phụ thuộc 15 Hình 1.24: Giải mờ theo phương pháp cực đại 17 Hình 1.25: Sơ đồ cấu trúc điều khiển Mờ 18 Hình 1.26: Cấu trúc điều khiển Fuzzy_PID 19 Hình 1.27: Cấu trúc khối Mờ 19 Hình 1.28: Hàm phụ thuộc sai lệch e 20 Hình 1.29: Hàm phụ thuộc sai lệch de 21 Hình 1.30: Hàm phụ thuộc biến K‟p,K‟i‟ K‟d 21 v Hồng Đức Hạnh - CB120350 Hình 1.31: Sơ đồ khối điều khiển Fuzzy-PID 22 Hình 1.32: Sơ đồ cấu trúc mơ hình bên điều khiển PID 22 Hình 1.33: Sơ đồ mơ hình hệ thống sử dụng điều khiển Fuzzy-PID 23 Hình 1.34: Đặc tính tốc độ động sử dụng điều khiển Fuzzy_PID 23 Hình 1.35: Mơ hình hệ thống sử dụng điều khiển 24 Hình 1.36: Kết mô cho hệ thống sử dụng điều khiển 24 Hình 1.37: Sơ đồ tác động nhiễu lên tín hiệu truyền từ Encoder 25 Vi điều khiển 25 Hình 1.38 Băng thơng tín hiệu đo từ Encoder 26 Hình 1.39: Mạch điện RC 26 Hình 1.40: Sơ đồ khối hệ thống không sử dụng lọc 27 Hình 1.41 Sơ đồ khối Encoder bao gồm nhiễu 27 Hình 1.42: Cài đặt tham số cho nhiễu 28 Hình 1.43: Đáp ứng tốc độ hệ thống khơng có lọc 28 Hình 1.44 Sơ đồ khối hệ thống có sử dụng lọc 29 Hình 1.45: Cài đặt tham số cho lọc thơng thấp 29 Hình 1.46: Đáp ứng tốc độ hệ thống có sử dụng lọc 30 Hình 2.1: Sơ đồ cấu hình hệ thống điều khiển tốc độ động DC 32 Hình 2.2: Cấu tạo Encoder Lego motor 32 Hình 2.3: Mơ hình Encoder quang sử dụng Lego motor 33 Hình 2.4: Sơ đồ xung encoder quang 33 Hình 2.5: Sơ đồ chân L298 34 Hình 2.6: Sơ đồ mạch kết nối động sử dụng L298N 35 Hình 2.7: Sơ đồ mạch lực điều khiển sử dụng L298 36 Hình 2.8: Mạch thực tế công suất sử dụng L298 37 Hình 2.9: Cấu trúc ARM 38 Hình 2.10: Kiến trúc Cortex M4 40 Hình 2.11: Cấu trúc chung STM32 41 Hình 2.12 Sơ đồ khối phần cứng STM32F4 42 Hình 2.13: Sơ đồ nguyên lý mạch 43 Hình 2.14:Kit STM32F4 Discovery 44 Hình 2.15: Sơ đồ khối ADC 45 vi Hồng Đức Hạnh - CB120350 Hình 2.16: Cấu hình khối ADC 45 Hình 2.17: Sơ đồ khối PWM 46 Hình 2.18: Tín hiệu PWM 46 Hình 2.19: Sơ đồ khối LCD 48 Hình 2.20: Đặt cấu hình cho LCD 48 Hình 2.21: Sơ đồ khối Receive 49 Hình 2.22: Đặt cấu hình cho khối Receive 49 Hình 2.23: Sơ đồ khối Transmit 49 Hình 2.24: Đặt cấu hình cho khối Transmit 50 Hình 2.25: Khối Encoder read 50 Hình 2.26: Đặt cấu hình cho khối Encoder read 51 Hình 2.27: Khối đọc tốc độ động từ encoder 51 Hình 2.28: Sơ đồ hệ thống sử dụng điều khiển PID 53 Hình 2.29: Cài đặt cấu hình cho PWM chương trình điều khiển 54 Hình 2.30: Sơ đồ điều khiển PID 54 Hình 2.31: Sơ đồ hệ thộng sử dụng điều khiển Fuzzy-PID 55 Hình 2.32: Sơ đồ khối điều khiển Fuzzy-PID 55 Hình 2.33: Sơ đồ khối điều khiển PID 56 Hình 3.1 Sơ đồ khối hệ thống điều khiển động DC 58 Hình 3.2: Sơ đồ khối Query Instrument 58 Hình 3.3 Đặt cấu hình cho khối Query Instrument 59 Hình 3.4: Chương trình giám sát tốc độ Simulink 60 Hình 3.5: Màn hình giám sát tốc độ động điện chiều 60 Hình 3.6 : Đáp ứng tốc độ sử dụng điều khiển PID 61 Hình 3.7: Đáp ứng tốc độ sử dụng điều khiển Fuzzy_PID 62 vii Hoàng Đức Hạnh - CB120350 DANH MỤC BẢNG Bảng 1.1: Dữ liệu thu thập vào động Bảng 1.2: Luật điều khiển tập mờ 21 Bảng 1.3: Bảng so sánh chất lượng hệ thống điều khiển 25 Bảng 2.1: Chức chân L298 35 Bảng 2.2: Các chế độ làm việc L298 35 Bảng 2.3: Các chế độ hoạt động ARM 39 Bảng 2.4 Chức PWM 47 viii Hoàng Đức Hạnh - CB120350 CHƢƠNG 3: KẾT QUẢ THỰC NGHIỆM 3.1 Sơ đồ khối cấu trúc phần cứng Máy Tính RS2 Encoder STM32F4 Module Discovery điều khiển Hình 3.1 Sơ đồ khối hệ thống điều khiển động DC Nguyên lý thực điều khiển tốc độ động cơ: - Kit Vi Điều Khiển STM32F4 Discovery thực chương trình điều khiển tốc độ động với chương trình nạp mục 2.2.1 chương trình điều khiển dụng điều khiển PID mục 2.2.2 chương trình điều khiển PID định mờ - Encoder có nhiệm vụ phản hồi tốc độ động với thời gian trích mẫu 0.005s - Mudule điều khiển động sử dụng IC L298 với điện áp điều khiển từ vi điều khiển 3.3V- DC Điện áp cấp tải 9V-DC - Cổng giao tiếp RS232: Có nhiệm vụ truyền liệu từ Vi điều khiển lên Máy tính 3.2 Chƣơng trình giám sát hệ thống Với hỗ trợ Kit STM32F4 Chúng ta viết chương trình giám sát cho hệ thống thơng qua phần mềm Matlab –Simulink có đáp ứng thời gian thực Trong phần sử dụng tool Query Instrument Hình 3.2: Sơ đồ khối Query Instrument Khối Query Instrument có nhận liệu từ hệ thống, cấu hình khởi tạo thực thi lúc với q trình thực thi hệ thống 58 Hồng Đức Hạnh - CB120350 Khối Query Instrument khơng có ngõ vào có ngõ Với thời gian trích mẫu mặc định -1 Giá trị thời gian trích mẫu phụ thuộc vào chương trình nạp cho Vi điều khiền STM32F4 Hình 3.3 Đặt cấu hình cho khối Query Instrument Với chương trình giới thiệu chương trước Trong hộp thoại Query Instrument ta chọn thơng số sau: Thời gian trích mẫu: 0.005s Kích thức đệm: 512 bit Kiểu giao tiếp: Nối tiếp Cổng truyền thông: COM Tốc độ truyền 115200 bps Chương trình giám sát thơng qua Matlab – Simulink 59 Hồng Đức Hạnh - CB120350 Hình 3.4: Chƣơng trình giám sát tốc độ Simulink Màn hình giao diện sử dụng Scope Simulink Hình 3.5: Màn hình giám sát tốc độ động điện chiều 3.3 Kết thực nghiệm đáp ứng hệ thống điều khiển ổn định tốc độ động điều khiển PID Hệ thống phần cứng giới thiệu mục 3.1 Chương trình trình nạp từ mục 2.2.1 kết hợp với hỗ trợ tool Wajiung Blockset cho STM32F4 Discovery giám sát tốc độ động với đáp ứng thời gian thực Matlab thông qua Scope sau: 60 Hoàng Đức Hạnh - CB120350 Hình 3.6 : Đáp ứng tốc độ sử dụng điều khiển PID Nhận xét: Từ đáp ứng tốc độ đo động chiều 9V-DC ta thấy độ điều chỉnh thỏa mãn yêu cầu thiết kế với độ điều chỉnh nhỏ 10% 3.4 Kết thực nghiệm đáp ứng hệ thống điều khiển ổn định tốc độ động điều khiển PID định mờ - Hệ thống phần cứng giới thiệu mục 3.1 Chương trình trình nạp từ mục 2.2.2 Kết hợp với hỗ trợ tool Wajiung Blockset cho STM32F4 Discovery giám sát tốc độ động với đáp ứng thời gian thực Matlab thông qua Scope sau: 61 Hồng Đức Hạnh - CB120350 Hình 3.7: Đáp ứng tốc độ sử dụng điều khiển Fuzzy_PID Nhận xét: Kết từ thực nghiệm cho thấy hệ thống làm việc ổn định có độ điều chỉnh nhỏ so với điều khiển PID từ mục 2.2.1 62 Hoàng Đức Hạnh - CB120350 3.5 Tổng kết chƣơng Trong chương tác giả kiểm tra đáp ứng hệ thống với hai điều khiển PID PID định mờ.Cả hai điều khiển đề cho kết hệ thống ổn định đáp ứng u cầu tốn 63 Hồng Đức Hạnh - CB120350 KẾT LUẬN VÀ HƢỚNG PHÁP TRIỂN Đề tài “Nghiên cứu ứng dụng vi điều khiển STM32F4 điều khiển tốc độ động điện chiều” thực nhiệm vụ đặt ra, cụ thể là: - Tìm hiểu Toolbox Identification Matlab ứng dụng vào nhận dạng động điện chiều 10W phịng thí nghiệm - Tìm hiểu thuật tốn thiết kế điều khiển PID điều khiển PID chỉnh định mờ - Xây dựng điều khiển vi điều khiển STM32F4 ngoại vi cho động DC 10W - Cài đặt giải thuật điều khiển lập trình Matlab-Simulink vào vi điều khiển - Lắp đặt hệ thống kiểm chứng thực nghiệm kết thiết kế Bằng việc sử dụng cơng cụ Matab-Simulink để viết chương trình điều khiển nên cài đặt cho động dạng thuật tốn khác như: Mờ trượt, Điều khiển thích nghi, Điều khiển bền vững, v.v Ngồi ra, viết chương trình trực tiếp FPGA hệ thống có độ xác tính ổn định cao 64 Hoàng Đức Hạnh - CB120350 TÀI LIỆU THAM KHẢO [1] Nguyễn Doãn Phước (2007) Lý thuyết điều khiển tự động nâng cao, In lần thứ hai – có sửa đổi bổ sung, Nhà xuất Khoa học kỹ thuật, Hà nội [2] Nguyễn Doãn Phước, Phan Xuân Minh (2001), Nhận dạng hệ thống điều khiển, Nhà xuât Khoa học kỹ thuật, Hà Nội [3] Nguyễn Doãn Phước, Phan Xuân Minh , Lý thuyết điều khiển mờ, Nhà xuất khoa học kỹ thuật, Hà nội [4] Nguyễn Phùng Quang(2005), Matlab & Simulink, Nhà xuất khoa học kỹ thuật, Hà Nội [5] Nguyễn Dỗn Phước (2002), Lý thuyết điều khiển tuyến tính, Xưởng in ĐHTC Đại học Bách khoa Hà Nội [6] Hoàng Minh Sơn (2006), Cơ sở hệ thống điều khiển trình, Nhà Xuất Bản Bách Khoa Hà Nội [7] Nguyễn Quốc Trung, Xử lý tín hiệu lọc số 2, Nhà xuất khoa học kỹ thuật, Hà nội 65 Hoàng Đức Hạnh - CB120350 PHỤ LỤC /* Chương trình điều khiển sử dụng điều khiển PID * File: pid_code_1.c * * Created with Waijung Blockset * * Real-Time Workshop code generated for Simulink model pid_code_1 * * Model version : 1.15 * Real-Time Workshop file version : 7.3 (R2009a) 15-Jan-2009 * Real-Time Workshop file generated on : Sat Sep 20 16:34:44 2014 * TLC version : 7.3 (Jan 18 2009) * C/C++ source code generated on : Sat Sep 20 16:34:46 2014 * * Target selection: stm32f4.tlc * Embedded hardware selection: ARM Compatible->Cortex - M4 * Code generation objectives: Unspecified * Validation result: Not run */ #include "pid_code_1.h" #include "pid_code_1_private.h" /* Block signals (auto storage) */ BlockIO_pid_code_1 pid_code_1_B; /* Block states (auto storage) */ D_Work_pid_code_1 pid_code_1_DWork; /* Real-time model */ RT_MODEL_pid_code_1 pid_code_1_M_; RT_MODEL_pid_code_1 *pid_code_1_M = &pid_code_1_M_; static void rate_scheduler(void); /* * This function updates active task flag for each subrate * The function must be called at model base rate, hence the * generated code self-manages all its subrates */ static void rate_scheduler(void) { /* Compute which subrates run during the next base time step Subrates * are an integer multiple of the base rate counter Therefore, the subtask * counter is reset when it reaches its limit (zero means run) */ 66 Hoàng Đức Hạnh - CB120350 if (++pid_code_1_M->Timing.TaskCounters.TID[1] == 5) {/* Sample time: [0.005s, 0.0s] */ pid_code_1_M->Timing.TaskCounters.TID[1] = 0; } if (++pid_code_1_M->Timing.TaskCounters.TID[2] == 10) {/* Sample time: [0.01s, 0.0s] */ pid_code_1_M->Timing.TaskCounters.TID[2] = 0; } } /* Model step function */ void pid_code_1_step(int_T tid) { /* local block i/o variables */ real_T rtb_DiscreteTransferFcn4; real_T rtb_TSamp; real_T rtb_Diff; real_T rtb_Gain3_c; real_T rtb_TSamp_h; real_T rtb_Add3; real_T rtb_DiscreteTimeIntegrator; { real_T rtb_Gain1; real_T rtb_y_a; if (pid_code_1_M->Timing.TaskCounters.TID[2] == 0) { /* DiscreteTransferFcn: '/Discrete Transfer Fcn4' */ rtb_DiscreteTransferFcn4 = pid_code_1_P.DiscreteTransferFcn4_C* pid_code_1_DWork.DiscreteTransferFcn4_DSTATE; /* Embedded MATLAB: '/Embedded MATLAB Function' */ /* Embedded MATLAB Function 'Embedded MATLAB Function': ':1' */ if (rtb_DiscreteTransferFcn4 > 2.85) { /* ':1:3' */ /* ':1:4' */ rtb_y_a = rtb_DiscreteTransferFcn4; } else { /* ':1:6' */ rtb_y_a = 0.0; } /* Embedded MATLAB: '/Embedded MATLAB Function1' */ /* Embedded MATLAB Function 'Embedded MATLAB Function1': ':1' */ if (rtb_y_a > 100.0) { /* ':1:3' */ 67 Hoàng Đức Hạnh - CB120350 /* ':1:4' */ pid_code_1_B.y = 100.0; } else { /* ':1:6' */ pid_code_1_B.y = rtb_y_a; } /* S-Function Block: /Basic PWM (stm32f4_basicpwm) */ TIM1->CCR2 = (uint32_t) (pid_code_1_B.y * BasicPWM_SF); /* DiscreteTransferFcn: '/Discrete Transfer Fcn1' */ rtb_DiscreteTimeIntegrator = pid_code_1_P.DiscreteTransferFcn1_C* pid_code_1_DWork.DiscreteTransferFcn1_DSTATE; /* Gain: '/Gain3' */ rtb_y_a = pid_code_1_P.Gain3_Gain * rtb_DiscreteTimeIntegrator; /* Embedded MATLAB: '/Embedded MATLAB Function2' */ /* Embedded MATLAB Function 'Embedded MATLAB Function2': ':1' */ if (!(rtb_y_a > 0.0)) { /* ':1:6' */ rtb_y_a = 0.0; } else { /* ':1:3' */ /* ':1:4' */ } /* DataTypeConversion: '/Data Type Conversion1' */ pid_code_1_B.DataTypeConversion1 = (real32_T)rtb_y_a; /* UARTTx: '/UART Tx' */ { UART3_FlushTxBuffer(); /* Flush tx, so we can print to DMA buffer directly */ sprintf((char*)UART3_Tx_Buffer, "%1.5f\r\n" , pid_code_1_B.DataTypeConversion1 ); UART3_TxUpdate(strlen((char*)UART3_Tx_Buffer)); /* Blocking, wait for DMA buffer */ UART3_FlushTxBuffer(); } } if (pid_code_1_M->Timing.TaskCounters.TID[1] == 0) { /* S-Function Block: /Encoder Read (stm32f4_encoderread) */ pid_code_1_B.EncoderRead_o1 = (TIM3->CR1 & 0x10) >> 4; 68 Hoàng Đức Hạnh - CB120350 pid_code_1_B.EncoderRead_o2 = (uint32_t) TIM_GetCounter(TIM3); } if (pid_code_1_M->Timing.TaskCounters.TID[2] == 0) { /* DataTypeConversion: '/Data Type Conversion' */ rtb_DiscreteTimeIntegrator = (real_T)pid_code_1_B.EncoderRead_o2; /* S-Function (sfix_tsampmath): '/TSamp' */ /* Sample Time Math Block: '/TSamp' * * y = u * K where K = / ( w * Ts ) * * Input0 Data Type: Floating Point real_T * Output0 Data Type: Floating Point real_T * * Parameter: WtEt == Weighted Elapsed Time * Data Type: Floating Point real_T * */ rtb_TSamp = rtb_DiscreteTimeIntegrator * pid_code_1_P.TSamp_WtEt; /* Sum: '/Diff' incorporates: * UnitDelay: '/UD' */ rtb_Diff = rtb_TSamp - pid_code_1_DWork.UD_DSTATE; } if (pid_code_1_M->Timing.TaskCounters.TID[1] == 0) { /* ADC Block */ RegularADCStartConversion(); pid_code_1_B.RegularADC = (double)RegularADC_ConvertedValue[0]; /* Gain: '/Gain1' */ pid_code_1_B.Gain1 pid_code_1_B.RegularADC; } = pid_code_1_P.Gain1_Gain * if (pid_code_1_M->Timing.TaskCounters.TID[2] == 0) { /* Gain: '/Gain3' incorporates: * Sum: '/Subtract' */ rtb_Gain3_c = (pid_code_1_B.Gain1 - rtb_y_a) * pid_code_1_P.Gain3_Gain_h; /* Gain: '/Gain' */ rtb_y_a = pid_code_1_P.Gain_Gain * rtb_Gain3_c; 69 Hoàng Đức Hạnh - CB120350 /* DiscreteIntegrator: '/Discrete-Time Integrator' */ rtb_DiscreteTimeIntegrator = pid_code_1_DWork.DiscreteTimeIntegrator_DSTATE; /* Gain: '/Gain1' */ rtb_Gain1 = pid_code_1_P.Gain1_Gain_p * rtb_DiscreteTimeIntegrator; /* S-Function (sfix_tsampmath): '/TSamp' */ /* Sample Time Math Block: '/TSamp' * * y = u * K where K = / ( w * Ts ) * * Input0 Data Type: Floating Point real_T * Output0 Data Type: Floating Point real_T * * Parameter: WtEt == Weighted Elapsed Time * Data Type: Floating Point real_T * */ rtb_TSamp_h = rtb_Gain3_c * pid_code_1_P.TSamp_WtEt_m; /* Sum: '/Add3' incorporates: * Gain: '/Gain2' * Sum: '/Diff' * UnitDelay: '/UD' */ rtb_Add3 = (rtb_TSamp_h - pid_code_1_DWork.UD_DSTATE_n) * pid_code_1_P.Gain2_Gain + (rtb_y_a + rtb_Gain1); } if (pid_code_1_M->Timing.TaskCounters.TID[2] == 0) { /* DiscreteTransferFcn Block: '/Discrete Transfer Fcn4' */ { pid_code_1_DWork.DiscreteTransferFcn4_DSTATE = rtb_Add3 + pid_code_1_P.DiscreteTransferFcn4_A* pid_code_1_DWork.DiscreteTransferFcn4_DSTATE; } /* DiscreteTransferFcn Block: '/Discrete Transfer Fcn1' */ { pid_code_1_DWork.DiscreteTransferFcn1_DSTATE = rtb_Diff + pid_code_1_P.DiscreteTransferFcn1_A* pid_code_1_DWork.DiscreteTransferFcn1_DSTATE; } 70 Hoàng Đức Hạnh - CB120350 } if (pid_code_1_M->Timing.TaskCounters.TID[2] == 0) { /* Update for UnitDelay: '/UD' */ pid_code_1_DWork.UD_DSTATE = rtb_TSamp; } if (pid_code_1_M->Timing.TaskCounters.TID[2] == 0) { /* Update for DiscreteIntegrator: '/Discrete-Time Integrator' */ pid_code_1_DWork.DiscreteTimeIntegrator_DSTATE = pid_code_1_P.DiscreteTimeIntegrator_gainval * rtb_Gain3_c + pid_code_1_DWork.DiscreteTimeIntegrator_DSTATE; if (pid_code_1_DWork.DiscreteTimeIntegrator_DSTATE >= pid_code_1_P.DiscreteTimeIntegrator_UpperSat) { pid_code_1_DWork.DiscreteTimeIntegrator_DSTATE = pid_code_1_P.DiscreteTimeIntegrator_UpperSat; } else { if (pid_code_1_DWork.DiscreteTimeIntegrator_DSTATE