1. Thiết kế mạch điều khiển động cơ DC-Encoder
1.2. Đọc Encoder bằng phương pháp ngắt ngoài
1.2.1. Giới thiệu về Encoder
Hệ thống optical encoder bao gồm một nguồn phát quang (thường là hồng ngoại – infrared), một cảm biến quang và một đĩa có chia rãnh. Optical encoder lại được chia thành 2 loại:
Encoder tuyệt đối (absolute optical encoder) và encoder tương đối (incremental optical encoder). Trong đa số các DC Motor, incremental optical encoder được dùng và
32
mô hình động cơ servo trong bài này cũng không ngoại lệ. Từ bây giờ khi nói encoder tức là incremental encoder.
Hình 3.4. Optical Encoder
Encoder thường có 3 kênh (3 ngõ ra) bao gồm kênh A, kênh B và kênh I (Index). Trong hình lỗ nhỏ bên phía trong của đĩa quay và một cặp phat-thu dành riêng cho lỗ nhỏ này. Đó là kênh I của encoder. Cữ mỗi lần motor quay được một vòng, lỗ nhỏ xuất hiện tại vị trí của cặp phát-thu, hồng ngoại từ nguồn phát sẽ xuyên qua lỗ nhỏ đến cảm biến quang, một tín hiệu xuất hiện trên cảm biến. Như thế kênh I xuất hiện một “xung” mỗi vòng quay của motor. Bên ngoài đĩa quay được chia thành các rãnh nhỏ và một cặp thu- phát khác dành cho các rãnh này. Đây là kênh A của encoder, hoạt động của kênh A cũng tương tự kênh I, điểm khác nhau là trong 1 vòng quay của motor, có N “xung” xuất hiện trên kênh A. N là số rãnh trên đĩa và được gọi là độ phân giải (resolution) của encoder. Mỗi loại encoder có độ phân giải khác nhau, có khi trên mỗi đĩa chĩ có vài rãnh nhưng cũng có trường hợp đến hàng nghìn rãnh được chia. Để điều khiển động cơ, phải biết độ phân giải của encoder đang dùng. Độ phân giải ảnh hưởng đến độ chính xác điều khiển và cả phương pháp điều khiển. Không được vẽ trong hình , tuy nhiên trên các encoder còn có một cặp thu phát khác được đặt trên cùng đường tròn với kênh A nhưng lệch một
33
chút (lệch M+0,5 rãnh), đây là kênh B của encoder. Tín hiệu xung từ kênh B có cùng tần số với kênh A nhưng lệch pha 90o. Bằng cách phối hợp kênh A và B người đọc sẽ biết chiều quay của động cơ. Hãy quan sát hình 3.
Hình 3.5. Hai kênh A và B lệch pha trong encoder (trích từ [1])
Hình trên cùng trong hình 3 thể hiện sự bộ trí của 2 cảm biến kênh A và B lệch pha nhau. Khi cảm biến A bắt đầu bị che thì cảm biến B hoàn toàn nhận được hồng ngoại xuyên qua, và ngược lại. Hình thấp là dạng xung ngõ ra trên 2 kênh. Xét trường hợp motor quay cùng chiều kim đồng hồ, tín hiệu “đi” từ trái sang phải. Quan sát lúc tín hiệu A chuyển từ mức cao xuống thấp (cạnh xuống) thì kênh B đang ở mức thấp. Ngược lại, nếu động cơ quay ngược chiều kim đồng hồ, tín hiệu “đi” từ phải qua trái. Lúc này, tại cạnh xuống của kênh A thì kênh B đang ở mức cao. Như vậy, bằng cách phối hợp 2 kênh A và B chúng ta không những xác định được góc quay (thông qua số xung) mà còn biết được chiều quay của động cơ (thông qua mức của kênh B ở cạnh xuống của kênh A).
Có 3 cách đọc Encoder :
34
- Dùng chức năng Counter - Cuối cùng là ngắt ngoài
1.2.2. Ngắt ngoài trong Arduino
Ngắt (interrupt) là những lời gọi hàm tự động khi hệ thống sinh ra một sự kiện. Những sự kiện này được nhà sản xuất vi điều khiển thiết lập bằng phần cứng và được cấu hình trong phần mềm bằng những tên gọi cố định.
Ngắt giúp chương trình gọn nhẹ và xử lý nhanh hơn. Chẳng hạn, khi kiểm tra 1 nút nhấn có được nhấn hay không, thông thường bạn cần kiểm tra trạng thái nút nhấn bằng hàm digitalRead() trong đoạn chương trình loop(). Với việc sử dụng ngắt, bạn chỉ cần nối nút nhấn đến đúng chân có hỗ trợ ngắt, sau đó cài đặt ngắt sẽ sinh ra khi trạng thái nút chuyển từ HIGH->LOW. Thêm 1 tên hàm sẽ gọi khi ngắt sinh ra. Vậy là xong, biến trong đoạn chương trình ngắt sẽ cho ta biết trạng thái nút nhấn.
Số lượng các ngắt phụ thuộc vào từng dòng vi điều khiển. Với Arduino Uno bạn chỉ có 2 ngắt, Mega 2560 có 6 ngắt và Leonardo có 5 ngắt.
Cú pháp:
attachInterrupt(interrupt, ISR, mode);
Thông số
interrupt: Số thứ tự của ngắt. Trên Arduino Uno, bạn có 2 ngắt với số thứ tự là 0 và 1. Ngắt số 0 nối với chân digital số 2 và ngắt số 1 nối với chân digital số 3.
ISR: tên hàm sẽ gọi khi có sự kiện ngắt được sinh ra. mode: kiểu kích hoạt ngắt, bao gồm
LOW: kích hoạt liên tục khi trạng thái chân digital có mức thấp HIGH: kích hoạt liên tục khi trạng thái chân digital có mức cao.
RISING: kích hoạt khi trạng thái của chân digital chuyển từ mức điện áp thấp sang mức điện áp cao.
35
FALLING: kích hoạt khi trạng thái của chân digital chuyển từ mức điện áp cao sang mức điện áp thấp.
Lưu ý: với mode LOW và HIGH, chương trình ngắt sẽ được gọi liên tục khi chân digital còn giữ mức điện áp tương ứng.
void setup() {
...
attachInterrupt(0, dao, FALLING); ...
}
void dao() // chương trình con ngắt, khi có ngắt xảy ra sẽ thực hiện chương trình này.
{
c=digitalRead(2); //chan ngat -kenh A b=digitalRead(13); //kenh B
if (c==b) a=a-1; // tru bot xung A (nghich)
else a=a+1; // cong them xung cua A (thuan) }
1.3. Ngắt Timer (để tính PID)
Trong Arduino có thư viện Timer.h phục vụ cho ngắt timer. Mục đích của ngắt timer là định sẵn một khoảng thời gian và đếm ngược đến hết thời gian đó timer sẽ thực hiện lệnh hoặc gọi chương trình con...
#include <Timer.h> Timer t0;
36 int time=0.025; void setup() { ... t0.every(time*1000, pid); }
The call void loop() { t0.update();
... }
void pid() {...}
t0.update() // cập nhật thời gian
t0.every sẽ gọi hàm pid sau mỗi 25ms .
1.4. Nguyên tắc điều khiển động cơ
Nguyên tắc chung:
Hình 3.7: Nguyên tắc điều khiển động cơ Công thức tính PID: 1 0 (e ) . . k k k p D I k e u K e K K e h h
37
h[s] thời gian lấy mẫu; k -lần tính thứ k;
. Để đơn giản hóa việc tính thành phần tích phân, chúng ta nên dùng phương pháp “cộng dồn” (hay đệ quy):
(k) (k 1) Ie(k) h
I I K
Tùy thuộc hàm truyền mà ta chọn dùng PID, PI, I...
Từ đó ra sẽ có sơ đồ nguyên tắc thiết kế điều khiển động cơ
Hình 3.8: Sơ đồ điều khiển
Ngắt ngoài khi chân 2 có tín hiệu xuống 1->0 liên tục gửi tín hiệu khi động cơ quay. So sánh kênh A và B để biết chiều thuận nghịch tương ứng q - hay +.
(1)Hàm tính U điều khiển (0-255) xuất ra chân 9; Hàm chỉ được gọi sau mỗi 25ms qua ngắt timer
T=0.025 s : thời gian lấy mẫu. Thời gian lấy mẫu càng nhỏ (tấn số cao) thì việc hiệu chỉnh càng tiến gần đến sự “liên tục” và chất lượng điều khiển sẽ tốt hơn. Trong các bộ
38
điều khiển số, thời gian lấy mẫu là một yếu tố rất quan trọng. Cần tính toán để thời gian này không quá lớn nhưng cũng đừng quá nhỏ, vì như thế sẽ hao phí thời gian thực thi. Code tính PID:
void pid() {
qnew=a; // dat goc moi = a if(menu==0){
t=t+time; // tang thoi gian dem enew=x*sin(y*t+1.57)-a; //e(k)
ipart+=0.05*(enew-eold)*time; //I(k) U=0.21*enew+ipart; }
if(menu==1) {
enew=z-abs(qold-qnew); //enew sai so van toc z xung dat tuong ung voi van to dat U+=0.21*enew; //ki=021; U chinh la I(k)
v=(qnew-qold); } // van toc dang xung in ra man hinh qold=qnew; // dat vi tri cu = vi tri moi cho vong lap moi eold=enew; // dat sai so cu = sai so moi cho vong lap moi dem++;
if (U<0) {U=-U; dau=1;
} else dau=0;
if (U>255) U=255; //Chan can tren if(U<90 && menu==0 && abs(enew)>5)U=90; //Chan can duoi }
39
Ý nghĩa đoạn lệnh:
Đoạn lệnh là chương trình con sẽ được gọi bởi timer sau mỗi 25 ms (thời gian lấy mẫu) Đầu ra của nó là U (0-255) trên chân 9.Có 2 menu: Menu=0 điều khiển vị trí với đầu vào là góc (xung) đo được sau khi đọc encoder; menu=1 điều khiển vận tốc với đầu vào là vận tốc góc (xung/0.025s) đo được sau khi đọc encoder. U được tính theo các công thức PID đã đề cập.
1.5. Thiết kế mạch trên Proteus
Sau khi thực hiện được lập trình cho mạch trên Arduino IDE ta có thể chạy thử xem mạch hoạt động như thế nào trên phần mền Proteus
Phần mền hoạt động khá đơn giản, ta chỉ việc kích vào biểu tượng P và đánh ra tên linh kiện trong khung Keywords và kích đúp vào để đưa linh kiện ra bên ngoài
40
Hình 3.10: Cách lấy linh kiện Từ đó chúng ta thiết kế mạch như trong hình vẽ dưới đây: Các chân được đánh số từ 1 đến 9 như trong hình vẽ
41
Hình 3.12: Các chân nối với LCD
42
Hình 3.14: Các đường nối của động cơ và L293D
2. Thiết kế giao diện điều khiển bằng Megunolink 2.1. Giới thiệu về Megunolink 2.1. Giới thiệu về Megunolink
Megunolink là phần mền cho phép nhận và gửi dữ liệu Serial Data.
Nó hỗ trợ thiết bị có sử dụng các dòng nối tiếp qua USB, RS-232 hoặc UDP được hỗ trợ bao gồm các Arduino, PIC, mbed, Beagle, và tem vi điều khiển cơ bản.
MegunoLink cung cấp cho bạn cách để giảo tiếp bộ điều khiển và máy tính của bạn theo những cách hữu ích và thú vị.Bạn cũng có thể gửi các lệnh để điều khiển điện thoại của bạn với các nút mà tạo ra các thông điệp hoặc bằng cách gõ vào một cửa sổ màn hình. Và có chức năng như:
Vẽ : hiển thị dữ liệu của bạn bằng đồ họa
Bảng điều khiển giao diện : nhanh chóng xây dựng một giao diện đơn giản để kiểm soát
hay cấu hình vi điều khiển của bạn
43
Bàn thông tin cấu hình báo cáo trong một dễ dàng để đọc các bảng:
Lập trình : chương trình tải lên tạo ra trong Microsoft Visual Studio, hoặc các công cụ
khác, trực tiếp đến Arduino vi điều khiển của bạn
Đăng nhập vào tập tin : lưu dữ liệu vào một tập tin để xem xét sau
Quản lý kết nối : làm việc với nhiều thiết bị nối tiếp cùng một lúc
2.2. Các bước thiết kế giao diện Megunolink
Phần mềm giúp ta tự xây dựng các thành phần giao diện theo ý muốn: Các mode:
Hình 3.16: Các mode
Các mode đã sử dụng: Connection manager; interface panel; time plot Mode Conection manager: Quản lí kết nối giữa máy tính và arduino
44
Hình 3.17: Khối kết nối
Mode Interface panel: Giao diện nhập số liếu đầu vào để điều khiển động cơ Mode khi chưa thiết kế
45
Hình 3.18: Mode Interface panel Ấn design xuất hiện cửa sổ design:
46
Hình 3.19: Cửa sổ thiết kế Thử kéo ra 1 button
47
Ví dụ:
Tạo ô nhập có tên menu:
Hình 3.21: Tạo ô menu
-Tạo một nút bấm bên cạnh nó có chứa lệnh kết nối trong ô “on click send”
Hình 3.22: Tạo nút bấm
Thư viện serial command handler chứa các lệnh cho phép khởi động 1 chức năng khi nhận lệnh tữ giao diện. Mặc định lệnh bắt đầu bằng “!” Và kết thúc bằng “\r”
Code arduino:
#include "MegunoLink.h" #include "CommandHandler.h"
CommandHandler<> SerialCommandHandler; //Khai báo kết nối int menu = 3; //biến menu ban đầu =3
void datmenu(CommandParameter &Parameters) //chương trình con datmenu {
menu=Parameters.NextParameterAsInteger(); //menu=[menu.value] }
48
void setup() {
Serial.begin(9600); //Khai báo tốc độ truyền
SerialCommandHandler.AddCommand(F("setmenu"), datmenu); //Gọi CT con khi click nút bấm
... }
void loop() {
SerialCommandHandler.Process(); //Kiểm tra tín hiệu giao tiếp giữa máy tính và arduino
... }
Đoạn chương trình có ý nghĩa:
Ta nhập 1 số vào ô nhập có tên name sau đó click nút bấm bên cạnh sẽ kích động gọi chương trình con datmenu và gán cho biến menu=value trong ô nhập menu trên giao diện Mode time plot: Hiển thị dưới dạng đồ thị góc quay và vận tốc động cơ theo thời gian
49
Hình 3.23: Mode time plot Code Arduino
#include "MegunoLink.h"
TimePlot MyPlot("goc"); //Tên đồ thị void setup() { ... Serial.begin(9600); //kết nối MyPlot.SetTitle("goc"); //Tiêu đề MyPlot.SetXlabel("thoigian"); MyPlot.SetYlabel("goc");
MyPlot.SetSeriesProperties("goc", Plot::Blue, Plot::Solid, 2, Plot::Square); ...
}
50
{ ...
MyPlot.SendData(F("goc"),a*0.8); //gửi dữ liệu cho trục “goc” ...
}
Sắp xếp các mode trong giao diện bằng cách kéo thả sử dụng chuột:
Hình 3.24: Giao diện thiết kế
51
Lượng đặt: d 400vg / ph
Hình 3.25: Điều khiển vận tốc Lượng đặt: d 100deg
52
Hình 3.26: Điều khiển vị trí Lượng đặt: d 150sin(250t90) deg
53
Chương 4: Mô phỏng tay máy 1DOF
1. Thiết lập hệ phương trình vi phân
Hình 4.1: Cơ cấu tay máy -Tay máy:
Khối lượng m; khối tâm C OC=l; momen quán tính với trục quay O: J; hệ số cản ổ trục tay máy b2
-Động cơ DC:
Điện trở Ra; cuộn cảm L; hằng số phản sức điện động Ke; hằng số momen Km; Momen quán tính rôto Jm; Hệ số cản trục động cơ b1
54
Hình 4.2: Sơ đồ Điện - Cơ *Thiết lập:
-Số bậc tự do: n=2 (1 bậc cơ và 1 bậc điện)
-Tọa độ suy rộng: q q -Động năng: 2 2 1 2 2 2 2 2 1 2 2 1 1 2 2 w 1 1 1 (J J w ) (J J ) (J ) 2 2 2 1 1 (J ) 2 2 e m m m m tg m e m tg T Li Lq r T r T T T Lq -Thế năng: sin m mgl -Hàm tán năng:
55 2 2 2 2 2 2 2 1 2 1 1 1 1 1 (b r b ) r b 2 2 2 2 0, m e a a R q R q b b b
-Lực suy rộng của các lực không thế: U; Vemf ; M (Momen điện trên trục động cơ)
1 ( ) emf e m e m e m A U q V q M U q K w q K i U q K r q K rq U K r q K rq
-Phương trình lagrang II:
* * q d T T Q dt d T T Q dt q q q q Jm cos tg m a e b mgl K qr Lq R q U K r
Bỏ qua sự thay đổi dòng điện: (t) q(t) 0 q e a d d L i L dt dt U K r R => Ta có PTVP: 2 1 2 1 (JJ rm ) (b K K Re m a )r mglcosK rR Um a 2. Mô phỏng bằng Simulink a.Hệ mở 1 1 2 2 1 ( ( ) cos ) m a e m a m K rR U b K K R r mgl J J r
56
Điều kiện đầu: 0; 0
%dong co
Ke=1;Km=1;Ra=1;Jm=0.001;
%tay may
m=2;J=1;l=0.4;g=9.81; b=0.001;
%hop giam toc
r=2;
Hình 4.3: Xây dựng thuật toán tên Simulink Kết quả:
57 Hình 4.4: Kết quả (rad) Hình 4.5: Kết quả (rad/s) b.Hệ phản hồi Vị trí:
58
Hình 4.6: Xây dựng thuật toán tên Simulink Chọn Kp=400;Ki=10;Kd=100
Hình 4.7: Phản hồi về vị trí Vận tốc:
59
Hình 4.8: Xây dựng thuật toán tên Simulink Chọn Kp=160;Ki=10
Hình 4.9: Phản hồi vận tốc
60
Hình 4.10:Xây dựng thuật toán tên Simulink Chọn Kp=400;Ki=10;Kd=100
Hình 4.11: Điều khiển bám quỹ đạo
61 a.Hệ mở 1 1 2 2 1 ( ( ) cos ) m a e m a m K rR U b K K R r mgl J J r
Điều kiện đầu: 0; 0
*bai6jj.m function xdot=baijj6(t,x); %dong co Ke=1;Km=1;Ra=1;Jm=0.001; %tay may m=2;J=1;l=0.4;g=9.81; b=0.001;
%hop giam toc
r=2; V=24; xdot=zeros(2,1); xdot(1)=x(2); xdot(2)=(1/(J+Jm*r^2))*((Km*r/Ra)*V-((b+Km*Ke/Ra)*r^2)*x(2)+m*g*l*cos(x(1))); end *Vẽ: [t,x]=ode45('bai6jj',[0:0.1:10],[0,0]); clf; figure(1);
plot(t,x(:,1)),xlabel('t[s]'),ylabel('q[rad]'), figure(2);
62
plot(t,x(:,2)),xlabel('t[s]'),ylabel('w[rad]'),
Hình 4.12: Kết quả điều khiển vị trí (rad)
63 b.Hệ phản hồi *Vị trí: function xdot=bai6jj(t,x); %dong co Ke=1;Km=1;Ra=1;Jm=0.001; %tay may m=2;J=1;l=0.4;g=9.81; b=0.001;
%hop giam toc
r=2; Kp=6000;Kd=50; V=-Kp*(x(1)-1.57)-Kd*(x(2)); xdot=zeros(2,1); xdot(1)=x(2); xdot(2)=(1/(J+Jm*r^2))*((Km*r/Ra)*V-((b+Km*Ke/Ra)*r^2)*x(2)+m*g*l*cos(x(1))); End In kết quả: [t,x]=ode45('bai6jj',[0:0.1:10],[0,0]); clf; figure(1);
plot(t,x(:,1)),xlabel('t[s]'),ylabel('q[rad]'), figure(2);
64 Hình 4.14: Kết quả vị trí *Vận tốc: function xdot=bai6jj(t,x); %dong co