D. GIẢI THUẬT ĐIỀU KHIỂN
3. Giao tiếp giữa máy tính, master, slave
Hình 5.9: Mô hình thực nghiệm giao tiếp
Mô hình thực nghiệm giao tiếp giữa máy tính với master bằng chuẩn truyền RS232. Sau đó, master sẽ giao tiếp với slave bằng chuẩn SPI để điều khiển một động cơ quay thuận, quay nghịch và dừng.
Kết luận:
- Mô hình hoạt động tốt.
46
KẾT LUẬN
Đồ án môn học đã hoàn thành được một số mục tiêu cơ bản đề ra ban đầu đó là điều khiển được sự co duỗi chân của robot, điều khiển vận tốc theo hai chiều tiến và lùi, thiết kế được mach điện điều khiển, thiết lập được giải thuật điều khiển định tâm, tính toán lực bám thành ống cho robot di chuyển trong các ống nghiêng khác nhau.
Tuy nhiên, đề tài vẫn còn nhiều hạn chế chưa giải quyết được đó là chưa chế tạo thực nghiệm sự định tâm của một cụm đĩa nên chưa kiểm tra tính đúng đắn của giải thuật điều khiển định tâm.
47
Tài liệu tham khảo
[1] Nguyễn Hữu Lộc, Cơ sở thiết kế máy, NXB ĐHQG Tp. Hồ Chí Minh, 2011. [2] Ninh Đức Tốn, Dung sai lắp ghép, NXB Giáo dục, 2010.
48
PHỤ LỤC 1. Giải thuật điều khiển vị trí
Khi đầu vào điều khiển là vị trí, yêu cầu đưa ra là điều khiển đạt được vị trí mong muốn trong một khoảng thời gian nào đó và ổn định tại giá trị này. Trong trường hợp này ta cũng có thể áp dụng giải thuật PID để đạt được yêu cầu điều khiển đưa ra.
Start
Vị trí hiện tại:
x_real
Tính:
err = x_real – x_desired p_part = kp*err
d_part = kd*(err - pre_err)/h i_part += ki*err*h
pre_err = err
out = p_part + d_part + i_part
out > duty_max out = duty_max out < - duty_max out = - duty_max out > 0 duty = out dir = 1 duty = -out dir = 0 Yes No Yes Yes Vị trí mong muốn: x_desired No No
49
Hình 6.1: Điều khiển vị trí
Giải thuật đọc vào vị trí mong muốn x_desired. Giải thuật cập nhật giá trị hiện tại x_real trong mỗi lần tính giá trị điều khiển của bộ PID.
Động cơ được điều khiển theo độ rộng xung PWM, nghĩa là tốc độ động cơ tỷ lệ với giá trị duty cấp vào động cơ, chiều quay động cơ được set theo biến dir.
Khi giá trị bộ PID tính được vượt quá giá trị duty lớn nhất duty_max , ta giới hạn duty cấp vào động cơ chính bằng giá trị duty_max này.
2. Giải thuật điều khiển lực bám
Giá trị lực bám được chuyển về giá trị số adc_desired. Lực bám thực tế được bộ chuyển đổi tương tự-số ADC chuyển thành giá trị adc_real. Giá trị này sẽ được dùng để điều khiển lực bám theo giải thuật sau đây.
Start adc_real Dừng động cơ Yes Yes |adc_real- adc_desired|<30 Chạy thuận adc_real > adc_desired adc_real < adc_desired Chạy nghịch Yes No No adc_desired
50
3. Giải thuật điều khiển vận tốc
Mục tiêu điều khiển là: vận tốc động cơ đạt được giá trị mong muốn và ổn định ở giá trị này, ta có thể áp dụng giải thuật PID số.
Start
Tốc độ hiện tại:
v_real
Tính:
err = v_real – v_desired p_part = kp*err
d_part = kd*(err - pre_err)/h i_part += ki*err*h
pre_err = err
out = p_part + d_part + i_part
out > duty_max out = duty_max out < - duty_max out = - duty_max duty += out Yes Yes Tốc độ mong muốn: v_desired
Hình 6.3: Giải thuật điều khiển vận tốc
Giải thuật đọc vào giá trị vận tốc mong muốn v_desired. Giải thuật cập nhật giá trị hiện tại v_real trong mỗi lần tính giá trị điều khiển của bộ PID.
Động cơ được điều khiển theo độ rộng xung PWM, nghĩa là tốc độ động cơ tỷ lệ với giá trị duty cấp vào động cơ.
51
Khi giá trị bộ PID tính được vượt quá giá trị duty lớn nhất duty_max , ta giới hạn duty cấp vào động cơ chính bằng giá trị duty_max này.