Hình 4-30: Kiến trúc phần mềm
Phần mềm điểu khiển được thiết kế theo dạng một vòng lặp vô hạn. Cứ mỗi 1 mili giây sẽ cập nhật lại giá trị các encoder và dữ liệu điều khiển một lần. Ngoài ra còn có 2 luồng ngắt chạy gần như song song dùng để đọc trạng thái encoder và giao tiếp với máy tính. Lưu đồ thuật toán hệ thống như sau:
Hình 4-31: Lưu đồ thuật toán chương trình điều khiển Chi tiết các bước chạy như sau:
- Khởi tạo hệ thống: Khởi tạo phần cứng, ngoại vi
- Khởi tạo các bộ điều khiển: Thiết lập các biến trạng thái, tham số của các bộ điều khiển
- Khởi tạo quỹ tạo: Khởi tạo chương trình tính toán quỹ đạo hình thang phục vụ việc điều chỉnh vị trí tham chiếu của xe khi thanh ở vị trí cân bằng
- Tính sai số vị trí từ các encoder: Vị trí của encoder được lưu bởi chương trình ngắt, sau đó được đọc ra bởi vòng lặp chính. Sai số được lấy bằng hiệu giá trị encoder hiện tại và giá trị đặt.
- Kiểm tra vị trí encoder ở miền làm việc nào. Nếu thanh thuộc vùng lật ngược Hình 4-2 thì chương trình sẽ thực hiện việc lật ngược thanh và ngược lại - Thực hiện các bộ điều khiển PD/PID: Tham khảo Mục 4.4.4
- Khâu tỉ lệ chuyển đổi điện áp: Từ giá trị PID tính được kết hợp với phương pháp PWM để tính ra độ rộng xung nhằm điều chỉnh điện áp cho động cơ. - Giải thuật tinh chỉnh sai số do khối tâm: Do thanh chế tạo và lắp ráp có sai số
nên khối tâm của thanh có thể không đúng nên cần điều chỉnh vị trí sai số này.