14- Tổng quát về bản thuyết minh:
3.3.2. Thiết kế chƣơng trình nhúng
Chƣơng trình nhúng trên MCU sử dụng tối đa chức năng của các ngoại vi hardware trên STM32F4 cho việc truyền dữ liệu, đọc cảm biến, timing để hạn chế ngắt dành tối đa hiệu suất của bộ xử lý cho giải thuật điều khiển.
ID V_PUT_LEFT DMA INPUT PROCESSING CONTROL & PUBLISHING CMD ID L_LEFT L_RIGHT USART RX CMD V_PUT_RIGHT RX Not Empty ENCODER COUNT SPEED scheduler V_LEFT V_RIGHT TIME STAMP
DMA USART RX TX Not Empty CPU WORKLOAD
Các tính năng của STM32F4 đƣợc khai thác gồm có:
- Vi điều khiển ARM Cortex M4 với tốc độ 168MHz, bộ xử lý số thực FPU và timer hệ thống System Tick đảm bảo tính real time cho hệ thống khi tần số lấy mẫu của bộ điều khiển vòng kín là 200Hz ( ).
- USART với DMA: Giao tiếp giữa node serial server và MCU thông qua các frame dữ liệu tối giản. Frame truyền từ máy tính có nội dung là CMD – mode điều khiển, ID – số nhận diện đƣợc lặp lại trong frame phản hồi từ MCU để đồng bộ và V_PUT_LEFT, V_PUT_RIGHT là các số nguyên có dấu mang vận tốc đặt trên mỗi bánh, đơn vị là số đếm encoder mỗi giây. Frame phản hồi từ MCU lặp lại giá trị CMD và ID và các giá trị trạng thái của từng bánh nhƣ quãng đƣờng đi đƣợc, vận tốc trên từng bánh và time stamp do kernel của hệ điều hành thời gian thực freeRTOS quản lý. Ngoại vi USART của STM32F4 cho phép truyền dữ liệu theo chuẩn RS232 với tốc độ baud lên đến hàng Mbps và đƣợc kết nối với bộ DMA. Cơ chế này tự động lấy dữ liệu thu đƣợc về vùng nhớ RAM của các biến, hay từ vùng nhớ chứa các biến về ngoại vi. CPU chỉ cần truy cập các biến và xử lý trong giải thuật, hoàn toàn không dùng chƣơng trình ngắt để lấy dữ liệu nhận đƣợc từ thanh ghi USART RX hay gửi dữ liệu tới USART TX.
- Timer với chức năng Encoder Interface – Input Capture – PWM Generator: STM32F4 có 14 bộ timer, chƣa kể các bộ timer WatchDog và SysTick với cấu trúc phức tạp có tốc độ cao. Hầu hết timer đều đa chức năng, có thể đƣợc thiết đặt thành một trong ba ngoại vi điều khiển động cơ. Với một MCU STM32F4, mỗi động cơ có một timer giải mã encoder, một timer capture tốc độ và một timer để phát xung PWM. Cho phép ta thiết đặt đầy đủ các chức năng điều khiển động cơ để thu thập thông tin về đƣờng đi, tốc độ và điều khiển tự động và độc lập. Việc giải mã encoder hoàn toàn tự động và chƣơng trình chỉ cần truy cập tới các thanh ghi hoạt động của Timer để lấy thông tin về trạng thái của các động cơ, hoàn toàn không dùng tới ngắt.
Hình 3.32: Sơ đồ khối của Timer điều khiển nâng cao (Advanced-control Timer) của STM32F4
Encoder Interface: Cho phép ta thiết lập bộ giải mã xung encoder 2 kênh. Từ đó có tể biết đƣợc chuyển động tuyệt đối cùng chiều quay của động cơ. Với phƣơng pháp giải mã tổng quát nhƣ hình dƣới, bộ giải mã encoder có thể phân biệt đƣợc cạnh lên và xuống trên mỗi kênh A,B. Từ đó tăng số lƣợng xung đếm đƣợc lên đến 4 lần. Với động cơ planet 13 xung, hệ số bánh răng là 19.2 và bộ giải mã có thể nhân bốn, có số xung mỗi vòng ở trục ra động cơ là 1000 xung, đảm bảo điều khiển chính xác động cơ.
Hình 3.33: Giản đồ xung giải mã quadrature encoder của Encoder Interface trên STM32F4
Input Capture: Bộ Timer /Counter trên STM32F4 có thể đƣợc thiết đặt với chức năng đo thời gian giữa các xung encoder, cho phép ta đo trực tiếp tốc độ động cơ. Đầu vào của bộ Timer/Counter cho phép ta XOR các tín hiệu từ kênh A và kênh B của encoder, làm tăng gấp đôi độ phân giải của tốc độ. Lợi ích thực tế của việc này là, khi robot có tốc độ chậm, sự kiện capture có thể không xảy ra trong chu kỳ lấy mẫu, làm cho giải thuật điều khiển hiểu nhầm thành tốc độ bằng không. Việc tăng độ phân giải làm tăng số lần capture trên mỗi đơn vị thời gian, giúp cho việc điều khiển ổn định hơn.
PWM Generator: Bộ Timer /Counter có thể đƣợc thiết đặt làm bộ phát xung PWM có thể đảo chiều động cơ bằng cách điều khiển mỗi nửa cầu riêng biệt bằng hai trong bốn ngõ ra của bộ PWM. Khi giá trị đặt đảo dấu, ngõ ra hiện thời bị bất hoạt và ngõ ra còn lại đƣợc kích hoạt.
Hình 3.34: Khối XOR ngõ vào giúp tăng đôi tần số encoder và các ngõ ra PWM giúp điều khiển đảo chiều động cơ
CHƢƠNG IV – LẬP TRÌNH TÍCH HỢP NAVIGATION STACK TRÊN MÔ HÌNH ROBOT
Để Navigation Stack hoạt động, ta cần phải thiết kế các node đặc thù hỗ trợ cho Navigation Stack cho mô hình robot trong đề tài. Chƣơng sau đây sẽ đề cập các thao tác lập trình thực tế để ứng dụng Navigation Stack.
Xem thêm phụ lục 1 về cách tạo một package mới trong ROS và sử dụng Môi trƣờng Phát triển Tích hợp (Integrated Development Environment – IDE) để lập trình cho ROS.