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:

Để đ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

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ì

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:

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).


‫ح‬
J = = 0.680272 ∗ 10
k
Phương pháp xác định J’,B’: 1
B = = 0.01088
k
∆w 73.5
k= = = 91.875
0.8 0.8
w=72.5(rad/s)

‫(7666.14=ح‬ms)
k = 90.625
=> J = 0.000459
B = 0.1103

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:


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:

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:

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);}

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ố.

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

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.

Đồ 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.

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;
up=sp*err;
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ể.

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ỏ.

ѡ 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.

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];

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

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;

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;

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':

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

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;

}

}

//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("%dn",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)

{

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;

}

else

{

n = (a*125)/100;

CCPR2L = n ;

TMR2ON=1;

}

return;

}

void forward()

{

dir=1;

ham=0;

}

void backward()

{

dir=0;

ham=1;

}

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

//vf=vf+3.18*0.02*(v-vf); //lọc nhiễu tần số thấp


up=sp*err;


//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;

}

Bao cao servo

More Related Content

What's hot (20)

Viewers also liked (20)

Similar to Bao cao servo (20)

Recently uploaded (20)

Bao cao servo