TRƯỜNG ĐẠI HỌC BÁCH KHOA HÀ NỘI

VIỆN ĐIỆN

Đồ án I
ĐỀ TÀI:

TÌM HIỂU NGUYÊN LÝ HOẠT ĐỘNG ENCODER, CẢM BIẾN GIA

TỐC MPU6050 KẾT HỢP BỘ LỌC KALMAN

Giảng viên hướng dẫn : TS. Vũ Vân Hà

Họ và tên sinh viên : Nguyễn Việt Hoàng
Mã số sinh viên : 20191856
Mã lớp : 717569

Hà Nội, 08/2022
 LỜI NÓI ĐẦU

Qua quá trình thực hiện đồ án I, em cũng đã tìm hiểu và học tập được nhiều điều .
Mặc dù đồ án được thực hiện trong thời gian khá dài, tuy nhiên do còn khá thiếu kinh
nghiệm trong việc tìm kiếm thông tin nên mong có gì sai sót mong thầy bỏ qua ạ. Em
đã cố gắng thực hiện đồ án môn học, và cũng đã tiếp thu những kiến thức, kỹ năng mới
trong kì học này.
Em xin chân thành cảm ơn.

Hà Nội, ngày 12 tháng 8 năm 2022

 MỤC LỤC

DANH MỤC HÌNH VẼ.............................................................................................................i

CHƯƠNG 1. ENCODER – CẢM BIẾN ĐO VỊ TRÍ GÓC, TỐC ĐỘ ĐỘNG CƠ.............1

1.1 Cấu tạo:...........................................................................................................................1

1.2 Phân loại:........................................................................................................................1

1.2.1 Incremental encoder (Tương đối).............................................................................1
1.2.2 Absolute encoder (Tuyệt đối)...................................................................................2

1.3 Thông số encoder cần quan tâm.....................................................................................3

1.4 Cách đọc encoder tương đối...........................................................................................3

1.5 Thực nghiệm...................................................................................................................4

1.5.1 Thiết bị phần cứng....................................................................................................4
1.5.2 Sơ đồ đấu dây...........................................................................................................6
1.5.3 Kết quả thực nghiệm................................................................................................7

CHƯƠNG 2. MPU6050 – CẢM BIẾN GIA TỐC VÀ BỘ LỌC KALMAN.......................8

2.1 Nguyên lý hoạt động.......................................................................................................8

2.2 Bộ lọc Kalman.................................................................................................................9

2.2.1 Bản chất bộ lọc Kalman...........................................................................................9
2.2.2 Bộ lọc Kalman cho hệ tuyến tính rời rạc................................................................10

2.3 Bài toán cảm biến MPU6050:......................................................................................11

2.4 Kết quả thực nghiệm:....................................................................................................13

KẾT LUẬN..............................................................................................................................14

TÀI LIỆU THAM KHẢO......................................................................................................15

PHỤ LỤC.................................................................................................................................16
 DANH MỤC HÌNH VẼ

Hình 1.1 Cấu tạo encoder..............................................................................................1

Hình 1.2 Encoder tương đối...........................................................................................2
Hình 1.3 Encoder tuyệt đối............................................................................................2
Hình 1.4 Tín hiệu kênh A và B......................................................................................4
Hình 1.5 Encoder 334....................................................................................................4
Hình 1.6 Module L298...................................................................................................5
Hình 1.7 Sơ đồ dây........................................................................................................6
Hình 1.8 Vị trí góc và vận tốc........................................................................................7
Hình 2.1 MPU6050........................................................................................................9
Hình 2.2 Mô hình Kalman...........................................................................................10
Hình 2.3 Thuật toán Kalman........................................................................................11
Hình 2.4 Mô hình bộ lọc Kalman cho hệ tuyến tính....................................................11
Hình 2.5 Vị trí không đổi.............................................................................................13
Hình 2.6 Vị trí dao động..............................................................................................13

i
 CHƯƠNG 1. ENCODER – CẢM BIẾN ĐO VỊ TRÍ GÓC, TỐC

ĐỘ ĐỘNG CƠ

1.1 Cấu tạo:

Encoder cấu tạo chính gồm:

 Đĩa quang tròn có rảnh nhỏ quay quanh trục: Trên đĩa được đục lỗ (rãnh), khi
đĩa này quay và chiếu đèn led lên trên mặt đĩa thì sẽ có sự ngắt quãng xảy ra.
Các rãnh trên đĩa chia vòng tròn 360o thành các góc bằng nhau. Và một đĩa có
thể có nhiều dãy rãnh tính từ tâm tròn.

 Bộ cảm biến thu (photosensor)

 Nguồn sáng (Light source).

Hình 1.1 Cấu tạo encoder

1.2 Phân loại:

1.2.1 Incremental encoder (Tương đối)

Tương ứng đĩa 2bit, cho ngõ ra dạng xung vuông pha AB, hoặc ABZ hoặc ABZA|
B|Z| (đọc là A đảo, B đảo, Z đảo). Loại Encoder có 2 tín hiệu xung A và B là phổ biến
nhất. Tín hiệu khe Z là tín hiệu để xác định động cơ quay được một vòng.

1
 Hình 1.2 Encoder tương đối

Từ số xung, Encoder có số dây tương ứng: 6 hoặc 4 dây tùy loại. Các dây bao gồm :
2 dây nguồn, 2 dây pha A và B, 1 dây pha Z...

1.2.2 Absolute encoder (Tuyệt đối)

Tương ứng đĩa quay 8bit hay 8 dãy rãnh, cho ngõ ra dạng mã kỹ thuật số(BCD),
Binary (nhị phân), Gray code…

Hình 1.3 Encoder tuyệt đối

Ví dụ: Encoder có ngõ ra dạng bit, độ phân giải 2500. Thì 1 vòng quay của encoder
sẽ cho ra 2500 dãy nhị phân 8bit:

00001011 00100010 10010011 ….

11110001 10010010 01011101 …

Absolute encoder (Tuyệt đối) có thể ghi nhớ vị trí khi bị mất nguồn do mỗi vị trí có
một mã tín hiệu riêng.

2
1.3 Thông số encoder cần quan tâm
 Đường kính trục, dạng trục: Encoder có dạng trục dương hoặc âm. đường kính
trục từ 5~50mm. Thường đường kính lớn hơn 6mm sẽ là loại trục âm (trục lõm).
 Độ phân giải: hay còn gọi là số xung - tương ứng số tín hiệu encoder đếm được
khi quay 1 vòng. Encoder có số xung càng cao thì giá càng cao. Thang máy thường
dùng xung 1024p/r, chế tạo máy vào khoảng 360p/r, 1000p/r. Ngoài ra còn có các
xung lên đến 6000p/r hoặc 6pr.
 Điện áp: Encoder thường xuyên bị cháy do khi lắp đặt không chú ý nguồn cấp.
Nếu encoder có dãy điện áp:5~24V thì rất dễ. Tuy nhiên với một số encoder trục
lớn: 30-40mm, encoder theo máy sẽ thường gặp điện áp xác định: 5V, 12V hoặc
15V.
 Ngõ ra: AB, ABZ, AB đảo, ABZ đảo.
 Dạng ngõ ra: Có nhiều dạng ngõ ra, liệt kê sơ sơ gồm: Open Collector, Voltage
Output, Complemental, Totem Pole, Line Driver. Dạng ngõ ra quy định nguồn cấp,
đầu đọc thông tin ...
 Dây cáp: cáp càng dài càng dễ bị nhiễu. Dây tiêu chuẩn từ 1-3m. Đôi khi lên đến
10m theo nhu cầu sử dụng.
 Phụ kiện: đi kèm encoder trục dương là Coubling, encoder trục âm là
Pass. Coubling giúp nối encoder trục dương với motor trục dương, Pass giúp gắn
encoder vào máy. Một encoder trục âm có 1-2 pass tùy loại.

1.4 Cách đọc encoder tương đối

Encoder hoạt động cho ngõ ra dạng tín hiệu xung vuông. Khi encoder quay, cơ cấu
cơ khí sẽ làm quay đĩa chia rãnh của nó cắt ngang qua đường tín hiệu quang được phát
bời mắt phát quang (hồng ngoại, laser..). Việc chia cắt tín hiệu này cho ra đường tín
hiệu dạng xung ở các kênh A, B và I. Xung này thường trả về tín hiệu ở mức logic
5V(High) và 0V(Low). Tín hiệu dạng xung này như bất kể các tín hiệu dạng xung điện
khác, nó đều có những đặc tính sau để ta có thể phát hiện và đo đạc nó, bao gồm:

1. Tín hiệu xung ở mức thấp 0V

2. Tín hiệu xung ở mức cao 5V

3
 3. Tín hiệu sườn lên của xung

4. Tín hiệu sườn xuống của xung

Hình 1.4 Tín hiệu kênh A và B

Để xác định độ thay đổi encoder có 2 cách:

 Sử dụng 2 ngắt ngoài: Theo dõi sự thay đổi đồng thời cả 2 kênh A và B.
 Sử dụng 1 ngắt ngoài: Chọn 1 kênh làm tín hiệu ngắt, sau đó đọc tín hiệu
kênh còn lại.

1.5 Thực nghiệm

1.5.1 Thiết bị phần cứng

Encoder 334 tích hợp với động cơ:

+ Điện áp hoạt động: DC 3V – 24V.

+ Tốc độ:

+ DC 12V 82000 rpm.

+ DC 24V 16800 rpm.

+ Hỗ trợ 2 kênh A, B.

+ Số xung: 334 Xung.

Hình 1.5 Encoder 334

4
Mạch cầu H L298:

+ Tích hợp hai mạch cầu H.

+ Điện áp điều khiển: +5 VDC ~ +35 VDC.

+ Dòng tối đa cho mỗi cầu H là: 2A.

+ Điện áp của tín hiệu điều khiển: +5 VDC ~ +7 VDC.

+ Dòng của tín hiệu điều khiển: 0 ~ 36mA.

+ 12V Power, Power GND: Nối với nguồn 12V cấp cho động cơ.

+ A Enable, B Enable: Nhận 2 tín hiệu enable cho 2 cầu.

+ IN1, IN2: Nhận tín hiệu điều khiển PWM đối xứng cho cầu thứ nhất.

+ IN3, IN4: Nhận tín hiệu điều khiển PWM đối xứng cho cầu thứ 2.

+ fPWM max: 5 MHz.

+ OutputA (Out1, Out2), OutputB (Out3, Out4): Nối với động cơ cần điều khiển.

Hình 1.6 Module L298

5
1.5.2 Sơ đồ đấu dây

Hình 1.7 Sơ đồ dây

Nhận thấy rằng sơ đồ mạch cầu H rất thích hợp để điểu khiển hai động cơ cùng lúc,
ta có thể sử dụng xung vuông để điều chỉnh tốc độ động cơ, và còn có thể đảo chiều
động cơ bằng cách thay đổi các giá trị logic của vi điều khiển. Động cơ sử dụng điện
áp 12 VDC làm nguồn cấp.

6
1.5.3 Kết quả thực nghiệm

Hình 1.8 Vị trí góc và vận tốc

Hình 1.8 là kết quả sau khi chạy thử động cơ tích hợp encoder 334 sử dụng vi điều
khiển arduino.

Vận tốc tên tính theo vị trí của bánh xe theo công thức:

V = Số ví trí thay đổi trong 1 giây * 60 / số xung 1 vòng đĩa

Công thức tính vận tốc tức thời thích hợp trong việc đo sự thay đổi vị trí bánh xe
nhanh, quãng đường di chuyển ngắn.

7
 CHƯƠNG 2. MPU6050 – CẢM BIẾN GIA TỐC VÀ BỘ LỌC

KALMAN

2.1 Nguyên lý hoạt động

MPU6050 GY521 6DOF IMU (Accelerometer & Gyro):

+ Con quay hồi chuyển 3 trục (3-axis MEMS gyroscope)

+ Cảm biến gia tốc 3 chiều (3-axis MEMS accelerometer)

Ngoài ra, MPU-6050 còn có 1 đơn vị tăng tốc phần cứng chuyên xử lý tín hiệu
(Digital Motion Processor - DSP) do cảm biến thu thập và thực hiện các tính toán cần
thiết. Điều này giúp giảm bớt đáng kể phần xử lý tính toán của vi điều khiển, cải thiện
tốc độ xử lý và cho ra phản hồi nhanh hơn. Đây chính là 1 điểm khác biệt đáng kể của
MPU-6050 so với các cảm biến gia tốc và gyro khác.

MPU-6050 có thể kết hợp với cảm biến từ trường (bên ngoài) để tạo thành bộ cảm
biến 9 góc đầy đủ thông qua giao tiếp I2C.

Các cảm biến bên trong MPU-6050 sử dụng bộ chuyển đổi tương tự - số (Anolog
to Digital Converter - ADC) 16-bit cho ra kết quả chi tiết về góc quay, tọa độ... Với
16-bit bạn sẽ có 2^16 = 65536 giá trị cho 1 cảm biến.

Tùy thuộc vào yêu cầu của bạn, cảm biến MPU-6050 có thể hoạt động ở chế độ tốc
độ xử lý cao hoặc chế độ đo góc quay chính xác (chậm hơn). MPU-6050 có khả năng
đo ở phạm vi:

+ Con quay hồi chuyển: ± 250 500 1000 2000 dps

+ Gia tốc: ± 2 ± 4 ± 8 ± 16g

Hơn nữa, MPU-6050 có sẵn bộ đệm dữ liệu 1024 byte cho phép vi điều khiển phát
lệnh cho cảm biến, và nhận về dữ liệu sau khi MPU-6050 tính toán xong.

8
 Hình 2.1 MPU6050

2.2 Bộ lọc Kalman

Bộ lọc là một quá trình xử lý nhằm loại bỏ những gì không có giá trị hoặc không
quan tâm đến và giữ lại những gì có giá trị sử dụng. Trong xử lý tín hiệu, bộ lọc được
thiết kế để lọc tín hiệu sạch (cần tìm) từ trong tín hiệu trộn lẫn giữa tín hiệu sạch và
nhiều tín hiệu bẩn ( không cần thiết ).

Vậy bộ lọc Kalman, một cách khái quát là một tập hợp các phương trình toán học
mô tả một phương pháp tính toán truy hồi hiệu quả cho phép ước đoán trạng thái một
quá trình sao cho trung bình phương sai của độ lệch ( giữa giá trị thực và giá trị ước
đoán) là nhỏ nhất. Bộ lọc Kalman rất hiệu quả trong việc ước đoán các trạng thái trong
quá khứ, hiện tại và tương lai thậm chí ngay cả khi chính xác của hệ thống mô phỏng
không được khẳng định.

2.2.1 Bản chất bộ lọc Kalman

Để mô hình hóa hoạt động của mạch lọc Kalman, chúng ta có tín hiệu đo được, có
mô hình của tín hiệu đo được ( đòi hỏi tuyến tính) và sau đó là áp dụng vào trong hệ
thống phương trình của mạch lọc để ước lượng trạng thái quan tâm. Để có thể ứng
dụng một cách hiệu quả mạch lọc Kalman thì chúng ta phải mô hình hóa được một
cách tuyến tính sự thay đổi của trạng thái cần ước lượng (estimate) hoặc ước
đoán(predict).

9
 Hình 2.2 Mô hình Kalman

2.2.2 Bộ lọc Kalman cho hệ tuyến tính rời rạc

Mô hình đối tượng tuyến tính rời rạc được biểu diễn thông qua hệ phương trình
trạng thái sau:

xk = Axk-1 + Buk-1 + wk-1

zk = Hxk + vk

Trong đó :

 x : biến trạng thái

 u : biến đầu vào
 z : trạng thái đầu vào ( đo lường được )
 w, v : nhiễu quá trình và nhiễu đo lường tương ứng.

Giả sử các nhiễu quá trình w(t) và nhiễu đo lường v(t) là nhiễu trắng , có phương
sai tương ứng là Q và R.

p(w) ~ N (0, Q)
p(v) ~ N (0, R)
Với hệ thống như vậy bộ lọc Kalman tuyến tính cho phép ta ước lượng một cách
tốt nhất biến trạng thái xk, kí hiệu là 𝑥̂ k , sao cho hiệp phương sai của chúng Pk đạt giá
trị nhỏ nhất.

Pk = E[ekekT]
Ek = xk - 𝑥̂ k

10
 Thuật toán Kalman cho hệ rời rạc như sau:

Như vậy ta có thể thấy thuật toán gồm hai bước : bước ước lượng dự đoán ( time
update) và bước làm chính xác đự doán này ( mcasurement update ) dựa trên thông tin
đầu vào và đo được zk.

Hình 2.3 Thuật toán Kalman

Chú ý rằng 𝑥̂ k- là giá trị dự đoán được cập nhật từ giá trị ước lượng 𝑥̂ k-1 Thuật
toán này đc trình bày trên sơ đồ khối như sau:

Hình 2.4 Mô hình bộ lọc Kalman cho hệ tuyến tính

2.3 Bài toán cảm biến MPU6050:

Có một thông số được đưa về từ cảm biến, tuy nhiên thông số chịu tác động bởi
nhiễu, ta sẽ thiết kế bộ lọc Kalman để ước lượng được giá trị tốt nhất giá trị cần đo này.

Mô hình bài toán :

xk = Axk-1 + Buk-1 + wk-1

= xk-1 + wk

11
 Với giá trị thu đươc z ∈ R
zk = Hxk + vk
= xk + vk

Với :

 Giá trị cần đo là x(t)

 Tín hiệu thu được từ cảm biến là z(t)
 Nhiễu của quá trình là w(t)
 Nhiễu đo lường là v(t)

Các phương trình cho bộ lọc Kalman :

Phương trình quá trình “Time update”:

𝑥̂ k- = 𝑥̂ k-1

Pk- = Pk-1 + Q

Phương trình quá trình “easurement update”:

−
Kk= 𝑝 −𝑘
𝑃
𝑘
+𝑅
𝑥̂ k = 𝑥̂ k- + Kk(zk - 𝑥̂ k-
) Pk = (1- Kk)
�
𝑃− �

Một số giả thiết:

Tín hiệu cần đo là x(t) ~ (0,50)
w(t) ~ (0,1e-3) Q = 1e-3
nhiễu đo lường trong bài toán mô phỏng này khi lấy giá trị từ cảm biến ta lấy
khoảng 10% giá trị thật

Ta có Kk tỉ lệ nghịch với R, R lớn thì tốc độ ước lượng chậm, giá trị ước lượng có
vẻ ít tin tưởng hơn giá trị đo được và ngược lại. Do đó việc chọn R khá quan trọng ảnh
hưởng đến tốc độ ước lượng của hệ thống.

Chọn thông số ước lượng ban đầu : x(0) = 0, P 0- = 5. Giá trị x(0) không quan trọng
vì nó sẽ thích nghi tưng bước để đạt đến giá trị cần xác định còn P0- khác 0 là được.

12
2.4 Kết quả thực nghiệm:
Ứng dụng của bộ lọc Kalman trong việc lấy góc từ cảm biến MPU6050:

Hình 2.5 Vị trí không đổi

Hình 2.6 Vị trí dao động

Kết quả trên là thông số lấy từ Gyroscopes X (vận tốc góc), đồ thị lọc tương đối tốt
khi ở cảm biến ở vị trí cố định hình 2.5 và khi thay đổi vị trị hình 2.6.

13
 KẾT LUẬN

Trong quá trình thực hiện đồ án I, em cũng đã thấy được nhiều kiến thức mới mẻ,
chuyên sâu hơn so với việc chỉ học mỗi lý thuyết trên lớp. Những công việc này giúp
em ngày càng tiếp thu và phát triển hơn. Việc tìm hiểu lý thuyết xong từng bước mô
phỏng, thực hành, và cả việc lựa chọn thiết bị cần thiết để kiểm tra kết quả thực
nghiệm của mô hình, tuy phần nào vẫn còn sai sót, nhưng vẫn có thể phát triển và hoàn
thiện thêm. Cuối cùng em cũng xin cảm ơn thầy vì đã hướng dẫn em trong môn đồ án
này.

Em xin chân thành cảm ơn!

14
 TÀI LIỆU THAM KHẢO

[1] Y. Kim and H. Bang, Introduction to Kalman Filter and Its Applications.

[2] Sangtae K. and SangJoo K. (2015). Dynamic Modeling of a Two-wheeled

Inverted Pendulum Balancing Mobile Robot. In- 350 ternational Journal of
Control, Automation, and Systems, 13 (4) pp. 926- 933.

[3] Ming Y., Shuang W. and Jian-Zhong S. (2016). Simultaneous balancing and
trajectory tracking control for two-wheeled in- 310 verted pendulum vehicles: A
composite control approach. Neurocomputing 191, pp.44-5

[4] Kim H. W. and Jung S. (2016). Control of a two-wheel robotic vehicle for
personal transportation. Robotica, 34 (5), pp. 1186-1208.

[5] Z. Li, C. Yang, and L. Fan, Advanced Control of Wheeled Inverted Pendulum
Systems, Springer, Berlin, Germany, 2013.

[6] K. G. Tran, T. T. Nguyen, T. Q. Pham, P. D. Nguyen, D. V. Nguyen, P. A Nguyen,

H. M. Pham, P. D. Nguyen and N. H. Nguyen, Control of TWIR using LQR
Controller and Compound Disturbance Observer.

[7] https://forum.machinelearningcoban.com/t/kalman-filter-va-bai-toan-chuoi-thoi-
gian/3625

15
 PHỤ LỤC

Chương trình lấy dữ liệu MPU6050:

#include<Wire.h>

/*

#define ACCEL_XOUT_H 0x3B

#define ACCEL_XOUT_L 0x3C

#define ACCEL_YOUT_H 0x3D

#define ACCEL_YOUT_L 0x3E

#define ACCEL_ZOUT_H 0x3F

#define ACCEL_ZOUT_L 0x40

#define TEMP_OUT_H 0x41

#define TEMP_OUT_L 0x42

#define GYRO_XOUT_H 0x43

#define GYRO_XOUT_L 0x44

#define GYRO_YOUT_H 0x45

#define GYRO_YOUT_L 0x46

#define GYRO_ZOUT_H 0x47

#define GYRO_ZOUT_L 0x48

#define ADDR_MPU6050 0x68

*/

const int MPU=0x68; // I2C address of the MPU-6050

int AcX,AcY,AcZ,Tmp,GyX,GyY,GyZ;

void setup(){

Wire.begin();

Wire.beginTransmission(MPU);

Wire.write(0x6B); // PWR_MGMT_1 register

16
Wire.write(0); // set to zero (wakes up the MPU-6050)

Wire.endTransmission(true);

Serial.begin(9600);

void loop(){

Wire.beginTransmission(MPU);

Wire.write(0x3B); // starting with register 0x3B (ACCEL_XOUT_H)

Wire.endTransmission(false);

Wire.requestFrom(MPU,14,true); // request a total of 14 registers

AcX=Wire.read()<<8|Wire.read(); // 0x3B (ACCEL_XOUT_H) & 0x3C

(ACCEL_XOUT_L)

AcY=Wire.read()<<8|Wire.read(); // 0x3D (ACCEL_YOUT_H) & 0x3E

(ACCEL_YOUT_L)

AcZ=Wire.read()<<8|Wire.read(); // 0x3F (ACCEL_ZOUT_H) & 0x40

(ACCEL_ZOUT_L)

Tmp=Wire.read()<<8|Wire.read(); // 0x41 (TEMP_OUT_H) & 0x42 (TEMP_OUT_L)

GyX=Wire.read()<<8|Wire.read(); // 0x43 (GYRO_XOUT_H) & 0x44 (GYRO_XOUT_L)

GyY=Wire.read()<<8|Wire.read(); // 0x45 (GYRO_YOUT_H) & 0x46 (GYRO_YOUT_L)

GyZ=Wire.read()<<8|Wire.read(); // 0x47 (GYRO_ZOUT_H) & 0x48 (GYRO_ZOUT_L)

Serial.print(“AcX = “); Serial.print(AcX);

Serial.print(” | AcY = “); Serial.print(AcY);

Serial.print(” | AcZ = “); Serial.print(AcZ);

Serial.print(” | Tmp = “); Serial.print(Tmp/340.00+36.53);

//equation for temperature in degrees C from datasheet

Serial.print(” | GyX = “); Serial.print(GyX);

Serial.print(” | GyY = “); Serial.print(GyY);

Serial.print(” | GyZ = “); Serial.println(GyZ);

delay(333);

Hàm dự đoán và đánh giá góc:

float Kalman(float newAngle, float newRate, float dt){

//Step 1

17
 rate=newRate-bias;

angle+=dt*rate;

//Step 2

P[0][0]+=dt*(dt*P[1][1]-P[1][0]-P[0][1]+Q_angle);

P[0][1]-=dt*P[1][1];

P[1][0]-=dt*P[1][1];

P[1][1]+=Q_bias*dt;

// step 3

y = newAngle-angle;

// Step 4

S= P[0][0]+R_measure;

// Step 5

K[0]=P[0][0]/S;

K[1]=P[1][0]/S;

// Step 6

angle += K[0]*y;

bias+= K[1]*y;

// Step 7

float P00_temp = P[0][0];

float P01_temp = P[0][1];

P[0][0]-= K[0]*P00_temp;

P[0][1]-=K[0]*P01_temp;

P[1][0]-=K[1]*P00_temp;

P[1][1]-=K[1]*P01_temp;

return angle;

18
 Chương trình đọc dữ liệu encoder:
#include <LiquidCrystal_I2C.h>

LiquidCrystal_I2C lcd(0x27,16,2); // set the LCD address to 0x27 for a 16

chars and 2 line display

/* encoder 334

* dien ap dong co hoat dong 3V

* dien ap encoder 3,3V

* 8200 vòng/phút ở 12VDC

* 16800 vòng/phút ở 24VDC

* đọ phân giải N=334 xung/vòng

*/

int i=0;

int motorp =10;

int motorm =9;

int chanelA = 2;//int0

int chanelB = 4;

int pulse =0;

int prepulse= 0;

float velocity;

float position1;

float Time;

float Ton;

float Toff;

//float Ts = Ton + Toff;

int count = 0;

19
int rotation = 0; // rotation =1 quay thuan, = 0 dung, = -1 quay nghich;

void encoder1(){

if (digitalRead (chanelB) == LOW){

pulse++;

if (pulse/334==0)

count++;

else{

pulse--; // Read position 1

if (pulse/334==0)

count--;

void speedRate(float D,float Hz ){

// D = Ton/Ts; D trong khoang (0,1)

// Ts = 1/Hz;

Ton = D*1/Hz;

Toff = (1-D)*1/Hz;

forward (rotation);

delay(Ton*1000);

forward (0);

delay(Toff*1000);

void forward(int x)

{ pinMode(motorp,OUTPUT

);

20
pinMode(motorm,OUTPUT);

21
 if (x == 1){

digitalWrite(9, LOW); // forward(1) quay thuan

digitalWrite(10, HIGH);

}else if (x == -1){

digitalWrite(10, LOW); // forward(-1) quay nghich

digitalWrite(9, HIGH);

}else if (x == 0){

digitalWrite(9, LOW); // forward(0) stop

digitalWrite(10, LOW);

void Display(float mlTime)

{ if(millis() - Time >

1000*mlTime){

position1=2*3.1415*pulse/334.0;

velocity = (pulse-prepulse)/344.0*60/mlTime;

Serial.print("Phi(rad): ");

Serial.println(position1);

Serial.print("Velocity (rpm): ");

Serial.println(velocity); Serial.print("\

n");

prepulse = pulse;

Time = millis();

void Display_lcd(float mlTime)

22
{ if(millis() - Time >

1000*mlTime){

23
 position1=2*3.1415*pulse/334.0;

velocity = (pulse-prepulse)/344.0*60/mlTime;

lcd.setCursor(12,0); lcd.print((int)position1);

// lcd.setCursor(3,0); lcd.print(count);

lcd.setCursor(11,1); lcd.print((int)velocity);

prepulse = pulse;

Time = millis();

void setup()

lcd.init();

lcd.backlight();

lcd.setCursor(0,0); lcd.print("Phi(rad):");

lcd.setCursor(0,1); lcd.print("V(rpm):");

rotation = -1;

// dieu chinh chieu quay, quay thuan

Serial.begin(9600);

forward(rotation);

pinMode(chanelB, INPUT_PULLUP);

pinMode(chanelA, INPUT_PULLUP);

encoder1();

attachInterrupt(0, encoder1, FALLING); // Int0 = pin 2

float Time = millis();

void loop()

24
{

Display(1);

speedRate (1,3);

25