Bao cao servo

Mục lục

Lời mở đầu

Lời cảm ơn

Chương 1:Thiết kế bộ điều khiển PID số……………………………………5

1.1/Thiết kế mô hình phần cứng mạch điều khiển………………………….5

1.1.1/Yêu cầu thiết kế……………………………………………………….…5

1.1.2/Các khối chức năng trên mạch điều khiển………………………………,5

a)Khối vi điều khiển trung tâm………………………………………………..5

b)Khối giao tiếp với máy tính thông qua RS232……………………………...6

c)Khối động lực điều khiển…………………………………………………...7

1.2/Thiết kế phần mềm……………………………………………………….8

1.2.1/Yêu cầu phần mềm……………………………………………………...8

1.2.2/Giải thuật chương trình………………………………………………....8

a)Loop điều khiển………………………………………………………….…8

b)Thuật toán điều khiển……………………………………………………...10

1.2.3/ Thiết kế phần mềm điều khiển và giám sát trên máy tính………….…10

1.3/Kit điều khiển…………………………………………………………....11

Chương 2: Điều khiển động cơ điện 1 chiều……………………………….12

2.1/Đối tượng điều khiển

2.1.1/Thông số kỹ thuật……………………………………………………... 12

2.2/Xác định các thông số động cơ:

2.2.1/Nhận dạng mô hình động học của động cơ điện một chiều……………15

2.2.2/Thu thập dữ liệu vào/ra của động cơ điện một chiều từ thực nghiệm….21

2.2.3/Nhận diện động cơ một chiều bằng Toolbox Identification của Matlab..23

2.2.4/Đánh giá chất lượng mô hình….….……………………………………..28

2.3/Điều khiển vận tốc…………………...…………………………………….28

2.3.1/Loop điều khiển………………….….…………………………………...28

2.3.2/Giải thuật chương trình…………….….…………………………………29

2.3.3/Ảnh hưởng của ѡ đến hệ thống…………………………………… …30

2.4/Điều khiển vị trí……………………………………………………… ….43

2.4.1/Loop điều khiển……………………………………………………… ..43

2.4.2/Giải thuật chương trình……………………………………………… …43

2.4.3/Ảnh hưởng của ѡ đến hệ thống………………………………………..44

Lời mở đầu

Khoa học công nghệ hiện đại đã có những bước tiến nhanh và xa đi theo đó là những thành tựu ứng dụng trong mọi lĩnh vực dời sống, công nghiệp. Kĩ thuật điều khiển trong tiến trình hoàn thiện lý thuyết cũng tạo cho mình nhiều phát triển có ý nghĩa. Bây giờ khi nhắc tới điều khiển con người dưòng như hình dung đến sự chính xác, tốc độ xử lý và thuật toán thông minh đồng nghĩa là lượng chất xám cao hơn.

Có thể nói trong lĩnh vực điều khiển và trong công nghiệp thì bộ điều khiển PID có ứng dụng kha rộng rãi, một giả pháp đa năng cho các ứng dụng cả Analog cũng như Digital. Thống kê cho thấy có tới hơn 90% các bộ điều khiển sử dụng trong thực tế là PID. Rõ ràng nếu có thiết kế và chọn lựa các thông số hợp lý cho bộ điều khiển PID thì việc đạt được các chỉ tiêu chất lượng mong muốn là khả thi Bộ điều khiển PID cũng giúp người sử dụng dễ dàng tích hợp cũng như chọn các luật điều khiển như : tỉ lệ(P), tích phân(I), tỉ lệ tích phân(PI), tỉ lệ vi phân(PD)… sao cho phù hợp đối với các đối tượng điều khiển. Nhiều quá trình trong công nghiệp việc sử dụng bộ điều khiển PID là không thể thay thế như khống chế nhiệt độ, mức, tốc độ…? Ngay cả những lý thuyết điều khiển hiện đại cũng không cho ta những hiệu quả cao như bộ điều khiển PID mang lại.Ngoài ra bộ điều khiển PID còn ứng dụng nhiều trong điều khiển thích nghi,bền vững vẫn mang lại hiệu quả cao trong các cơ cấu chỉnh định.

Bài toán thiết kế và điều khiển động cơ một chiều là bài toán cơ bản và quen thuộc trong ngành cơ điện tử. Có thể thiết kế điều khiển cho đối tượng độngcơ điện một chiều theo nhiều phương pháp như :dùng PLC & biến tần, điện tử công suất, vi điều khiển… Mỗi phương pháp có ưu và nhược điểm khác nhau nhưng đều có mục đích ổn định và điều khiển được tốc độ động cơ. Ngày nay vi điều khiển phát triển sâu rộng và ngày càng ứng dụng nhiều trong cài đặt thiết kế bộ điều khiển cho các đối tượng công nghiệp. Trên cơ sở muốn tìm hiểu về lĩnh vực điều khiển tự động chúng em chọn đề tài: Thiết kế bộ điều khiển PID số trên nền vi điều khiển PIC. Vì khả năng và thời gian có hạn nên không thể tránh khỏi những thiếu sót trong project. Do vậy chúng em rất mong được thầy cô và bạn bè đóng góp xây dựng để đồ án của chúng em được hoàn thiện hơn.

Lời cảm ơn

Đầu tiên chúng em xin chân thành gửi lời cảm ơn tới nhà truờng, khoa Cơ Khí Máy, bộ môn Cơ Điện tử, các thầy cô đã dạy dỗ và dìu dắt chúng em trong suốt 3 năm học vừa qua để có được những kiến thức chuyên môn cơ sở sau này chúng em có thể vào đời làm việc, sử dụng có ích cho xã hội.

Để thực hiện thành công project là sự hướng dẫn, chỉ bảo tận tình của TS Trương Nguyễn Vũ, Người đã hướng dẫn tận tình , giúp chúng em định hướng, góp ý và cung cấp ý tưởng cũng như chỉ dẫn tài liệu và các tiến trình thực hiện project .Sự hướng dẫn của thầy là một yếu tố quan trọng để chúng em có thể hoàn thành project này .

Cuối cùng chúng em xin chân thành gửi những lời cảm ơn sâu sắc đến cha mẹ và gia đình, những người luôn sát cánh cùng chúng em, nuôi dưỡng chăm sóc chúng em tạo điều kiện tốt nhất cho chúng em học tập để có kết quả như ngày hôm nay.

Chúng em xin chân thành cảm ơn!

Chương1:Thiết kế bộ PID số

Chương 1:Thiết kế bộ PID số 1.1/Thiết kế mô hình phần cứng mạch điều khiển

1.1.1/Yêu cầu thiết kế:

-Tốc độ tính toán nhanh.

-Có khả năng giao tiếp với:

+Máy tính.

+Các thiết bị chấp hành:Cảm biến,động cơ.

1.1.2/Các khối chức năng trên kit điều khiển:

a/Khối vi điều khiển trung tâm


b/Khối giao tiếp với máy tính


c/Khối động lực điều khiển-Sử dụng L298D-Thay thế mạch cầu H

L298D Datasheet:


1.2/Thiết kế phần mềm:

1.2.1/Yêu cầu thiết kế:

-Nhận dữ liệu từ máy tính và bàn phím.

-Truyền dữ liệu máy tính lên thông qua cổng truyển thông RS232.

-Tính toán đầu ra cho bộ điều khiển thiết kế,trong đề tài là bộ PID số.

1.2.2/Giải thuật chương trình.

a)Loop điều khiển


1.2.3/ Thiết kế phần mề m điều khiển và giám sát trên máy tính.

-Thu nhận dữ liệu do người dùng nhập vào.

-Phân tích xử lý dữ liệu,truyền xuống cho vi điều khiển.

-Nhận dữ liệu từ vi điều khiển và vẽ đồ thị.


1.3/Kit điều khiển

Chương 2 : Điều khiển động cơ điện 1 chiều

Chương 2:Điều khiển động cơ điện 1 chiều

2.1/Đối tượng điều khiển:

2.1.1/Thông số kỹ thuật:

Động cơ được sử dụng trong project

Chúng ta sử dụng Servo Motor với các thông số cơ bản sau:

-Điện áp nguồn tối đa:12V

-Tốc độ tối đa 850(vòng/phút).

-Có gắn liền với encoder quang tương đối 200 xung.

-Công suất 30-50W.

Encoder:

Chương 2 : Điều khiển động cơ điện 1 chiều Để điều khiển số vòng quay hay vận tốc động cơ thì chúng ta nhất thiết phải đọc được góc quay của motor. Một số phương pháp có thể được dùng để xác định góc quay của motor bao gồm tachometer (thật ra tachometer đo vận tốc quay), dùng biến trở xoay, hoặc dùng encoder. Trong đó 2 phương pháp đầu tiên là phương pháp analog và dùng optiacal encoder (encoder quang) thuộc nhóm phương pháp digital. 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à mô hình động cơ servo trong bài này cũng không ngoại lệ. Từ bây giờ khi tôi nói encoder tức là incremental encoder. Hình 2 là mô hình của encoder loại này.

Hình 2. Optical Encoder (trích từ [1]).

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 2 bạn thấy hãy chú ý một 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

Chương 2 : Điều khiển động cơ điện 1 chiều 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ơ, bạn 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 2, 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 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. 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. Bạn hãy 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ì

Chương 2 : Điều khiển động cơ điện 1 chiều 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).

2.2/Xác định thông số J’,B’ của động cơ:

2.2.1/Nhận dạng mô hình động học của động cơ điện một chiều

Cấu trúc mô hình động học được đề xuất tham khảo các tài liệu điều khiển tự động có dạng:

Cơ sở lý thuyết:

a. Khảo sát trong miền thời gian:


Chương 2 : Điều khiển động cơ điện 1 chiều Các biểu thức trên cho thấy đặc tính thời gian của khâu dao động bậc hai có dạng dao động tắc dần. Hàm quá độ suy giảm về giá trị xác lập K và hàm trọng lượng suy giảm về 0. Giá trị ؏ càng lớn, dao động suy giảm càng nhanh, do đó ؏ gọi là hệ số suy giảm hay hệ số tắt dần.


b. Khảo sát trong miền tần số:



2.2.2/ Thu thập dữ liệu vào/ra của động cơ điện một chiều từ thực nghiệm

a/Xác định các thông số J’,B’ thông qua đồ thị:


Đồ thị trên thể hiện vận tốc của động cơ theo thời gian tuân theo qui tắc:

-Động cơ chạy với 80% PWM trong thời gian 3s sau đó dảo chiều.

-Thời gian lấy mẫu là 20(ms).


Phương pháp xác định J’,B’:

Vì đáp ứng của động cơ ở phần chạy thuận và chạy nghịch là gần như giống nhau nên ta tính 2 thông số J’,B’.Nếu như đáp ứng chạy thuận và chạy nghịch của động cơ là khác nhau ,chúng ta phải tìm 4 thông sô J’,B’ và J”,B”.

Mô phỏng Matlab:

J =حk

= 0.680272 ∗ 10

B =1k

= 0.01088

k =∆w0.8

=73.50.8

= 91.875

w=72.5(rad/s)

(ms)41.6667=ح

=>k = 90.625

J = 0.000459B = 0.1103


Kết quả mô phỏng

2.2.3/Nhận diện động cơ một chiều bằng Toolbox Identification của Matlab:

Chương 2 : Điều khiển động cơ điện 1 chiều Xuất số liệu từ mô phỏng ra bảng excel: Số liệu từ thực nghiệm:


Mô phỏng dựa vào số liệu thực:

Import số liệu vào Matlab:

Mở ứng dụng Identification:

>>ident


Đưa dữ liệu vào:


Chương 2 : Điều khiển động cơ điện 1 chiều 2.2.4/Đánh giá chất lượng mô hình:

Tính toán:

>>sysc=d2c(pss1,'zoh'); >>step(sysc); >>[a,b,c,d]=ssdata(sysc); >>[num,den]=ss2tf(a,b,c,d); >>ss=tf(num,den); >>step(sysc);

2.3/Điều khiến vận tốc:

2.3.1/Loop điều khiển:


Thay số Kp,Ki vào mô phỏng và xem xét đáp ứng hệ thống.

퐾 = 2ѡ 퐽 − 퐵

퐾 = 퐽′ѡ

=> 퐾푝 = 0.0138퐾푖 = 0.1836

2.3.2/Giải thuật chương trình

Thuật toán điều khiển

{v=(count*3.14/2)/2;

err = setpoint - v;

up=kp*err;

ui=ui + ki*err*0.02;

pid = up + ui ;

if(pid>=256.0)pid=256.0;

if(pid<=0.0)pid=0.0;

pwm((int)pid);}

Chương 2 : Điều khiển động cơ điện 1 chiều 2.3.3/Ảnh hưởng của wn đến hệ thống

->Ứng với mỗi ѡ khác nhau hệ số Kp,Ki khác nhau .

Bộ điều khiển PI với ѡ푛=20


Thời gian đáp ứng vận tốc nhanh hơn so với wn=20


Bộ điều khiển PI với wn=40

Cho động cơ chạy với vận tốc 20(rad/s)->100(rad/s)->20(rad/s). Tốc độ đáp ứng nhanh hơn nhưng độ nhiễu tăng lên.



Cho động cơ chạy với vận tốc 100(rad/s)->50(rad/s).

Bộ điều khiển PI với ѡ푛==100

Cho động cơ chạy với tốc độ 100(rad/s).


Tốc độ đáp ứng tăng lên khá nhiều và chưa bị vọt lố.

Bộ điều khiển PI với ѡ푛==120

Tốc độ đáp ứng của động cơ nhanh nhưng bị vọt lố,động cơ dao động nhẹ.

Chương 2 : Điều khiển động cơ điện 1 chiều Bộ điều khiển PI với ѡ푛==150

Tốc độ đáp ứng của động cơ rất nhanh nhưng bị vọt lố mạnh,động cơ dao động mạnh.

Bộ điều khiển PI với ѡ푛==200 Đồ thị vận tốc có dạng dao động tắt dần kiều bậc 2,tốc độ đáp ứng nhanh nhưng độ vọt lố cao. Kết luận: Càng tăng ѡ푛 tốc độ đáp ứng của động cơ càng nhanh nhưng đồng thời độ vọt lố cũng tăng theo. Giải thích: Với cách chọn hệ số tắt dần ؏=1 hàm truyền của hệ có dạng:

G(s)=ѡ ѡ

G(t)=1-푒 ѡ푛푡-ѡ푛t.푒 ѡ푛푡

Chương 2 : Điều khiển động cơ điện 1 chiều Hệ có nghiệm kép:-ѡ푛 =>Cặp cực này luôn nằm phần âm của trục thực =>Đáp ứng của hệ thông không có dao động. ѡ푛 cảng tăng->hàm G(t) càng nhanh tiến về 1 ->Nhanh đạt được tốc độ tương ứng.

Mô hình nhiễu trong điều khiển động cơ:

Nhiễu là các biến mà nó không chứa trong mô hình hệ thống nhưng ảnhhưởng đến đáp ứng của hệ thống. Chúng có thể được xác định, chẳng hạn nhưmômen tải trong hệ thống điều khiển vị trí, cũng như các nhiễu từ các cảm biến hay từ cơ cấu chấp hành.


Điều khiển động cơ với bộ PI chưa có lọc nhiễu:

Bộ điều khiển PI với vận tốc động cơ 30rad/s

Tốc độ càng thấp càng dễ ảnh hưởng bởi nhiễu. Độ mịn của đồ thị ít(có nhiều răng cưa).


Bộ điều khiển PI với vận tốc động cơ 50rad/s

a. Điều khiển động cơ với bộ PI có lọc nhiễu tần số thấp: i. Chương trình code:

v=(count*3.14/2);

vf=vf+3.18*0.02*(v-vf);

err = setpoint - vf;

up=sp*err;

ui=ui + si*err*0.02;

pi = up + ui ;

if(pi>=256.0)pid=256.0;

if(pi<=0.0)pid=0.0;

pwm((int)pi);


ii. Đồ thị vận tốc :

Bộ điều khiển PI có bộ lọc nhiễu với vận tốc V=100rad/s

Tốc độ càng cao thì càng ít bị ảnh hưởng nhiễu của tần số thấp .

Qua đồ thị ta có thể thấy ở tốc độ này,những “răng cưa”(nhiễu) đã nhỏ lại rất nhiều

Vì thế độ mịn của đồ thị tăng lên.


Bộ điều khiển PI có bộ lọc nhiễu với vận tốc V=50rad/s

Độ răng cưa đã giảm đáng kể,khi động cơ đạt được tốc độ định sẵn thì các nhiễu tần số nhỏ gần như được lọc bỏ hoàn toàn.

b. Điều khiển động cơ với bộ PI có lọc nhiễu giá trị trung bình: i. Chương trình code:

{ vf=(vf+count*3.14/2)/2; err = setpoint - vf; up=sp*err; ui=ui + si*err*0.02; pi = up + ui ; if(pi>=256.0)pi=256.0; if(pi<=0.0)pi=0.0; pwm((int)pi); }


ii. Đồ thị vận tốc :

Bộ điều khiển PI có bộ lọc nhiễu trung bình với vận tốc ѡ =30rad/s

Ở tốc độ này cũng với cách lọc nhiễu trung bình,mặc dù đã được loại bỏ nhưng động cơ vẫn bị ảnh hưởng nhiều.

Do đó,độ mịn của đồ thị còn rất thấp.



Chạy ở tốc độ cao hơn,động cơ ít bị ảnh hưởng bởi nhiễu hơn do đó độ mịn của đồ thị được tăng lên.



Chạy ở tốc độ cao động cơ ít chịu ảnh hưởng của nhiễu vì thế độ răng cưa đã giảm bớt đáng kể.

Chương 2 : Điều khiển động cơ điện 1 chiều 2.4/Điều khiển vị trí

2.4.1/Loop điều khiển

2.4.2/Giải thuật chương trình

Thuật toán điều khiển:

Các hệ số:

K2 = 2J‘ѡ –B’

K1 k2 =J‘ѡ 2

퐽 = 0.00068027퐵 = 0.01088

Giải thuật:

v=count*3.14/2.0; vf=vf+3.18*0.02*(v-vf); err = setpoint - post; u2=k2*(k1*err-vf); u2=abss(u2); CCPR2L=(int)u2;


2.4.3/Ảnh hưởng của ѡ đến hệ thống

ѡ =50

Cho động cơ chạy tới vị trí 100(rad)-Đáp ứng:60(rad)


Cho động cơ chạy ngược tới vị trí 80(rad)-Đáp ứng:120(rad)

Hệ thống chỉ đáp ứng được với những vị trí có góc >=60(rad),còn đối với những góc nhỏ hơn thì không thể đáp ứng được.

Xung PWM được điều chế không đủ điện áp cho động cơ chạy với những góc nhỏ

Sai số kéo về nằm trong khoảng(-2;2)(rad).->Sai số khá lớn.


ѡ =80




Hệ thống có thể đáp ứng được với góc nhỏ khoảng 30(rad).

Sai số kéo về trong khoảng (-2,2)(rad).


ѡ =100







Nhưng sai số kéo về vẫn chưa được cải thiện(-2;2)(rad).


ѡ =200



Đáp ứng của động cơ rất nhanh.

Động cơ có thể quay được với các góc nhỏ hơn 10(rad)

Sai số kéo về được cải thiện nằm trong khoảng (-1,1)(rad).


ѡ =250




Động cơ đáp ứng rất nhanh tớsi vị trí đặt trước.

Với những vị trí có góc càng lớn thì độ chính xác càng cao.

Sai số kéo về nằm trong khoảng(-0.5;0.5)(rad)

Vẫn chưa khắc phục được sai số ở các vị trí có số đo góc thấp.

Kết luận:

ѡ càng nhỏ hệ thống đáp ứng càng lâu,đồng thời không đáp ứng được những vị trí có góc nhỏ.

Chương 2 : Điều khiển động cơ điện 1 chiều ѡ nhỏ sai số càng lớn.

ѡ càng lớn,thời gian đáp ứng nhanh,sai số nhỏ đồng thời đáp ứng được các vị trí có góc nhỏ.

Giải thích:

Khi ѡ nhỏ,các hệ số K1 và K2 nhỏ nên tín hiệu hồi tiếp về cho vi điều khiển cấp xung PWM nhỏ nên không đủ điện áp cho động cơ chạy.

Ngoài ra,động cơ có độ “trì hoãn” ban đầu nên ít nhiều đã ảnh hưởng đến sai số.

Một nguyên nhân khác có thể là các hệ số J’,B’-rất quan trọng-chúng ta tìm chưa chính xác hoặc chưa hợp lý.

Muốn điều khiển chính xác các vị trí có góc nhỏ chúng ta cũng cần có thuật toán lọc nhiễu,xác định mô hình toán của động cơ thật chính xác đồng thời cách điều chế xung PWM cũng phải hợp lý,chính xác và nhanh.

Chương 2 : Điều khiển động cơ điện 1 chiều Phụ lục code phần mềm:

Điều khiển vận tốc:

#include<htc.h>

__CONFIG(FOSC_HS & WDTE_OFF & PWRTE_ON & MCLRE_ON & CP_OFF & BOREN_OFF & IESO_OFF & FCMEN_OFF & LVP_OFF & DEBUG_OFF); //1st config. Word

__CONFIG(BOR4V_BOR21V); //2st config. Word

#ifndef _XTAL_FREQ

#define _XTAL_FREQ 20000000

#endif

#include<stdio.h>

#include<string.h>

#include<ctype.h>

#include<stdlib.h>

#include<conio.h>

#define dir RE0

#define ham RE1

unsigned int count,setpoint,v,vf,k;

char x;

char data;

bit tx;

char send;

char chuoi[20];

Chương 2 : Điều khiển động cơ điện 1 chiều char i;

void ram();

int pwm(int a);

double abss(double a);

void forward();

void backward();

void brake();

void velocity();

void reset();

void delstr(void);

void guikytu(char c);

void guichuoi(const char* s);

// Khai bao bo PID

int err;

double u1,pid, ui,up,u;

float q;

//Nhap kp,ki tai day

float sp;//=0.070748;

float si;//;=2.44897;

float sd;//=0.1;

//Chuong trinh chinh

Chương 2 : Điều khiển động cơ điện 1 chiều void main()

{

ANSEL=ANSELH=0;

TRISE0=TRISE1=0;

dir=0;

ham=0;

//Khoi tao PWM

TRISC2=1;

TRISC1=0;

T2CKPS1=0;

T2CKPS0=0;

PR2=124;//Tao xung PWM 10kHz

CCP2CON = 0X0C;

DC2B0 = 0;

DC2B1 = 0;

TMR2ON=1;

//Thiet lap UART

TXSTA = 0x24;

RCSTA = 0x90;

BRG16 = 1; //chon baund rate 20MhZ/(20*(520+1))~9600

SPBRGH = 2;

Chương 2 : Điều khiển động cơ điện 1 chiều SPBRG = 8;

RCIF=0;

RCIE=1;

PEIE=1;

GIE=1;

//THIET LAP TIMER0 LAM COUNNTER

TRISA4=1;

T0CS=1;

T0SE=0;

PSA=1;

TMR0=0;

//NGAT TIMER0

T0IE=1;

T0IF=0;

//THIET LAP TIMER1 LAM TIMER DINH THOI GIAN LAY MAU 20ms

TMR1CS=0;

T1CKPS0=1;

T1CKPS1=0;

T1SYNC=1;

//T1OSCEN=1;

TMR1L=0Xb0;

TMR1H=0X3c;

Chương 2 : Điều khiển động cơ điện 1 chiều TMR1ON=1;

//THIET LAP NGAT TIMER1

TMR1IF=0;

TMR1IE=1;

tx=0;

while(1)

{

}

}

//Chuong trinh ngat

void interrupt isr()

{

//Ngat Uart

if(RCIE&&RCIF)

{

RCIF=0;

data=RCREG;

switch(data)

{

//setpoint

case 'v':

Chương 2 : Điều khiển động cơ điện 1 chiều setpoint=atoi(chuoi);

i=0;

delstr();

break;

//cho phep Send

case 't':

tx=1;

RE1=1;

RE0=0;

break;

//stop

case 's':

brake();

pwm(0);

break;

//gui kp

case 'p':

sp=atof(chuoi);

i=0;

delstr();

break;

//gui ki

Chương 2 : Điều khiển động cơ điện 1 chiều case 'i':

si=atof(chuoi);

i=0;

delstr();

break;

//gui kd

case 'd':

sd=atof(chuoi);

i=0;

delstr();

break;

case 'z':

reset();

break;

default:

chuoi[i++]= data;

break;

}

}

Chương 2 : Điều khiển động cơ điện 1 chiều //Ngat T1 10ms

if(TMR1IE&&TMR1IF)

{

TMR1IF=0;

TMR1ON=0;

TMR1L=0Xb0;

TMR1H=0X3c;

TMR1ON=1;

count=x*256+TMR0;

x=0;

TMR0=0;

velocity();

//ram();

if(tx)

{

//k++;

printf("%d\n",vf);

}

}


//Ngat T0

if(T0IE&&T0IF)

{

T0IF=0;

x++;

}

}

//Chuong trinh con

void guikytu(char c)

{

while(TXIF==0);

TXREG=c;

}

void guichuoi(const char* s)

{

while(*s){

guikytu(*s++);

}

}

void putch (char c)

Chương 2 : Điều khiển động cơ điện 1 chiều {

guikytu(c);

}

// Ham de xoa chuoi sau khi da nhan dc gia tri

void delstr(void)

{

for(char i=20; i>0;i--)

{

chuoi[i]='\0';

}

return;

}

//Dieu che PWM tu PID

int pwm(int a)

{

int n;

if (a>=100)

{

n=100;

CCPR2L=125;

TMR2ON=1;

Chương 2 : Điều khiển động cơ điện 1 chiều }

else

{

n = (a*125)/100;

CCPR2L = n ;

TMR2ON=1;

}

return;

}

void forward()

{

dir=1;

ham=0;

}

void backward()

{

dir=0;

ham=1;

}

Chương 2 : Điều khiển động cơ điện 1 chiều void brake()

{

ham=1;

pwm(0);

TMR2ON=0;

pid=0;

}

double abss(double a)

{

double b;

if (a<0){

b=-a;

}

else {

b=a;

}

return b;

}

void velocity()

{

vf=count*3.14/2;

//vf=(vf+count*3.14/2)/2; // lọc nhiễu trung bình

Chương 2 : Điều khiển động cơ điện 1 chiều //vf=vf+3.18*0.02*(v-vf); //lọc nhiễu tần số thấp

err = setpoint - vf;

up=sp*err;

ui=ui + si*err*0.02;

//der=(err-errold)/0.02;

pid = up + ui ;

pid=abss(pid);

if(pid>=256.0)pid=256.0;

if(pid<=0.0)pid=0.0;

pwm((int)pid);

}

void reset()

{

setpoint=v=count=TMR0=x=0;

sp=si=0;

err=0;

pid=0.0;

dir=0;

ham=0;

}


Education

Bao cao servo