Con lắc ngược dùng atmega32

Trường ĐH Công Nghiệp Hà Nội Đề Tài Con Lắc Ngược
Nhóm Đề Tài Con Lắc Ngược
 Giáo Viên Hướng Dẫn: Nhữ Quý Thơ
 Nhóm Sinh Viên Thực Hiện:
• Vũ Khắc Trung
• Lê Minh Sáng
• Lê Đức Trọng
• Đặng Quốc Toản
• Trần Xuân Thanh
• Lưu Ngọc Bình
• Mai Văn Thành
• Trần Khắc Vinh
Page 1 of 32
Trường ĐH Công Nghiệp Hà Nội Đề Tài Con Lắc Ngược
1.GIỚI THIỆU VỀ ĐỀ TÀI
Trong thời kỳ công nghệ đang trên đà phát triển mạnh mẽ thì thành công của công
nghệ là yếu tố rất quan trọng của mỗi đất nước. Chỉ có nền khoa học kĩ thuật tiên tiến
mới giúp đất nước đó có khả năng khẳng định vị thế của mình. Nhằm giảm bớt sức lao
động của con người và nâng cao năng suất lao động thì công nghệ là yếu tố không thể
thiếu. Hiện nay với sự giúp đỡ của máy tính và các loại robot trong công nghiệp đã phần
nào giải quyết được những yêu cầu của xã hội. Robot đã giúp đỡ con người rất nhiều
trong rất nhiều lĩnh vực như công nghiệp , y tế , nghiên cứu, giải trí…nhưng vẫn còn
đang rất hạn chế bởi vì chúng không thể tự di chuyển mà thường cố định ở một nơi. Vì
vậy một trong những vấn đề đặt ra là làm sao có thể giúp robot có thể giữ thăng bằng và
có thể đi lại được giống như con người. Đây là một bài toán đang cần tìm ra lời giải đáp.
Bài toán cơ bàn trong dạng này là bài toán về con lắc ngược. Nó bao gồm có một con lắc
có khối lượng ở trên điểm trục của nó và điểm trục còn lại được gắn trên xe có thể
chuyển động theo chiều ngang hoặc một bàn trượt dọc. Vấn đề là làm sao có thể phối hợp
giữa con lắc và chuyển động của xe (bàn trượt) sao cho đỉnh của con lắc luôn hướng lên
trên. Có thể nói đây là một bài toán rất hay về điều khiển và có rất nhiều ứng dụng trong
thực tế.

2.Ý TƯỞNG , SƠ ĐỒ KHỐI LƯU ĐỒ THUẬT TOÁN
Để giải quyết bài toán về thăng bằng này trước hết ta cần phải lấy được giá trị góc
lệch của con lắc so với phương thẳng đứng cùng với vận tốc hiện tại của động cơ để có
thể đưa ra đáp ứng đầu ra điều khiển bàn trượt chuyển động theo hướng và vận tốc để giữ
thăng bằng cho con lắc. Để lấy được giá trị góc lệch của con lắc ở đây chúng em dùng
enconder tuyệt đối có độ phân giải là 1024 nhằm xác định vị trí cũng như góc lệch của
con lắc với vị trí thẳng đứng. Từ những thông số đo được sẽ được đưa vào vi điều khiển
Page 2 of 32
Trường ĐH Công Nghiệp Hà Nội Đề Tài Con Lắc Ngược
để sử lý sau đó vi điều khiển sẽ đưa ra tín hiệu điều khiển , điều khiển động cơ chuyển
động sao cho đáp ứng của góc lệch pha là nhỏ nhất để có thể giữ thăng bằng con lắc.
3. GIỚI THIỆU VỀ SƠ ĐỒ CẤU TẠO VÀ NGUYÊN TẮC HOẠT
ĐỘNG CỦA MỘT SỐ LINH KIỆN ĐƯỢC SỬ DỤNG

+>Bộ mã hóa vòng quay tuyệt đối (Absolute rotate encoder) :
Lloại EP50S8-1024-1-R-N-5 đây là loại enconder có đường kính 50mm, độ phân
giải 1024, mã ngõ ra dạng BCD , giá trị ngõ ra tăng khi quay ngược chiều kim đồng hồ ,
ngõ ra điều khiển NPN collector hở và điện áp cấp vào là 5V

Page 3 of 32
Tín hiệu thu
được từ ecoder
Vi điều khiển Drive DS810
Động cơ
Visual Basic
Vị trí con lắc
Trường ĐH Công Nghiệp Hà Nội Đề Tài Con Lắc Ngược
360 độ chia ( ngõ ra BCD)
Sơ đồ ngõ ra điều khiển
Sơ đồ kết nối

Page 4 of 32
Trường ĐH Công Nghiệp Hà Nội Đề Tài Con Lắc Ngược
+> Digital DC Servo Driver-DCS810:
DCS810 là một bộ điều khiển số của động cơ secvo DC. Trong việc điều khiển vị
trí,người sử dụng dễ dàng thay đổi các nấc điều khiển cho DCS810 mà không phải thay
đổi hệ thống điều khiển,bởi vì lệnh đầu vào của nó là tín hiệu PUL/DIR tương ứng với
các nấc điều khiển.Trong các ứng dụng điều khiển chuyển động nguồn thấp,các hệ thống
Servo DC làm việc tốt hơn các hệ thống Servo AC ở vận tốc,độ chính xác,nhiễu,độ ổn
định hay ít nhất là tốt hơn các hệ thống Servo AC số
Một số đặc tính :
• 18-80VDC,0-20V,20-400W.
• Dựa trên kỹ thuật điều khiển DSP và thuật toán điều khiển servo với độ
mượt cao.
• Các công cụ điều chỉnh thông số,bao gồm Pro Tuner,EZ Tuner và STU.
• Độ phân giải : 4 x độ phân giải encoder.
• Sai lệch vị trí giới hạn trong 1 lần đếm, và sai lệch vận tốc là 0,3% vận tốc
mong muốn.
• Tỉ số truyền điện tử là từ 1/255 đến 255.
• Hỗ trợ các tín hiệu điều khiển PUL/DIR và CW/CCW.
• Đầu vào cách ly quang,cung cấp các tín hiệu đơn và khác nhau.
• Chức năng tự động nhớ 10 lỗi gần nhất.
• Bảo vệ quá dòng,quá áp,không đủ điện áp,lỗi xung,lỗi encoder,lỗi vị trí.
• Kích thước nhỏ,kỹ thuật sắp đặt bề mặt.
Mô hình kết nối
Page 5 of 32
Trường ĐH Công Nghiệp Hà Nội Đề Tài Con Lắc Ngược
Các chân PUL/DIR/EN:
Chức
năng chân
Chi tiết

PUL+ Tín hiệu xung:Trong chế độ xung
đơn(xung/hướng),đầu vào này là xung tín hiệu;4-5V khi xung
cao,0-0,5V khi xung thấp.Trong chế độ xung
đôi(xung/xung),đầu vào này là xung clockwise(CW),hoạt
động ở mức thấp(Mức cố định).Muốn cho đáp ứng ổn
định,độ rộng xung nên lớn hơn 1.0us.Chuỗi các điện trở mắc
nối tiếp để giới hạn dòng khi tín hiệu điều khiển được sử
dụng là +12V hoặc +24V.
PUL-
Page 6 of 32
Trường ĐH Công Nghiệp Hà Nội Đề Tài Con Lắc Ngược
DIR+ Tín hiệu chiều:Trong chế độ xung đơn,tín hiệu này có
mức điện áp cao/thấp thể hiện hai chiều quay của động
cơ(thiết lập bởi R33 và R37 ở bên trong);trong chế độ xung
đôi ,tín hiệu này là xung counter-clock (CCW),tác động ở
mức thấp(mức cố định).Để đáp ứng chuyển động được ổn
định,tín hiệu DIR nên có trước tín hiệu xung ít nhất 5us.Mức
DIR cao là 4-5V,mức DIR thấp là 0-0.5V.Chú ý rằng hướng
của chuyển động liên quan tới cách đi dây động cơ -driver và
các pha tín hiệu A/B của encoder.Thay đổi kết nối hai dây
của động cơ đến driver và các pha tín hiệu A/B của encoder
sẽ làm đổi chiều động cơ.
DIR-
EN+ Tín hiệu cho phép:tín hiệu này được sử dụng để cho
phép/không cho phép driver.Mức cao( với tín hiệu điều khiển
NPN,PNP và các tín hiệu điều khiển vi phân thì ngược lại,
nghĩa là ở mức thấp để cho phép) cho phép driver và mức
thấp để không cho phép driver.Thông thường để chân này
KHÔNG KẾT NỐI (CHO PHÉP).Có mắc chuỗi điện trở để
giởi hạn dòng khi sử dụng tín hiệu điều khiển +12V hoặc

+24V.
EN-
Thiết lập chế độ PUL/DIR và CW/CCW
DCS810 có 4 điện trở (R31/R32/R33/R37) bên trong để thiết lập cụ thể cho các
chế độ PUL/DIR và CW/CCW,được chỉ ra trên hình 2.Thiết lập mặc định là hình (b) cho
chế độ PUL/DIR,DIR thấp cho chiều dương và RIR cao cho chiều ngược lại.
Hình 2-Thiết lập PUL/DIR và CW/CCW.
Kết nối tín hiệu điều khiển:
DCS810 có thể chấp nhận đầu vào một ngõ hoặc vi phân(gồm collector hở và đầu
ra PNP).DCS810 có hai đầu vào logic cách ly quang để nhận đường tín hiệu điều khiển
của driver.Những đầu vào này được cách ly tối đa và được loại bỏ các nhiễu điện tử kèm
theo trong tín hiệu điều khiển.Nên sử dụng dây tín hiệu điều khiển của driver để làm tăng
Page 7 of 32
Trường ĐH Công Nghiệp Hà Nội Đề Tài Con Lắc Ngược
khả năng loại bỏ nhiễu trong môi trường giao thoa.Những hình dưới miêu tả cách kết nối
collector hở và các tín hiệu điều khiển khác nhau.
Hình 3-Kết nối với tín hiệu điều khiển collector hở(anode chung)
Hình 4-Kết nối với tín hiệu PNP(catot chung)
Page 8 of 32
Trường ĐH Công Nghiệp Hà Nội Đề Tài Con Lắc Ngược
Hình 5-Kết nối với tín hiệu điều khiển khác.
Kết nối encoder:
DCS810 có thể chấp nhận đầu vào của encoder vi phân hay encoder một ngõ.Tuy
nhiên encoder vi phân thường được lựa chọn do khả năng chống nhiễu tốt của nó.Việc
kết nối với encoder một ngõ tương tự với encoder vi phân chỉ khác là không kết nối với
các kênh A- và kênh B-(các dây A- và B- bên trong được kéo lên đến +2.5V).Chú ý rằng
dây xoắn đôi được bọc có khả năng chống nhiễu tốt nhất.
Nếu encoder tiêu thụ nhỏ hơn 50mA,DCS810 có thể cung cấp nguồn trực tiếp cho
encoder,và cách kết nối như trong hình 6 hoặc hình 7.Nếu encoder tiêu thụ hơn 50mA,có
thể sử dụng nguồn 1 chiều bên ngoài và cách kết nối như hình 8 và 9.

Hình 6-DCS810 cung cấp nguồn trực tiếp cho encoder vi phân.
Hình 7-DCS810 cung cấp nguồn trực tiếp cho encoder single-ended.
Page 9 of 32
Trường ĐH Công Nghiệp Hà Nội Đề Tài Con Lắc Ngược
Hình 8-Sử dụng nguồn 1 chiều bên ngoài để cung cấp cho encoder vi phân.
Hình 9-Sử dụng nguồn 1 chiều bên ngoài để cung cấp cho encoder single-
ended.
Giao diện kết nối RS232:
Hình 10-Giao diện kết nối RS232.
.
Page 10 of 32
Trường ĐH Công Nghiệp Hà Nội Đề Tài Con Lắc Ngược
Hình 11-Kết nối điển hình (Tín hiệu điều khiển collector hở và encoder
single-ended)
Hình 12-Kết nối điển hình (Tín hiệu điều khiển vi phân và encoder vi phân)
Page 11 of 32
Trường ĐH Công Nghiệp Hà Nội Đề Tài Con Lắc Ngược
Vi điều khiển ATMEGA32
Đây là họ vi điều khiển do hãng Atmel sản xuất với 40 chân cùng với nguồn tài
nguyên trong chip rất lớn có thể đáp ứng được các yêu cầu trong học tập cũng như trong
nghiên cứu.
Page 12 of 32
Trường ĐH Công Nghiệp Hà Nội Đề Tài Con Lắc Ngược
Dùng Atmega 32 để kiểm tra ngắt với thanh ghi điều khiển MCU-MCUCR thanh
ghi xác lập chế độ ngắt ngoài còn thanh ghi GICR là thanh ghi mặt nạ ngắt giúp xác định
chế độ hoạt động của ngắt.
Sử dụng chế độ timer/counter để điều khiển tần số cấp cho driver bằng cách chọn
chế độ fastPWM với giá trị TOP là thanh ghi ICR vì vậy ta có thể dễ dàng thay đổi giá trị
 tần số thay đổi.
4.PHƯƠNG HƯỚNG THỰC HIỆN

Đầu tiên cần xác định vị trí cân bằng tức là điểm mà con lắc đang ở vị trí cao
nhất, bằng cách đọc vào vi điều khiển giá trị của encoder khi con lắc đang ở vị trí thấp
nhất, rồi cộng trừ đi giá trị 512 tùy thuộc vào giá trị đọc được từ điểm thấp nhất thì ta sẽ
xác định được vị trí của điểm cao nhất( vì độ phân giải của ecnoder là 1024 nên một nửa
giá trị của nó sẽ là 512 ở đây lấy ½ giá trị của độ phân giải bởi vì điểm ở vị trí cao nhất
và vị trí thấp nhất lệch nhau một góc 180
0
= ½ của một vòng quay). Sau khi đã xác định
được vị trí đỉnh(điểm gốc) mà con lắc cần phải đạt được để có thể giữ thăng bằng ta cần
phải xét vị trí của con lắc ở các thời điểm khác nhau để có thể so sánh với giá trị của
điểm gốc nhằm xác định giá trị của góc lệch. Đây chính là nhiệm vụ của vi điều khiển là
phải đọc vào được giá trị của điểm gốc(giá trị được đọc vào từ PINA và PINB) và liên
tục đọc giá trị thu được từ encoder khoảng 22ms lấy mẫu một lần để có thể xác định góc
lệch của con lắc so với điểm gốc ( ở đây dùng timer0 để tạo khoảng thời gian lấy mẫu
này). Tuỳ thuộc vào độ lệch của con lắc mà vi điều khiển sẽ đưa ra tín hiệu điều khiển
tới driver của động cơ servo. Ở đây dùng timer1 để điều chế dải tần số cấp cho driver
tương ứng với những khoảng góc lệch khác nhau thì ta sẽ có giá trị tần số tương ứng.
Ứng với dải tần số càng lớn thì tốc độ của động cơ cũng lớn. Để thay đổi được giá trị của
tần số ta chọn chế độ hoạt động của timer1 để cho giá trị Top bằng giá trị trong chứa
trong thanh ghi ICR1 giúp cho ta có thể dễ dàng thay đổi giá trị Top  giá trị chu kì thay
đổi  tần số thay đổi. Vì vậy nếu muốn thay đổi giá trị của tần số ta chỉ cần thay đổi giá
trị trong thanh ghi ICR1 còn giá trị chứa trong thanh ghi OCR1A và OCR1B bằng ½ giá
trị chứa trong thanh ghi OCR1 để tạo xung ra xung vuông có thời gian ở mức cao bằng
với thời gian ở mức thấp. Bây giờ ta chỉ cần đưa vào giá trị của góc lệch của để đưa ra
giá trị của tần số thích hợp. Giá trị tần số tính toán sẽ được đưa ra chân OC1A và OC1B
rồi nối với chân PUL+ trong bộ driver để driver điều khiển động cơ hoạt động. Ngoài ra
vì ở đây dùng bàn trượt dọc nên chỉ cho phép trượt trong một giới hạn nhất định vì vậy
muốn cho con trượt không vượt qua giới hạn thì ở đây ta sử dụng cữ chặn mềm (dùng
công tắc từ). Hai cữ chặn mềm sẽ được đặt ở hai đầu của bàn trượt hai chân của nó sẽ
được nối với hai chân ngắt của vi điêu khiển (IR0 và IR1) chân còn lại được nối mass,

khi con trượt ( có gắn nam châm) trượt qua cữ chặn mềm thì sẽ làm suất hiện một sườn
âm trên vi điều khiển kích hoạt chương trình ngắt trong vi điều khiển. Trong chương trình
ngắt ta đảo chiều quay của động cơ. Để đảm bảo trong trường hợp hai ngắt mềm không
kịp đáp ừng thì còn có hai cữ chặn cứng ( rơle ) đặt sau hai cữ chặn mềm được nối vào
chân EN+ còn chân EN- được nối mass trong driver để ngay khi có tín hiệu thì ngay lập
tức sẽ dừng động cơ. Một nhiệm vụ nữa của vi điều khiển là truyền giá trị đọc được từ
enconder và sai lệch lên visual basic.
Page 13 of 32
Trường ĐH Công Nghiệp Hà Nội Đề Tài Con Lắc Ngược
Nhiệm vụ của driver là nhận tần số từ vi điều khiển đề điều khiển tốc độ của động
cơ qua chân PUL+ còn chân PUL- được nối xuống mass. Hai chân EN+ và EN- là hai
chân cho phép , chân EN- được nối với mass , còn chân EN+ nối với VCC khi rơle bị
nhấn thì chân EN+ thông với chân EN- làm suất hiện ngắt trực tiếp trên driver ngừng cấp
nguồn cho động cơ  dừng động cơ.
5.PHƯƠNG PHÁP ĐIỀU KHIỂN
Ở đây để nhằm đáp ứng được những yêu cầu về quá độ thì chúng em có sử dụng
phương pháp PID để điều chỉnh các đáp ứng ở đầu ra tương ứng với những tín hiệu từ
đầu vào để làm sao tối ưu được hệ thống. PID là cách viết tắc của các từ Propotional (tỉ
lệ), Integral (tích phân) và Derivative (đạo hàm). Tuy xuất hiện rất lâu nhưng đến nay
PID vẫn là giải thuật điều khiển được dùng nhiều nhất trong các ứng dụng điều khiển tự
động với các yêu cầu: chính xác (accurate), nhanh (fast response), ổn định (small
overshot). Trong bài toán này chúng ta muốn con lắc nhanh đạt được vị trí cân bằng thì
cần phải đưa ra tần số phù hợp để điều khiển driver của động cơ Một cách đơn giản để
công thức hóa ý tưởng này là dùng quan hệ tuyến tính pp=Kp*e ( với Kp là một hằng số
dương nào đó mà chúng ta gọi là hệ số P (Propotional gain) và e là sai số cần điều khiển
tức là độ lệch của con lắc so với vị trí cân bằng. Mục đích là ở đây làm sao cho đưa sai số
e càng sớm về 0 càng tốt, nếu Kp càng lớn thì pp sẽ lớn con lắc nhanh chóng tiến về vị trí
cân bằng. Tuy nhiên nếu Kp quá lớn thì theo quán tính con lắc sẽ bị lệch sang chiều
ngược lại gây ra sai số giá trị này đựơc gọi là độ vọt lố (overshoot) cứ như vậy thì con lắc
sẽ bị lắc sang hai bên nên rất khó đạt được tới vị trí cân bằng. Để giải quyết vấn đề này

thì ta sẽ sử dụng một thành phần hãm trong bộ điều khiển tức là khi con lắc gần đạt đến
vị trí cân bằng thì điều khiển cho tốc độ của động cơ giảm xuống. Ta cần phải biết rằng
tại vị trí cân bằng sai số e thay đổi nhanh nhất. Mặt khác, tốc độ thay đổi của e có thể tính
bằng đạo hàm của biến này theo thời gian. Như vậy khi con lắc tiến gần về vị trí cân bằng
thì đạo hàm của sai số e tăng giá trị nhưng ngược chiều của pp (do e đang giảm). Vì vậy
nếu sử dụng đạo hàm làm thành phần hãm thì có thể giảm được độ vọt lố của con lắc.
Thành phần hãm này chính là thành phần D ( Derivative) trong bộ điều khiển PID mà
chúng ta đang xét. Thêm thành phần D vào bộ điều khiển P hiện tại, chúng ta thu được bộ
điều khiển PD như sau: PD= Kp*e + Kd*(de/dt) (trong đó (de/dt) là vận tốc của sai số e
và Kd là một hằng số không âm gọi là hệ số D (Derivative gain). Nhưng chúng ta lại vấp
phải một vấn đề là nếu thành phần D lớn hơn thành phần P hoặc P nhỏ khi con lắc chưa
đạt tới vị trí cân bằng thì đã dừng cấp tần số cho driver khi đó sẽ không làm cho conlắc
có thể cân bằng được sai số lúc này được gọi là steady state error ( sai số trạng thái tĩnh) .
Để tránh sai số tĩnh ta thêm vào bộ điều khiển một thành phần có tác dụng cộng dồn sai
số khi sai số trạng thái tĩnh xảy ra, 2 thành phần P và D mất tác dụng thành phần còn lại
sẽ cộng dồn sai số theo thời gian và làm tăng giá trị pp đến một lúc nào đó nó sẽ khiến
cho giá trị pp đủ lớn để đưa con lắc về vị trí cân bằng đây chính là thành phần I (Intergral
– tích phân) trong bộ điều khiển PID. Chúng ta biết rằng tích phân theo thời gian chính là
tổng của đại lượng đó theo thời gian ta có
PI = Kp*e +Kd*(de/dt) +Ki*§edt
Page 14 of 32
Trường ĐH Công Nghiệp Hà Nội Đề Tài Con Lắc Ngược
Như vậy là ta đã có thể có một bộ điều khiển PID đáp ứng lại với yêu cầu của bài
toán đặt ra.
6.Sơ đồ nguyên lý mạch in và giao diện trên VB
Mạch nguyên lý
T X D
V C C
R 3 4 K 7
R 1 7

4 K 7
E 1 6
G N D
I N T 1
R E S E T
R 2
4 K 7
J 2
1 2 V D C
1
2
1
2
M O S I
D 1 4
L E D
1 2
X T A L 1
J 2 2
J A C K 2 0 0
1
2
1
2
X T A L 1
D 1 0
L E D
1 2
J 1 4
X U N G 2

1
2
1
2
E 3
G N D
R 8
4 K 7
J 1 8
N G A T 1
1
2
1
2
E 6
Y 1
1 2 M H z
V C C
E 1 6
R 1 0
4 K 7
J 2 0
N G A T 3
1
2
1
2
R I N
D 1 6
L E D

1 2
E 6
V C C
G N D
V C C
E 1 2
S C K
R 1
1 0 K
D 1 7
L E D
1 2
E 2
G N D
R 1 5
4 K 7
D 1 1
L E D
1 2
V C C
D I R 2
U 1
A T M E G A 1 6
1
2
3
4
5
6
7

8
9
1 0
1 1
1 2
1 3
1 4
1 6
1 7
1 9
2 0 2 1
2 2
2 3
2 4
2 5
2 6
2 7
2 8
2 9
4 0
3 9
3 8
3 7
3 6
3 5
3 2
3 1
3 0
3 4
3 3

1 5
1 8
P B 0
P B 1
P B 2
P B 3
P B 4
P B 5
P B 6
P B 7
R e s e t
V C C
G N D
X T A L 2
X T A L 1
P D 0
P D 2
P D 3
P D 5
P D 6 P D 7
P C 0
P C 1
P C 2
P C 3
P C 4
P C 5
P C 6
P C 7
P A 0
P A 1

P A 2
P A 3
P A 4
P A 5
A R E F
G N D
A V C C
P A 6
P A 7
P D 1
P D 4
E 1 1
G N D
G N D
V C C
V C C
D 1 2
L E D
1 2
+
C 1
2 2 0 0 u F
P W M 2
R E S E T
P W M 1
X T A L 2
E 1 0
D 9
L E D
1 2

J 1 5
X U N G 3
1
2
1
2
R 1 4
4 K 7
R 1 8
1 0 K
J 3
J A C K 2 0 0
1
2
1
2
V C C
J 6
J A C K 2 0 0
1
2
1
2
E 7
V C C
C 9
3 3 P
E 1 5
V C C
T O U T

G N D
G N D
V C C
V C C
R 9
4 K 7
J 1 9
N G A T 2
1
2
1
2
M I S O
E 3
V C C
V C C
C 8
3 3 P
R X D
G N D
J 4
J 5 X 2 1 0 0
1 2
3 4
5 6
7 8
9 1 0
G N D
V C C
G N D

E 1 3
D I R 1
E 7
V C C
R 6
4 K 7
D 1
D I O T C A U
1
2
3
4
V +
~
~
V -
G N D
E 4
G N D
E 8
E 1 4
D 7
L E D
1 2
V C C
J 1 6
E 9
1
2
1

2
R 7
4 K 7
P W M 1
S C K
V C C
R 5
4 K 7
+
C 2
2 2 0 0 u F
T O U T
E 9
V C C
D 1 3
L E D
1 2
M O S I
+
C 4
1 u F
G N D
R 2 0
5 6 R
D 1 5
L E D
1 2
V C C
+
C 6

1 u F
E 9
I N T 0
J 5
J A C K 2 0 0
1
2
1
2
D 4
L E D
1 2
T X D
+
C 7
1 u F
G N D
E 2
R 1 3
4 K 7
J 1
1 2 V A C
1
2
1
2
D 3
L E D
1 2
E 1 4

E 4
D I R 2
C 3
1 0 4
G N D
G N D
V C C
R 1 1
4 K 7
G N D
V C C
D I R 1
M I S O
E 1 0
E 1
G N D
D 2
L E D
1 2
R 1 2
4 K 7
D 8
L E D
1 2
R I N
V C C
J 1 0
J A C K 2 0 0
1
2

1
2
D 6
L E D
1 2
S W 1
R E S E T
G N D
R 4
4 K 7
J 2 4
J A C K 2 0 0
1
2
1
2
R 1 6
4 K 7
J 1 3
X U N G 1
1
2
1
2
I N T 0
G N D
E 5
G N D
U 2
7 8 0 5

1
2
3
I N
G N D
O U T
G N D
R 1 9 1 0 K
V C C
+
C 5
1 u F
J 1 7
E 1 0
1
2
1
2
E 1 5
V C C
J 2 1
J A C K 2 0 0
1
2
1
2
V C C
D 5
L E D
1 2

J 2 3
J A C K 2 0 0
1
2
1
2
E 1 3
G N D
V C C
U 3
M A X 2 3 2
1
2
3
4
5
6
7
8 9
1 0
1 1
1 2
1 3
1 4
1 5
1 6
C 1 +
V s +
C 1 -
C 2 +

C 2 -
V s -
T 2 O U T
R 2 I N R 2 O U T
T 2 I N
T 1 I N
R 1 O U T
R 1 I N
T 1 O U T
G N D
V C C
V C C
E 1 1
X T A L 2
R E S E T
E 8
G N D
E 5
G N D
R X D
V C C E 1 2
G N D
I N T 1
E 1
G N D
P W M 2
Page 15 of 32
Trường ĐH Công Nghiệp Hà Nội Đề Tài Con Lắc Ngược
Sơ đồ layout
Giao diện trên visual basic

Page 16 of 32
Trường ĐH Công Nghiệp Hà Nội Đề Tài Con Lắc Ngược
7.PHẦN LẬP TRÌNH
Code của chương trình
/*****************************************************
This program was produced by the
CodeWizardAVR V2.03.4 Standard
Automatic Program Generator
© Copyright 1998-2008 Pavel Haiduc, HP InfoTech s.r.l.

Project : Con lac nguoc
Version : 6.0
Date : 09/05/2010
Author : Vu Khac Trung
Company : HaUI
Comments:
Tiep tuc voi thach anh 12MHz va chip ATmega32L
Chip type : ATmega32L
Program type : Application
Clock frequency : 12.000000 MHz
Memory model : Small
External RAM size : 0
Data Stack size : 512
*****************************************************/
#include <mega32.h>
#include <delay.h>
Page 17 of 32
Trường ĐH Công Nghiệp Hà Nội Đề Tài Con Lắc Ngược
#include <stdio.h>
//===================== Dinh nghia cac chan dieu khien

=========================//
#define DIR PORTD.6 //PORTD.6
//====================== Dinh nghia cac gia tri cho timer
=========================//
#define Freq_Timer 12000000 //tan so timer la 16MHz
#define Freq_Min 1000 //Tan so PWM min
#define Freq_Max 150000 //Tan so PWM max
#define T_laymau 22
//============================ Dinh nghia chieu quay
==============================//
#define phai 0
#define trai 1
//============================== Khai bao bien toan cuc
===========================//
unsigned char re_dir=1;
unsigned int top=0,bottom=0,top_origin=0,bottom_origin=0;
unsigned char auto_balance=0;
unsigned char error_left=0,error_right=0;
float kp=267,kd=0,ki=48; //da can bang dc
float pp=0,pd=0,pi=0;
int error=0,pre_error=0;
float output=0;
unsigned int dem=0;
unsigned int pre_encoder=0;
//=========================================================
========================//
//=================================== Cac ham con
=================================//
//=========================================================
========================//

void PWM(unsigned long int tanso,unsigned char chieuquay)
{
unsigned long int x;
/*
if (re_dir!=chieuquay)
{
Page 18 of 32
Trường ĐH Công Nghiệp Hà Nội Đề Tài Con Lắc Ngược
ICR1H=0;
ICR1L=0;
DIR=chieuquay;
delay_ms(3);
re_dir=chieuquay;
}
*/
DIR=chieuquay;
//if (tanso<Freq_Min) tanso=0;
if (tanso>Freq_Max) tanso=Freq_Max;
if (tanso!=0)
x=Freq_Timer/tanso;
else
x=0;
ICR1H=x/256;
ICR1L=x%256;
//OCR1A=x/2;
OCR1B=x/2;
}
void truyen(long x)
{
unsigned char a,b;

char dau='';
if (x>=0)
{
dau='D';
}
else
{
x=-x;
dau='A';
}

a=x/256;
b=x%256;
putchar(dau);
putchar(a);
putchar(b);
}
Page 19 of 32
Trường ĐH Công Nghiệp Hà Nội Đề Tài Con Lắc Ngược
void truyen_error(void)
{
putchar(38);
putchar('E');
truyen(error);
}
void truyen_PWM(void)
{
long temp;
temp=(ICR1H*256+ICR1L);
if (DIR==trai)

temp=-temp;
putchar(38);
putchar('P');
truyen(temp);
}
void clear(void)
{
putchar(38);
putchar('C');
truyen(0);
}
unsigned int input_encoder(void)
{
unsigned int temp,i;
unsigned char x1,x2,x3,x4,x5,x6,x7,x8,x9,x10,x11,x12,x13;
i=0;
while (i<5)
{
i++;
x1= ~ PINC.2;
x2= ~ PINA.1;
x3= ~ PINA.2;
x4= ~ PINA.3;
x5= ~ PINA.4;
x6= ~ PINA.5;
x7= ~ PINA.6;
x8= ~ PINC.1; // PINA.7;
x9= ~ PINC.7;
x10=~ PINC.6;
x11=~ PINC.5;

x12=~ PINC.4;
x13=~ PINC.3;
Page 20 of 32
Trường ĐH Công Nghiệp Hà Nội Đề Tài Con Lắc Ngược

temp=(x1+2*x2+4*x3+8*x4)+10*(x5+2*x6+4*x7+8*x8)+100*(x9+2*x10+4*x11+8*x1
2)+1000*x13;
if ((temp==pre_encoder)|(temp==pre_encoder+1)|(temp==pre_encoder-1))
break;
}
pre_encoder=temp;
return temp;
}
void lay_gia_tri_tham_chieu(void)
{
unsigned int temp1=0,temp2=0;
unsigned char i;
i=0;
temp1=input_encoder();
while (i<10)
{
delay_ms(100);
temp2=input_encoder();
if (temp2==temp1)
i++;
else
{
i=0;
temp1=input_encoder();
}

}

bottom_origin=temp1;
if (bottom_origin>=512) top_origin=bottom_origin-512; //Diem goc "0" bi
lech sang phai
else if (bottom_origin<512) top_origin=bottom_origin+512; //Diem goc "0" bi
lech sang trai
bottom=bottom_origin;
top=top_origin;
}
void lay_gia_tri_tham_chieu2(void)
{
unsigned int temp1=0,temp2=0;
unsigned char i;
i=0;
temp1=input_encoder();
Page 21 of 32
Trường ĐH Công Nghiệp Hà Nội Đề Tài Con Lắc Ngược
while (i<10)
{
delay_ms(100);
temp2=input_encoder();
if (temp2==temp1)
i++;
else
{
i=0;
temp1=input_encoder();
}
}

top_origin=temp1;
if (top_origin>=512) bottom_origin=top_origin-512; //Diem goc "0" bi lech
sang trai
else if (top_origin<512) bottom_origin=top_origin+512; //Diem goc "0" bi
lech sang phai
bottom=bottom_origin;
top=top_origin;
}
void truyen_gia_tri_tham_chieu(void)
{
delay_ms(300);
putchar(38);
putchar('T');
truyen(top_origin);
delay_ms(300);
putchar(38);
putchar('B');
truyen(bottom_origin);
}
int rotate_encoder(void)
{
int error_temp,temp;
temp=input_encoder();
if (top>=512) //Truong hop diem goc "0" lech sang trai
{
if ((temp>=0)&(temp<bottom))
error_temp=-(1024-top+temp);
if ((temp>=bottom)&(temp<=1023))
Page 22 of 32

Trường ĐH Công Nghiệp Hà Nội Đề Tài Con Lắc Ngược
error_temp=top-temp;
}
if (top<511) //Truong hop diem goc "0" lech sang phai
{
if ((temp>=0)&(temp<bottom))
error_temp=top-temp;
if ((temp>=bottom)&(temp<=1023))
error_temp=top+1024-temp;
}
return error_temp;
}
// External Interrupt 0 service routine
interrupt [EXT_INT0] void ext_int0_isr(void)
{
/*
GICR|=0x00;
MCUCR=0x00;
MCUCSR=0x00;
GIFR=0x00;
*/
delay_ms(20);
if (PIND.2==0)
{
auto_balance=0;
PWM(50000,trai);
while (PIND.2==0) {;}
delay_ms(200);
PWM(0,trai);
error_left=0;

error_right=1;
}
/*
GICR|=0xC0;
MCUCR=0x0A;
MCUCSR=0x00;
GIFR=0xC0;
*/
}
// External Interrupt 1 service routine
interrupt [EXT_INT1] void ext_int1_isr(void)
{
/*
GICR|=0x00;
Page 23 of 32
Trường ĐH Công Nghiệp Hà Nội Đề Tài Con Lắc Ngược
MCUCR=0x00;
MCUCSR=0x00;
GIFR=0x00;
auto_balance=0;
PWM(50000,phai);
while (PIND.3==0) {;}
delay_ms(200);
PWM(0,phai);
error_left=1;
error_right=0;
GICR|=0xC0;
MCUCR=0x0A;
MCUCSR=0x00;
GIFR=0xC0;

*/
delay_ms(20);
if (PIND.3==0)
{
auto_balance=0;
PWM(50000,phai);
while (PIND.3==0) {;}
delay_ms(200);
PWM(0,phai);
error_left=1;
error_right=0;
}
}
void dung_con_lac(void)
{
unsigned long int vantoc=80000;
unsigned int time_over_flow=10000;
int temp1,temp2;
PWM(90000,trai);
delay_ms(600);
PWM(0,0);
delay_ms(50);
PWM(90000,phai);
delay_ms(1000);
PWM(0,0);
delay_ms(50);
Page 24 of 32
Trường ĐH Công Nghiệp Hà Nội Đề Tài Con Lắc Ngược
dem=0;
while (dem<time_over_flow)

{

PWM(vantoc,trai);
while (1)
{
temp1=error;
delay_ms(5);
temp2=error;
if (temp1>=temp2)
break;
}
if ((error>-300)&(error<300))
{
dem=time_over_flow;
break;
}

while (error>0) {}

PWM(vantoc,phai);
while (1)
{
temp1=error;
delay_ms(1);
temp2=error;
if (temp1 <=temp2)
break;
}
if ((error>-300)&(error<300))
{

dem=time_over_flow;
break;
}

while (error<0) { }
}
}
Page 25 of 32