Đang tải... (xem toàn văn)
dành cho những người mới bắt đầu tiếp cận cho môn học điều khiển tự động ,sẽ nêu lên các bước cơ bản dễ hiểu nhất cho người đọc được hiểu ,từ đó tìm kiếm được mục tiêu về việc học cũng như các định hướng rõ ràng hơn về môn học,không chỉ cung cấp những kiến thức cơ bản mà còn giúp cho người đọc hình dung tốt hơn
Trích từ Luận Văn Thạc Sĩ – Tác giả: Nguyễn Gia Minh Thảo . Khoa Điện-Điện Tử, ĐH Bách Khoa TpHCM. 4.3. Phần mềm – Giải thuật chương trình cho mô hình robot thực nghiệm 4.3.1. Giải thuật chương trình chính Khởi đầu Cho phép tất cả các ngắt ở nhân chính và nhân X-Gate hoạt động Kiểm tra đã được 2,5ms chưa ? 1 Khởi động các khối chức năng Kiểm tra đã được 7,5ms chưa ? Có tín hiệu điều khiển RF ? 2 Sai Bộ điều khiển thăng bằng cho robot Cập nhật thông số của bộ lọc Kalman Sai Sai Đúng Đúng Đúng Đã được100ms? Hiển thị một số thông tin quan trọng của robot Thực hiện các chế độ hoạt động của robot (A) (B) (C) (D) Đúng Sai Trích từ Luận Văn Thạc Sĩ – Tác giả: Nguyễn Gia Minh Thảo . Khoa Điện-Điện Tử, ĐH Bách Khoa TpHCM. 4.3.2. Chi tiết trong các hàm con • Hàm con (A)-Khởi động các khối chức năng. Thực hiện theo trình tự sau: + Bộ PLL: Bus-Clock là 40 Mhz đối với lõi xử lý chính của vi điều khiển, 80 Mhz đối với lõi xử lý phụ X-Gate. +Timer OC2: Sử dụng ngắt để tạo khoảng thời gian lấy mẫu là 2,5ms. + Bộ SCI0: Tốc độ giao tiếp là 115200 Bit/s , có sử dụng ngắt khi nhận dữ liệu. + Bộ điều xung PWM: Tần số xung vuông là 10 Khz, tạo 4 kênh điều rộng xung từ PWM0 – PWM3 cho 2 mô-đun cầu H. + Bộ bắt giữ tín hiệu xung ngõ vào (TIC 6-7) để đọc tín hiệu phản hồi từ Encoder của động cơ bánh bên trái, có sử dụng ngắt. + Bộ bắt giữ tín hiệu xung ngõ vào (TIC 0-1) để đọc tín hiệu phản hồi từ Encoder của động cơ bánh bên phải, có sử dụng ngắt. +Khối chức năng Key-WakeUp: sử dụng ngắt để đọc giá trị phím được nhấn + Cho phép các IC cầu H (MC33932) được hoạt động, bằng cách cho các chân I/O được kết nối tương ứng lên mức cao. + Các chân I/O kết nối với mô-đun thu nhận tín hiệu điều khiển qua RF. Kiểm tra các nút nhấn trên mô - Hiển thị các chế độ đang hoạt động của robot trên các LED Thực hiện các chức năng từ nút nhấn Sai Đúng (E) 1 2 Trích từ Luận Văn Thạc Sĩ – Tác giả: Nguyễn Gia Minh Thảo . Khoa Điện-Điện Tử, ĐH Bách Khoa TpHCM. + Khối hiển thị: Khởi động khối hiển thị LCD 4x20 , các Led đơn hiển thị. + Khởi động lõi phụ X-Gate, và chuyển tất cả các ngắt : ngắt Timer OC2, ngắt nhận phím nhấn vào thực hiện trong lõi phụ X-Gate. • Hàm con (B)-Bộ điều khiển thăng bằng cho robot. Thực hiện theo trình tự sau: + Từ các giá trị thông số đã được cập nhật của bộ lọc Kalman, thực hiện tính toán ra giá trị góc nghiêng chính xác của thân robot. + Đọc giá trị xung phản hồi từ hai encoder, sau đó tính toán ra giá trị của tốc độ động cơ và vị trí hiện thời của robot. + Thực hiện giải thuật điều khiển vị trí robot theo phương pháp PID. + Thực hiện giải thuật điều khiển cuốn chiếu (ở các phương trình (3.36) (3.46) ) hay giải thuật điều khiển trượt (ở các phương trình (3.47)(3.55) ) để ổn định góc nghiêng thân robot. + Kết hợp hai tín hiệu điều khiển từ bộ điều khiển vị trí và bộ điều khiển góc nghiêng thân robot thành một tín hiệu điều khiển cuối cùng. + Xuất tín hiệu điều khiển cuối cùng (điều rộng xung) vào 2 mô-đun cầu H để điều khiển tốc độ, chiều quay của hai động cơ. • Hàm con (C) – Thực hiện các chế độ hoạt động của robot. + Trường hợp robot đứng thăng bằng tại chỗ thì: giá trị góc đặt cho thân robot là 0 [độ] và vị trí đặt cho robot là 0 [m]. + Trường hợp robot di chuyển về phía trước: giá trị góc đặt cho thân robot là góc dương (lớn hơn 0 [độ]). + Trường hợp robot di chuyển về phía sau: giá trị góc đặt cho thân robot là góc âm (nhỏ hơn 0 [độ]). Trích từ Luận Văn Thạc Sĩ – Tác giả: Nguyễn Gia Minh Thảo . Khoa Điện-Điện Tử, ĐH Bách Khoa TpHCM. + Trường hợp robot quay : điều khiển một động cơ đứng yên. Muốn cho động cơ quay từ trái sang phải thì ta giá trị góc đặt cho thân robot là góc dương (lớn hơn 0 [độ]). Muốn cho động cơ quay từ phải sang trái thì ta giá trị góc đặt cho thân robot là góc âm (nhỏ hơn 0 [độ]). • Hàm con (D) – Hiển thị một số thông tin quan trọng của robot + Giá trị góc nghiêng thân robot đo được + Giá trị các thông số của bộ điều khiển phi tuyến (cho góc nghiêng) và thông số của bộ điều khiển PD vị trí robot. + Giá trị offset của cảm biến vận tốc góc và offset của cảm biến gia tốc góc. + Hiển thị robot đang ứng yên, di chuyển về phía trước hay di chuyển về phía sau, hay quay tròn trên các Led đơn. • Hàm con (E) – Thực hiện các chức năng từ nút nhấn, gồm các chức năng sau: + Điều chỉnh giá trị offset cho cảm biến gia tốc góc và cảm biến vận tốc góc. + Điều chỉnh giá trị của các thông số bộ điều khiển phi tuyến (cho góc nghiêng thân robot) và bộ điều khiển PD vị trí robot. + Điều chỉnh giá trị góc nghiêng đặt cho robot. + Chọn các chế độ hoạt động của robot, gồm: đứng thăng bằng, di chuyển tiến-lùi, quay tròn. 4.3.3. Cách khai báo và quản lý các chương trình ngắt Các ngắt tạo thời gian lấy mẫu, ngắt từ nút nhấn đều được thiết lập để thực hiện ở lõi xử lý phụ X-Gate. Khi có tín hiệu ngắt xảy ra thì chương trình trong lõi X-Gate sẽ cho Trích từ Luận Văn Thạc Sĩ – Tác giả: Nguyễn Gia Minh Thảo . Khoa Điện-Điện Tử, ĐH Bách Khoa TpHCM. các cờ báo tương ứng (là biến đã được khai báo từ trước) lên mức 1. Chương trình ở lõi xử lý chính sẽ liên tục kiểm tra các cờ báo này để thực hiện những chức năng tương ứng. Việc chuyển các ngắt qua lõi phụ X-Gate xử lý và 2 lõi này cùng truy xuất được vào các biến thuộc vùng bộ nhớ khai báo dùng chung đã cải thiện đáng kể thời gian thực hiện chương trình của vi điều khiển trung tâm MC9S12XDP512. Điều đó góp phần tạo sự hoạt động ổn định của mô hình robot hai bánh tự cân bằng thực nghiệm. Trích từ Luận Văn Thạc Sĩ – Tác giả: Nguyễn Gia Minh Thảo . Khoa Điện-Điện Tử, ĐH Bách Khoa TpHCM. Khởi đầu Khởi động ngắt Timer3 ,với chu kỳ ngắt nhỏ nhất là 2.5ms Có ít nhất một tín hiệu ngắt xảy ra? Ngắt Timer, đã đủ 2.5ms ? Sai Đúng Sai Đúng Ngắt từ 1 phím được nhấn ? Cho cờ báo đủ 2.5ms lên mức 1. Đúng Sai Khởi động ngắt từ nút nhấn (Key-wake up) Chuyển hai ngắt ở trên qua thực hiện ở lõi xử lý phụ (lõi X-Gate) Cho cờ báo có phím đ ượ c nhấn lên mức 1 Thực hiện ở lõi xử lý phụ: X-Gate Thực hiện ở lõi xử lý chính . chế độ hoạt động của robot. + Trường hợp robot đứng thăng bằng tại chỗ thì: giá trị góc đặt cho thân robot là 0 [độ] và vị trí đặt cho robot là 0 [m]. + Trường hợp robot di chuyển về phía. thân robot) và bộ điều khiển PD vị trí robot. + Điều chỉnh giá trị góc nghiêng đặt cho robot. + Chọn các chế độ hoạt động của robot, gồm: đứng thăng bằng, di chuyển tiến-lùi, quay tròn. 4. 3.3 trí hiện thời của robot. + Thực hiện giải thuật điều khiển vị trí robot theo phương pháp PID. + Thực hiện giải thuật điều khiển cuốn chiếu (ở các phương trình (3.36) (3 .46 ) ) hay giải thuật