Thiết Kế Hệ Thống Điều Khiển Mô Phỏng Chuyển Động Cánh Tay.

BỘ GIÁO DỤC VÀ ĐÀO TẠO
TRƯỜNG ĐẠI HỌC BÁCH KHOA HÀ NỘI

---------------------------------------
Phạm Quốc Anh
THIẾT KẾ HỆ THỐNG ĐIỀU KHIỂN MÔ PHỎNG CHUYỂN ĐỘNG

CÁNH TAY
Chuyên ngành : KỸ THUẬT Y SINH
LUẬN VĂN THẠC SĨ KỸ THUẬT

KỸ THUẬT Y SINH
NGƯỜI HƯỚNG DẪN KHOA HỌC :

1. TS. Nguyễn Phan Kiên
Hà Nội – 2015.
-1-
Lời Cam Đoan
Tôi là PHẠM QUỐC ANH, học viên cao học lớp Kỹ Thuật Y Sinh khóa 2013 -
2015. Thầy giáo hướng dẫn là TS. Nguyễn Phan Kiên.
Tôi xin cam đoan toàn bộ nội dung được trình bày trong bản luận văn nay là
kết quả tìm hiểu và nghiên cứu của riêng tôi, trong quá trình nghiên cứu đề tài
“Thiết kế hệ thống điều khiển mô phỏng chuyển động cánh tay”. Các kết quả và dữ
liệu được nêu trong luận văn là hoàn toàn trung thực và rõ ràng. Mọi thông tin trích
dẫn đều được tuân theo luật sở hữu trí tuệ, liệt kê rõ ràng các tài liệu tham khảo. Tôi
xin chịu hoàn toàn trách nhiệm với những nội dung được viết trong luận văn này.
Hà nội, ngày 25 tháng 9 năm 2015.
Học viên.
Phạm Quốc Anh.
-2-
Mục Lục
Lời Cam Đoan .............................................................................................................2
Mục Lục ......................................................................................................................3
Danh mục ký hiệu và viết tắt.......................................................................................5
Danh mục hình vẽ .......................................................................................................6
Danh mục bảng biểu....................................................................................................7
Mở Đầu........................................................................................................................8
Chương 1: Tổng quan về điều khiển mô phỏng chuyển động cơ thể. ......................10
1.1 Định nghĩa về mô phỏng và điều khiên mô phỏng: .........................................10
1.2 Sơ lược về các hệ thống điều khiển mô phỏng chuyển động cánh. .................11
1.2.1 Sơ lược về các hệ thống điều khiển mô phỏng chuyển động cánh tay trên
thế giới ................................................................................................................12
1.2.2 Sơ lược về các hệ thống điều khiển mô phỏng chuyển động cánh tay ở
Việt Nam .............................................................................................................13
Chapter 2: Cơ sở lý thuyết của hệ thống điều khiển mô phỏng chuyển động cánh
tay. .............................................................................................................................15
2.1 Cánh tay và chuyển động của các bộ phận trên cánh tay.................................15
2.1.1 Cấu tạo cánh tay .........................................................................................15
2.1.2 Chuyển động của các phần trên cánh tay. ..................................................17
2.2 Cảm biến ghi nhận chuyển dộng. .....................................................................17
2.2.1 Cảm biến biến dạng, cảm biến dịch chuyển ..............................................19
2.2.2 Cảm biến gia tốc góc..................................................................................21
2.3 Giải thuật xử lý dữ liệu và lý thuyết điều khiển...............................................25
2.3.1 Giải thuật xử lý dữ liệu: .............................................................................26
2.3.2 Lý thuyết điều khiển ..................................................................................27
Chương 3: Thiết kế hệ thống điều khiển mô phỏng cho cánh tay. ...........................29
3.1 Giới thiệu chung về hệ thống điều khiển mô phỏng chuyển động cánh tay ....29
3.2 Khối cảm biến và nhận dạng chuyển động- Bộ cảm biến biến trở trượt và
MPU6050 ...............................................................................................................30
-3-
3.2.1 Bộ cảm biến biến trở thanh trượt ...............................................................30
3.2.2 Cảm biến gia tốc góc/ la bàn MPU6050 ....................................................32
3.3 Khối xử lý- Arduino Uno .................................................................................37
3.3.1 Thông số của Arduino UNO R3 .................................................................38
3.3.2. Chương trình điều khiển: ..........................................................................44
Chương 4: Kết quả và Bàn luận. ...............................................................................53
Tài Liệu tham khảo ...................................................................................................56
Phụ Lục .....................................................................................................................57
-4-
Danh mục ký hiệu và viết tắt
DMP Digital Motion Processor
EMG Electromyo Graphy
I2C Interfacing Intergrated Circuit
MEMS Microelectromechanical systems
PWM Pulse Width Modulation
SCL Serial Clock for I2C
SDA Serial Data for I2C
-5-
Danh mục hình vẽ
Hình 1-1. Minh họa cho hệ thống mô phỏng lắp ráp máy cơ khí và hệ 11
thống mô phỏng chuyển động cánh tay trên máy tính.
Hình 1-2. Bên trái: Cánh tay 22 dùng cho người khuyết tật với 22 bậc 13
tự do. Bên phải: Myo Gesture control band của Myo, Mỹ.
Hình 2-1. Cấu tạo xương cánh tay. 16
Hình 2-2. Cấu tạo xương cẳng tay. 17
Hình 2-3. Cấu tạo xương bàn tay. 17
Hình 2-4. Mô tả nguyên lý làm việc chung cả cảm biến. 19
Hình 2-5. So sánh bề mặt tiếp xúc của ngón tay và cẳng tay. 19
Hình 2-6. Nguyên lý làm việc của cảm biến gia tốc vi cơ. 23
Hình 2-7. Nguyên lý hoạt động của cảm biến gia tốc MEMS. 25
Hình 2-8. Tiến trình xử lý tín hiệu của hệ thống. 27
Hình 3-1. Sơ đồ khối của Hệ thống điều khiển mô phỏng chuyển động 30
cánh tay.
Hình 3-2. Hình minh họa cho bộ cảm biến biến trở thanh trượt 31
Hình 3-3. Nguyên lý hoạt động của biến trở. 1 là vị trí của thanh biến 31
trở, 2 là vị trí của con chạy trên biến trở
Hình 3-4. Trạng thái của ngón tay và trạng thái tương ứng của con 32
chạy biến trở.
Hinh 3-5. Sơ đô khối của cảm biến la bàn MPU6050. 34
Hình 3-6. Module cảm biến MPU6050 35
Hinh
Hình 33-8.
7 Vi
Cácđiều
cổngkhiển ATMEGA328
vào của Arduino Uno. 39
44
Hình 3-9. Kết nối giữa Arduino và MPU6050 theo giao thức I2C. 46
Hình 3-10. Minh họa cho kết nối giữa biến trở và Arduino. 50
Hình 4-1. Các cử động mô phỏng qua cơ cấu chấp hành của ngón tay 53
giữa.
-6-
Danh mục bảng biểu
Bảng 1-biểu diễn phạm vi góc hoạt động các khớp của chi trên 18
-7-
Mở Đầu
Trong giai đoạn phát triển như vũ bão của công nghệ điện tử y sinh nói
chung và chuyên ngành cơ sinh nói riêng, ngành cơ sinh Việt Nam đứng trước rất
nhiều thời cơ vận hội và thách thức mới trên con đường hội nhập với nền kinh tế thế
giới. Để đáp ứng nhu cầu phát triển của xã hội nhằm bắt kịp bước tiến của khoa học
kỹ thuật đòi hỏi đội ngũ các nhà khoa học, cán bộ kỹ thuật và công nhân lành nghề
phải không ngừng nghiên cứu, học tập nâng cao trình độ để kịp thời tiếp cận làm
chủ các kiến thức khoa học kỹ thuật hiện đại và công nghệ tiên tiến. Các khoá đào
tạo thạc sỹ tại Trường Đại học Bách Khoa Hà Nội nhằm đào tạo những cán bộ khoa
học có trình độ cao để tiếp thu và làm chủ kỹ thuật hiện đại để phục vụ cho công tác
nghiên cứu, giảng dạy. Để đánh giá kết quả học tập trong toàn khoá học tôi được
giao đề tài luận văn tốt nghiệp: “Thiết kế hệ thống điều khiển mô phỏng chuyển
động cánh tay” Trong quá trình phát triển của ngành điện tử y sinh, lĩnh vực cơ sinh
đang phát triển hết sức nhanh chóng, nhiều mặt của cuộc sống với nhiều mức, nhiều
quy mô và công nghệ hiện đại, tiên tiến nhằm đáp ứng được nhu cầu của xã hội hiện
nay. Trong đó phải kể đến sự tiến bộ vượt bậc của khoa học kỹ thuật, nhất là sự ra
đời của các hệ thống điều khiển mô phỏng thông minh đã tạo tiền đề cho sự phát
triển mạnh mẽ của nền kỹ công nghệ có tính chất tự động hoá cao, đã dần thay thế
sức lao động của con người đồng thời hiệu quả của nó đem lại cho cuộc sống là rất
lớn. Hiện nay sự xuất hiện của các cơ cấu điều khiển Robot trong đời sống sinh hoạt
đã trở nên phổ biến. Chúng được ứng dụng rộng rãi trong nhiều lĩnh vục khác nhau,
đặc biệt nổi lên hiện nay là trong ứng dụng điều khiển thực tại ảo và các ứng dụng
điều khiển mô phỏng chuyển động cơ thể con người. Chúng đóng vai trò quan
trọng, chúng vừa đảm bảo độ chính xác vừa đảm bảo tính tin cậy và an toàn mà với
con người hay những máy móc thông thường khó có thể đạt được. Đồng thời nó có
thể thay thế con người làm việc trong những môi trường độc hại, nơi con người khó
có thể đặt chân tới như vũ trụ Nói chung, ứng dụng điều khiển dựa vào chuyển động
cơ thể là một mảng công nghệ rất hứa hẹn, vì vậy mà trong tương lai đây sẽ là nhân
-8-
tố rất tiềm năng trong sự phát triển của các cuộc sống hiện đại. Do vậy việc nghiên
cứu các vấn đề về mô phỏng chuyển động mang tính thời sự. Để nghiên cứu và thiết
kế hệ thống điều khiển mô phỏng chuyển động cho cánh tay bằng sử dụng cảm biến
biến trở trượt và cảm biến la bàn trên nền Arduino, luận văn của tôi gồm bốn
chương: Chương 1: Giới thiệu chung về mô phỏng chuyển động của cơ thể. Chương
2: Cơ sở lý thuyết về cảm biến và điều khiển, Chương 3: Thiết kế hệ thống điều
khiển mô phỏng cho cánh tay. Chương 4: Kết quả và Bàn luận. Trong luận văn này,
tác giả tập trung chính vào mô phỏng vị trí chuyển động của cánh tay. Đề tài đã
được hoàn thành đúng thời hạn dưới sự hướng dẫn tận tình của TS. Nguyễn Phan
Kiên – Giảng viên Viện Điện Tử - Viễn Thông - Trường Đại học Bách Khoa Hà
Nội và các bạn đồng nghiệp cùng sự nỗ lực của bản thân. Tôi xin chân thành cảm
ơn thầy giáo hướng dẫn, các thầy giáo, cô giáo thuộc trường Đại học Bách Khoa Hà
Nội đã giúp đỡ tôi trong quá trình học tập cũng như quá trình nghiên cứu thực hiện
luận văn. Vì nhiều điều kiện khách quan và khả năng của bản thân, luận văn hoàn
thành chắc chắn còn thiếu sót. Rất mong sự góp ý của các thầy cô giáo và các bạn
đồng nghiệp.
-9-
Chương 1: Tổng quan về điều khiển mô phỏng chuyển động cơ thể.
1.1 Định nghĩa về mô phỏng và điều khiên mô phỏng:
Mô phỏng là việc nghiên cứu trạng thái của mô hình để qua đó hiểu được hệ
thống thực, mô phỏng là tiến hành thử nghiệm trên mô hình. Đó là quá trình tiến
hành nghiên cứu trên vật thật nhân tạo, tái tạo hiện tượng mà người nghiên cứu cần
để quan sát và làm thực nghiệp, từ đó rút ra kết luận tương tự vật thật. Ta có thể
thực hiện việc mô phỏng từ những phương tiện đơn giản như giấy, bút đến các
nguyên vật liệu tái tạo lại nguyên mẫu (mô hình bằng gỗ, nhựa, sợi thủy tinh…) kết
hợp với việc đo đạc đánh giá với mô hình trên máy tính, cũng như các thiết bị đo
đạc chuyên dụng.
Hình 1-1. Minh họa cho hệ thống mô phỏng lắp ráp máy cơ khí và hệ thống mô
phỏng chuyển động cánh tay trên máy tính.
Điều khiển mô phỏng sử dụng mô tả tham số đầu vào, mô hình của hệ thống
thực ở dạng tín hiệu điện tương tự hoặc số để ứng dụng trong điều khiển một hệ
thống xác định. Điều khiển mô phỏng thường được sử dụng rất có hiệu quả để
nghiên cứu trạng thái động của nguyên mẫu trong những điều kiện nếu nghiên cứu
trên vật thật sẽ khó khăn, tốn kém và không an toàn. Điều khiển mô phỏng chuyển
động cánh tay là điều khiển sử dụng tham số đầu vào hoặc các mô hình của cánh tay
ở dạng tín hiệu điện để điều khiển đặc tả cơ cấu chấp hành theo một chuyển động
- 10 -
nào đó của cánh tay. Điều khiển mô phỏng chuyển động cánh tay là một mảng nhỏ
trong điều khiển mô phỏng sinh học. Bên cạnh việc mô tả hình dáng bên ngoài của
cánh tay, các nhà khoa học hiện nay còn sử dụng phương pháp trích xuất đặc trưng
chuyển động của cánh tay thông qua đặc trưng tín hiệu EMG hoặc EEG tương ứng
với cử động. Đó không chỉ là một phương pháp tiếp cận mới mà còn là một cách
tiếp cận vô cùng khoa học, đi thẳng vào vấn đề của điều khiển là thực hiện các lệnh
theo ý của người điều khiển.
Thực tế, mô phỏng là một dạng tạo hiệu ứng, truyền sinh khí và chuyển động
cho những đối tượng khô khan. Mô phỏng chuyển động cánh tay và tính hiệu của
cánh tay là xu hướng phát triển mới của điều khiển học hiện đại, hiện đã và đang
được nghiên cứu và áp dụng rộng rãi trên nhiều lĩnh vực. Nó góp phần không nhỏ
trong quá trình tối ưu hóa các thao tác trong khi khai thác vận hành, sử dụng các hệ
thống. Thay vì việc sử dụng cơ chế điều khiển bằng bàn phím thông thường, người
điều khiển có thể áp dụng ngay những cử động của mình vào việc xử lý công việc
một cách đa dạng hơn. Tuy là phương pháp mới với nhiều ưu điểm nhưng do là
công nghệ ra đời sau nên việc điều khiển sử dụng mô phỏng chuyển động cánh tay
vẫn còn rất nhiều điểm tồn tại cần nghiên cứu thêm so với công nghệ panel điều
khiển hoặc bảng điểu khiển sử dụng nút bấm vốn đã có lịch sử phát triển hàng trăm
năm.
Trong lĩnh vực cơ sinh, việc thiết kế hệ thống điều khiển có ứng dụng mô
phỏng kết hợp cơ cấu chấp hành sẽ tạo cho sinh viên nhiều kỹ năng như: khả năng
hoạt động quan sát (các hình ảnh tĩnh hoặc động), khả năng thao tác trên đối tượng,
khả năng tự do phát triển tư duy, lựa chọn con đường tối ưu để nhận thức và phát
triển các hệ thống mới dựa trên hệ thống điều khiển thế hệ trước.
1.2 Sơ lược về các hệ thống điều khiển mô phỏng chuyển động cánh.
Các hệ thống điều khiển mô phỏng chuyển động hiện nay được ứng dụng rất
rộng rãi trong các lĩnh vực trong cuộc sống nhưng tập trung chính trong giải dạy tại
- 11 -
các trường đại học, trong các ngành như xây dựng, công nghiệp nhẹ, và đặc biệt là
trong y học. Các đột phá trong việc xử lý các tín hiệu y học đã tạo tiền đề cho việc
phát triên các phương thức mô phỏng rất chính xác và có khả năng đáp ứng cao với
nhiều điều kiện hoạt động.
1.2.1 Sơ lược về các hệ thống điều khiển mô phỏng chuyển động cánh tay trên
thế giới
Thế giới không còn quá xa lạ với các công nghệ điều khiển học và tự động
dựa vào sự mô phỏng sinh học các trạng thái của con người. Điển hình là tại Mỹ,
năm 2012, TS. Nitish Thakor cùng nhóm đồng sự đã tiến hành nghiên cứu, phát
triển và cho ra đời sản phẩm là cánh tay thay thế cho người khuyết tật với 22 bậc tự
do được điều khiển dựa trên tín hiệu thần kinh của con người. Với cách tiếp cận
trực tiếp tín hiệu điện não đồ EEG và xử lý chúng dựa trên nền tảng VSLI, hệ thống
điều khiển mô phỏng chuyển động này của TS Thakor và các đồng sự tại Khoa Y
Sinh trường ĐH John Hopkins đã cho thấy sự tiềm năng, cũng như tính hiệu quả khi
khai thác triệt để các thành tựu của các ngành cơ sinh, xử lý tín hiệu trên đối tượng
xử lý là tín hiệu sinh học. Bên cạnh đó cơ cấu chuyển động mô phỏng cánh tay này
cũng được đánh giá là có độ tin cậy cao khi thực hiện các động tác mô phỏng
chuyển động của cánh tay thật.
Hình 1-2. Bên trái: Cánh tay 22 dùng cho người khuyết tật với 22 bậc tự do. Bên
phải: Myo Gesture control band của Myo, Mỹ.
- 12 -
Bên cạnh ứng dụng dành cho ngành y tế, các hệ thống điều khiển mô phỏng
chuyển động còn có giá trị rất lớn trong lĩnh vực thực tại ảo. Điển hình là sản phẩm
Myo Gesture Control Armband của Hãng Myo – Mỹ. Là sản phẩm khai thác tín
hiệu sEMG – tín hiệu điện cơ bề mặt, Myo Gesture Control Armband đo đạc và
trích xuất các đặc trưng tín hiệu sEMG trong các chuyển động cánh tay, qua đó
phân tích và xử lý các tín hiệu này để thực hiện các lệnh thao tác đã được lập trình
trước. Thiết bị này có khả năng tương thích rất cao với các loại smart TV, smart
phone. Với nền tảng giao tiếp Bluetooth Smart và công nghệ cảm biến Medical
Grade EMG stainless steel – công nghệ cảm biến EMG y tế sử dụng điện cực sắt
không gỉ, Myo Gesture Control Armband có thể thực hiện các công việc thay thế
cho hầu hết các bộ điều khiển không dây có thể kết nối đến máy tính, điện thoại
thông minh và máy tính bảng.
Trên đây chỉ 2 là 2 ví dụ tiêu biểu về các hệ thống điều khiển mô phỏng
chuyển động cánh tay trên thế giới. Các sản phẩm này đã đánh dấu bước tiến mạnh
mẽ của công nghệ nhận diện tín hiệu sinh học trong lĩnh vực điều khiển.
1.2.2 Sơ lược về các hệ thống điều khiển mô phỏng chuyển động cánh tay ở
Việt Nam
Tại Việt Nam, các hệ thống điều khiển mô phỏng chuyển động cánh tay vẫn
chưa thực sự phát triển mạnh. Có thể điểm đến một số nghiên cứu của các nhóm
nghiên cứu như cánh tay cứu hộ các tai nạn công trình của Nghiên Cứu Sinh – Thạc
sỹ Đào Mạnh Hùng công bố năm 2012, cánh tay cứu hộ này hoạt động dựa trên các
cảm biến gia tốc nhằm mô phỏng lại chuyển động của cánh tay. Sử dụng động cơ
thủy lực trong cơ cấu chấp hành để nâng các vật nặng, và cơ cấu kẹp cang cua để
giữ các vật nặng này. Bên cạnh đó là các nghiên cứu của các nhóm sinh viên chuyên
ngành điện tử Tử Y sinh của các trương đại học Bách Khoa Hà Nội, Bách Khoa Đà
Nẵng khi nghiên cứu và mô phỏng các tín hiệu sEMG và EEG để thực hiện mô
phỏng chuyển động của cánh tay như nghiên cứu “thiết kế mô hình điều khiển cánh
tay 2 bậc tự do sử dụng tín hiệu điện cơ bề mặt” của nhóm sinh viên Nguyễn Minh
- 13 -
Phương và Phạm Quốc Anh,v.v.… Các nghiên cứu này mới chỉ dừng lại ở mức
nghiên cứu sơ khởi, các cơ cấu chấp hành còn nhiều thiết sót trong mô tả chuyển
động, tốc độ phản hồi, tốc độ xử lý của hệ thống xử lý còn thấp và chưa cao. Tuy
nhiên, tất cả các nghiên cứu này đều rất tiềm năng nếu được đầu tư và phát triển
đúng hướng.
Trên đây là vài nét về điều khiển mô phỏng cơ thể con người và sơ lược về
các hệ thống điều khiển mô phỏng chuyển động của cánh tay. Ta thấy được sự
phong phú trong các tiếp cận của các nhóm phát triển, tham số điều khiển có thể là
tín hiệu sEMG, EEG sinh ra trong quá trình chuyển động của cánh tay như cách tiếp
cận của Hãng Myo, có thể là các giá trị góc của cánh tay trong quá trình vận động
như cách tiếp cận của cánh tay cứu hộ. Qua đó ta có thể thấy sự tiềm năng của các
công nghệ mô phỏng thông qua các ứng dụng đa dạng mà chúng có thể đóng góp
đối với cuộc sống của chúng ta.
- 14 -
Chương 2: Cơ sở lý thuyết của hệ thống điều khiển mô phỏng chuyển động
cánh tay.
Với phương pháp tiếp cận là xác định độ biến dạng hình dáng của ngón tay
trong các cử động co-duỗi và sự thay đổi về vị trí dẫn đến sự thay đổi về góc của
cánh tay trong các chuyển động nhằm mô phỏng lại chuyển động của cánh tay.
Trong Chương 2 này, nội dung chính đề cập đến các vấn đề là cánh tay của con
người, cảm biến thu nhận tín hiệu của chuyển động cánh tay và thuật toán được
dùng cho vi điều khiển mô phỏng lại chuyển động cánh tay thông qua tín hiệu thu
nhận được từ cảm biến.
2.1 Cánh tay và chuyển động của các bộ phận trên cánh tay
2.1.1 Cấu tạo cánh tay
Cánh tay hay chi trên là bộ phận linh hoạt nhất của con người. Chi trên chịu
trách nhiệm cho hầu hết mọi hoạt động trong cuộc sống hàng ngày. Chi trên được
chia làm 3 phần : Cánh tay, cẳng tay và bàn tay.
Hình 2-1. Cấu tạo xương cánh tay.
Cánh tay là phần nối của chi trên với vai. Cấu tạo gồm 1 xương lớn là xương
cánh tay. Đầu nối với vai có cấu tạo giống như 1 khớp động dạng khớp tròn xoay.
Khớp động này có thể tạo 1 góc xoay 3 chiều rất linh hoạt lên tới 142 0.
- 15 -
Cẳng tay là phần nối giữa cánh tay và bàn tay. Cấu tạo gồm 2 xương chính là
xương quay và xương trụ. 2 xương này được nối với nhau bởi màng gian cốt. Cấu
tạo của cẳng tay khá đắc biệt cho phép cẳng thay thực hiện động tác xoay quanh
trục là xương trụ. Giới hạn xoay của cẳng tay quanh trục xương trụ lên tới 770.
Hình 2-2. Cấu tạo xương cẳng tay.
Bàn tay là phần cuối cùng của cánh tay, gồm 5 ngón tay. Trong đó, chỉ trừ
ngón cái có 2 xương phụ trách chuyển động, các ngón khác đều có 3 xương phụ
trách chuyển động. Chuyển động của 5 ngón tay này phối hợp tạo thành các thao tác
vô cùng tinh vi và phức tạp như cầm, nắm, bấm,v.v…..
Hình 2-3. Cấu tạo xương bàn tay.
- 16 -
2.1.2 Chuyển động của các phần trên cánh tay.
Bảng 1-biểu diễn phạm vi góc hoạt động các khớp của chi trên
Tên của hoạt động – Loại khớp tương ứng Phạm vi góc trung bình
1 Bẻ cổ tay – Khớp cổ tay (khớp động) 900
2 Gập cổ tay – Khớp cổ tay (khớp động) 990
3 Xoay cổ tay sang trái – Khớp cổ tay ( Khớp 270 với tay phải và 470 với
động) tay trái
4 Xoay cổ tay sang phải – Khớp cổ tay (Khớp 470 với tay phải và 270 với
động) tay trái
5 Gập cẳng tay – Khớp cẳng tay ( khớp động ) 1190
6 Xoay cẳng tay – Khớp cẳng tay (khớp động) 770
7 Xoay cánh tay – Khớp vai ( khớp động) 1420
2.2 Cảm biến ghi nhận chuyển dộng.
Trong các hệ thống điều khiển sử dụng lý thuyết điều khiển hiện đại, cảm
biến đóng vai trò rất quan trọng trong việc cung cấp giá trị đầu vào cho hệ thống.
Qua đó cảm biến là các thiết bị nhằm đo đạc các tín hiệu. Cảm biến ghi nhận
những trạng thái hay quá trình biến dạng dọc của bề mặt ngón tay trong quá trình co
duỗi ngón, cũng như trạng thái và quá trinh thay đổi của giá trị góc trên cánh tay; và
biến đổi thành tín hiệu điện để thu thập thông tin về trạng thái hay quá trình đó [1].
Thông tin được xử lý để rút ra tham số định tính hoặc định lượng của chuyển động,
phục vụ cho quá trình xác định đầu vào cho chương trình xử lý đã được viết sẵn để
mô tả chuyển đông theo thời gian thực.
- 17 -
Tín hiệu điện
Hình 2-4. Mô tả nguyên lý làm việc chung của cảm biến.
Do ngón tay là khu vực cơ thể có diện tích bề mặt tiếp xúc nhỏ nên việc lựa
chọn cảm biến gặp nhiều hạn chế. Đa phần các cảm biến có sẵn thường có kích
thước lớn hơn bề mặt tiếp xúc của ngón tay. Bên cạnh đó, sự không đồng nhất về
kích thước của ngón tay cũng làm tăng thêm sự hạn chế này. Đó chính là lý do, ta
cần sử dụng 2 loại cảm biến khác nhau cho 2 đối tượng đo là ngón tay và cánh tay.
Với ngón tay, ta sử dụng cảm biến biến dạng và cánh tay sử dụng cảm biến đo góc
hoặc cảm biến la bàn.
Hình 2-5. So sánh bề mặt tiếp xúc của ngón tay và cẳng tay.
- 18 -
2.2.1 Cảm biến biến dạng, cảm biến dịch chuyển
Đối với các cử động ngón tay cảm biến biến dạng có thể coi là một trong số
những phương pháp hiệu quả để mô phỏng chuyển động của ngón tay. Khi ngón tay
chuyển động, sự biến đổi diện tích trên bề mặt da do sự đàn hồi, yêu tố đó hoàn toàn
có thể là cơ sở để thiết kế một cảm biến. Bằng cách đo biến dạng và chuyển đổi
chúng thành tín hiệu ta có thể tính được sự thay đổi về diện tích, qua đó xác định
được chuyển động của ngón tay. Trong nghiên cứu này, ta tạm bỏ qua các biến dạng
vuông góc với chuyển động của ngón tay mà chỉ chú ý đến biến dạng dọc – biến
dạng dọc theo chuyển thay đổi của chuyển động.Khi đó ta có biến dạng ε: là tỉ số
giữa độ biến thiên kích thước (Δl) và kích thước ban đầu (l)
(2.1)
Do biến dạng bề mặt da là biến dạng đàn hồi, nên việc duy trì cơ cấu đo đảm
bảo tính đàn hồi hoặc tự phục hồi trạng thái ban đầu là việc vô cùng quan trọng.
Yêu cầu này đảm bảo cho việc xác định chính xác giá trị điện tích đặc trưng cho
trạng thái của ngón tay. Ví dụ: FS flex sensor của hãng Spectra Symbol là một thiết
bị khác hữu dụng trong việc đo đạc biến dạng dọc. Với điện trở ở trạng thái phẳng
không biến dạng là 10k Ohm, FS flex sensor có thể lên tới 110k Ohm khi được uốn
cong 1 góc là 90 0. Bên cạnh đó, do được làm bằng chất liệu dẻo có độ biến dạng
đàn hồi nên FS flex sensor dễ dàng tự trở lại trạng thái phẳng không biến dạng.
Nếu như việc sử dụng các cảm biến biến trở biến dạng là phương pháp hiệu
quả nhất song rào cản thực hiện áp dụng các biến trở này không hề nhỏ - nhiều thiết
bị và cảm biến trong lĩnh vực này không có sẵn trên thị trường. Do đó, thiết kế một
thiết bị tương đương về chức năng là cần thiết và cũng để thể hiện sự linh hoạt trong
cách tiếp cận đến vấn đề biến dạng trong thực tế. Thiết bị này cần thỏa mãn những
tính năng và tham số sau:
- 19 -
 Độ nhạy
 Độ chính xác
 Độ phân giải
 Độ chọn lọc
 Độ tuyến tính
 Công suất
 Dải tần
 Độ trễ
 Tốc độ đáp ứng
 Độ ổn định
Dựa trên những điều kiện cần thỏa mãn ở trên, một “cảm biến” dựa trên nền
biến trở là một lựa chọn khá hợp lý bởi độ nhạy của một số cảm biến khá cao ( sai
số của biến trở chính xác là từ 0.1~1%). Độ phân giải và tuyến tính của biến trở
cũng hoàn toàn phù hợp với tiêu chí của cảm biến. Trên thực tế có rất nhiều cảm
biến đề nghị sử dụng biến trở. Trong yêu cầu của hệ thống đo chuyển động của
ngón tay, các cử động co duỗi của ngón tay được tuyến tính hóa dựa vào vị trí của
vị trí con chạy trên biến trở. Về độ ổn định và tốc độ đáp ứng, biến trở có độ ổn
định khá cao và tốc độ đáp ứng gần như là tức thì khi xuất hiện dòng điện trong
biến trở. Hoàn toàn có hiểu được độ ổn định cao của biến trở, bởi trong số các linh
kiện điện tử, điện trở và biến trở là các linh kiện có độ ổn định cao nhất. Đồng thời,
ưu điểm lớn nhất của biến trở nói chung, và biến trở thanh trượt nói riêng là chúng
ta có thể dễ dàng sử dụng phương pháp điều chế độ rộng xung (PWM). Ở phần 2.3,
chúng ta sẽ đi sâu hơn về phương pháp điều chế độ rộng xung.
- 20 -
Tuy nhiên trên thực tế do chất liệu của các biến trở là không đồng nhất, độ
đồng đều không cao nên tồn tại những vấn đề sau đối với biến trở thanh trượt cơ
học:
 Tỉ số Rx/Rn của con chạy ở 2 đầu biến trở là không đồng nhất.
 Tiếp xúc giữa con chạy và thanh biến trở tham chiếu thay đổi giảm dần theo
thời gian, dẫn đến sự già hóa cảm biến nhanh nếu dùng hoạt động liên tục.
2.2.2 Cảm biến gia tốc góc
Đối với các cử động của cánh tay, việc sử dụng cảm biến gia tốc mà đặc biệt
là gia tốc góc là phương pháp vô cùng hiệu quả. Do đây là khu vực thực hiện các cử
động lớn và là khu vực có bề mặt tiếp xúc lớn, việc gắn các cảm biến gia tốc vào bề
mặt da để đo đạc góc chuyển động là khả thi và có độ tin cậy cao. Trong các công
nghệ của cảm biến gia tốc, chúng ta sẽ đề cập và chú trọng vào công nghệ vi cơ điện
tử.
Hệ thống vi cơ điện tử (Micro Electro Mechanicals Systems – MEMS) là

công nghệ cho là sự tích hợp các cấu trúc vi cơ, vi cảm biến, vi chấp hành, vi điện
tử thành một hệ thống trên cùng một đế silicon bằng công nghệ vi chế tạo. Một
thiết bị MEMS thông thường là hệ thống vi cơ tích hợp trên một chip kết hợp những
phần chuyển động cơ học với những yếu tố sinh học, hóa học, quang hoặc điện. Do
đó các linh kiện MEMS có thể đáp ứng với nhiều loại đầu vào như hóa học, ánh
sáng, áp suất, chuyển động, góc quay, từ tính… Với ưu thế có thể tạo ra những cấu
trúc cơ học nhỏ và nhạy, công nghệ vi cơ điện tử cho phép tạo ra những bộ cảm
biến (sensor), bộ chấp hành (actuator) được ứng dụng rộng rãi trong đời sống, y tế,
công nghiệp...
- 21 -
Hình 2-6. Nguyên lý làm việc của cảm biến gia tốc vi cơ
Các ưu điểm và nhược điểm của MEMS:
- MEMS có các ưu điểm chính so với các thiết bị cơ học truyền thống.
+ Trước tiên là khả năng sản xuất số lượng lớn dễ dàng, cùng với sự phát
triển của công nghệ vi điện tử, ngày nay công nghệ VLSI cho phép sản xuất các
thiết bị MEMS với số lượng lớn (lên đến 100,000 thiết bị trên 1 đế Poly- Si), điều
mà khó có thể đạt được với các thiết bị cơ học bình thường.
+ Do có thể sản xuất số lượng lớn, nên giá cho mỗi đơn vị rẻ hơn.
+ Các thiết bị chế tạo bằng công nghệ vi điện tử, nên kích thước bé, hiệu
năng cao, tiêu thụ ít năng lượng, dễ dàng tích hợp vào các thiết bị khác.
+ Các thành phần cơ học nhỏ gọn, ít bị ảnh hưởng bởi các tác động gây hại
từ môi trường nên bền hơn.
- Tuy vậy, các thiết bị MEMS cũng có những giới hạn và nhược điểm của mình.
Dưới đây là một số nhược điểm của các cảm biến MEMS:
+ Do kích thước bé nên các thành phần vật lý của MEMS không chịu được
các lực tác động lớn.
+ Có thể bị ảnh hưởng bởi các nhiễu điện từ…
- 22 -
Cảm biến gia tốc chế tạo theo công nghệ vi cơ điện tử có hai loại là cảm biến
kiểu tụ và cảm biến kiểu áp trở. Trong nhiều ứng dụng việc lựa chọn cảm biến kiểu
tụ hay kiểu áp trở là rất quan trọng. Cảm biến kiểu áp trở có ưu điểm là công nghệ
cấu tạo rất đơn giản. Tuy nhiên nhược điểm của nó là hoạt động phụ thuộc nhiều
vào sự thay đổi nhiệt độ và có độ nhạy kém hơn cảm biến kiểu tụ. Các cảm biến
kiểu tụ có độ nhạy cao hơn, ít bị phụ thuộc vào nhiệt độ, ít bị nhiễu và mất mát năng
lượng. Tuy nhiên chúng có nhược điểm là mạch điện tử phức tạp hơn. Hiện nay
cảm biến gia tốc kiểu tụ được ứng dụng rộng rãi hơn.
Ứng dụng của cảm biến gia tốc
Cảm biến gia tốc vi cơ đã nhanh chóng thay thếcác loại cảm biến gia tốc thông
thường trước đây trong nhiều ứng dụng. Một vài những ứng dụng điển hìnhcủa cảm
biến gia tốc vi cơ.
 Cảm biến góc Roll –Pitch, góc Euler, góc
 Định hướng 3D trong không gian
 Phát hiện va chạm : những thông tin về gia tốc, vận tốc và độ dịch chuyển
giúp phân biệt sự va chạm và việc không xảy ra va chạm
 Đo và điều khiển mức rung
 Điều khiển và dự đoán khả năng làm việc của máy móc, thiết bị
 Đo một số thông số sinh học trong cơ thể con người.
Nguyên tắc hoạt động của cảm biến vi cơ MEMS dạng tụ
Việc đo gia tốc thông qua cảm biến gia tốc MEMS có thể được mô tả theo
một sơ đồ mô tả dưới đây, gồm một hệ gồm một khối lượng m và một lò xo.
- 23 -
Hình 2-7. Nguyên lý hoạt động của cảm biến gia tốc MEMS.
Khi hệ quy chiếu được gia tốc, gia tốc này được truyền cho khối m thông qua
lò xo. Lò xo giãn ra và độ dịch chuyển này được xác định bởi một cảm biến độ dịch
chuyển.
Theo định luật Hooke, lực kéo khối lượng m tỉ lệ với độ biến dạng của lò xo F =
k.x, với k là hệ số tỉ lệ hay độ cứng của lò xo, x là khoảng dịch chuyển so với vị trí
cân bằng. Theo định luật II Newton, trong hệ quy chiếu quán tính đứng yên, lực F
này của khối lượng m có một gia tốc a theo công thức F = ma. Tại vị trí cần bằng ta
có F = ma = kx.
Hệ thống có thể được mô tả bởi phương trình vi phân sau:
(3.2)
với D là hệ số ma sát.
Do đó, chúng ta thu được a=F/m trong hệ quy chiếu quán tính đứng yên. Như vậy,
để đo gia tốc ta chỉ cần đo khoảng dịch chuyển x. Để đo khoảng dịch chuyển này,
người ta có thể sử dụng thuộc tính điện của tụ điện có hai bản cực song song
khoảng cách giữa hai bản tụ có thể thay đổi được. Điện dung của tụ điện đơn là C=
k/x0, với k là hằng số phụ thuộc vào thuộc tính của môi trường nằm giữa hai bản tụ.
Nếu biết k, điện dung của tụ điện C ta có thể tính được x0.
- 24 -
Nếu bản tụ nằm giữa CA và CB dịch chuyển một khoảng là x thì:
(3.3)
hay có thể viết lại là:
(3.4)
Do đó,
(3.5)
Với khoảng x dịch chuyển nhỏ, phương trình trên có thểrút gọn thành:
(3.6)
Như vậy, nếu gắn khối lượng m của cảm biến vào bản tụ nằm giữa hệ hai tụ
điện nối tiếp thì có thể xác định được độ dịch chuyển của nó dưới tác dụng của lực
F, tức là xác định được gia tốc thông qua việc xác định giá trị ∆C.
Nguyên lý này phù hợp với việc đo chuyển động lớn, khi giá trị của gia tốc
chuyển động ảnh hưởng trực tiếp lên độ biến dạng của điện dung. Độ biến thiên của
giá trị cảm biến lúc này hoàn toàn có thể loại bỏ các ảnh hưởng của môi trường như
độ ẩm và nhiệt độ, do cơ thể có cơ chế duy trì nhiệt độ bề mặt da khá tốt.
2.3 Giải thuật xử lý dữ liệu và lý thuyết điều khiển.
Việc thực hiện thu nhận và xử lý dữ liệu phụ thuộc chính vào phương pháp
tiếp cận của nghiên cứu, khi áp dụng lý thuyết điều khiển hiện đại nhằm đảm bảo
- 25 -
tính liên tục của thời gian, việc áp dụng các cấu trúc và thư viện được xây dựng sẵn
nhằm tăng tốc độ phản ứng của hệ thống tiệm cận với chế độ thời gian thực ( real-
time mode). Khác với các hệ thống sử dụng nguyên lý điều khiển cổ điển như PID,
giới hạn của hệ thống điều khiển hiện đại nằm chủ yếu ở phần cứng. Các giới hạn sẽ
được bảo vệ bởi các cơ chế, các mạch cứng “bảo vệ” (shield).
2.3.1 Giải thuật xử lý dữ liệu:
Là hệ thống điều khiển trực tiếp không phản hồi, giải thuật được thực hiện
theo tiến trình như sau:
• Sensor ghi nhận các trạng thái thay đổi của đối tượng đo.
Thu nhận dữ •Tiền xử lý các dữ liệu để tăng tốc quá trình xử lý nếu có.
liệu
• Dữ liệu analog được xử lý trực tiếp trên module ADC của sensor hoặc vi điều
khiển
Chuyển đổi và •Thực hiện các thao tác chuyển đổi dữ liệu theo chương trình định sẵn và thực hiện
xử lý dữ liệu các lệnh dựa vào giá trị đầu vào.
• Dựa vào giá trị đã được xử lý, VĐK so sánh với mức giá trị đầu ra tương đương để
thực hiện lệnh điều khiển cơ cấu chấp hành, hoặc lệnh hiển thị lên màn hình thông
Thực hiện qua giao thức được định sẵ trong chương trình.
thao tác
Hình 2-8. Tiến trình xử lý tín hiệu của hệ thống.
Theo lưu đồ giải thuật trên, tiến trình xử lý của hệ thống là tiến trình không
có phản hồi, mọi thao tác của hệ thống và cơ cấu chấp hành được thực hiện liên tục
theo một chương trình được xây dựng sẽ. Trong trường hợp này các dữ liệu được
thao tác theo 3 bước chính:
 Cảm biến thu nhận dữ liệu tương tự từ môi trường, số hóa chúng theo
nguyên tắc làm việc của nó. Với các cảm biến được tích hợp khối chuyển đổi
- 26 -
tương tự số. Các giá trị tương tự được quy đổi thành giá trị số theo độ phân
giải định trước của cảm biến.
 Tín hiệu số từ cảm biến sẽ được gửi về cổng giao tiếp của vi điều khiển.
Theo từng nhóm dữ liệu đầu vào định sẵn, các vi điều khiển sẽ thực hiện các
chuyển đổi các giá trị này thành các giá trị trung gian để chuẩn bị cho quá
trình thực thi lệnh đối với cơ cấu chấp hành hoặc cơ cấu hiển thị. Quá trình
này sẽ so sánh giá trị của tín hiệu điện từ cảm biến, qua đó xác định được
trạng thái tiếp theo của cơ cấu điều khiển, chấp hành hoặc hiển thị.
 Thực thi lệnh: sau khi được xử lý, chuyển đổi dữ liệu đầu vào; vi điều khiển
sẽ thực thi việc xuất tín hiệu đầu ra để các cơ cấu chấp hành, hiện thị thực
hiện lệnh tương tự với giá trị của đầu vào tương ứng.
Toàn bộ quá trình này cần đảm bảo sự liên tục trong ngưỡng của điều khiển
thời gian thực.
2.3.2 Lý thuyết điều khiển
Với số lượng đầu vào các biến của chuyển động là 5 biến cho 5 ngón tay và
2*n biến cho chuyển động lớn của cánh tay (n=1, 2, 3) tương ứng với cặp 2 cử động
lớn của cổ tay, khớp khuỷu tay, và khớp bả vai. Thay vì quy định chuyển động
chung cho cả hệ thống điều khiển mô phỏng dựa vào ma trận đầu vào (2*n+5) x 1
giá trị. Chúng ta sẽ thực hiện mô phỏng riêng lẻ từng chuyển động với các cảm biến
đặt tương ứng với các vị trí tạo ra điều khiển và thực hiện mô phỏng trực tiếp trạng
thái thông qua việc điều chế độ rộng xung của tín hiệu và cơ chế servo.
Phương pháp điều xung PWM (Pulse Width Modulation) là phương pháp
điều chỉnh điện áp ra tải, hay nói cách khác, là phương pháp điều chế dựa trên sự
thay đổi độ rộng của chuỗi xung vuông, dẫn đến sự thay đổi điện áp ra. Các PWM
khi biến đổi thì có cùng 1 tần số và khác nhau về độ rộng của sườn dương hay sườn
âm.
- 27 -
PWM được ứng dụng nhiều trong điều khiển. Điển hình nhất mà chúng ta
thường hay gặp là điều khiển động cơ và các bộ xung áp, điều áp... Sử dụng PWM
điều khiển độ nhanh chậm của động cơ hay cao hơn nữa, nó còn được dùng để điều
khiển sự ổn định tốc độ động cơ. Ngoài lĩnh vực điều khiển hay ổn định tải thì
PWM còn tham gia và điều chế các mạch nguồn như : boot, buck, nghịch lưu 1 pha
và 3 pha...
PWM còn gặp nhiều trong thực tế ở các mạch điện điều khiển. Điều đặc biệt
là PWM chuyên dùng để điều khiển các phần tử điện tử công suất có đường đặc tính
là tuyến tính khi có sẵn 1 nguồn 1 chiều cố định .Như vậy PWM được ứng dụng rất
nhiều trong các thiết bị điện- điện tử. PWM cũng chính là nhân tố mà các đội
Robocon sử dụng để điều khiển động cơ hay ổn định tốc độ động cơ.
Với việc sử dụng biến trở, ta có thể đơn giản xác định được chế độ điều chế
xung của cơ cấu điều khiển của chuyển động ngón tay theo công thức:

 


 

Trong đó, Rx là điện trở hiện thời của biến trở và Rbiến trở là giá trị định danh



của biến trở. Khi đó, tỉ lệ sẽ tương đương với hệ số điều chỉnh D trong

điều chế độ rộng xung vuông. Cơ chế điều khiển này có ưu điểm rất mạnh trong
điều khiển động cơ bởi đây gần như là cơ cấu điều khiển khá phổ biến cho các hệ
thống robot, và không đòi hỏi người sử dụng cần có nhiều kiến thức về điều khiển
học.
- 28 -
Chương 3: Thiết kế hệ thống điều khiển mô phỏng cho cánh tay.
Dựa vào các cơ sở lý thuyết được phân tích trong chương 2, chương 3 sẽ tập
trung vào phần thực hiên các yêu cần thiết để có thể thiết kế mô hình hợp lý nhất
phù hợp điều kiện môi trường nghiên cũng như tạo ra được kết quả khả thi nhất.
3.1 Khái quát về hệ thống điều khiển mô phỏng chuyển động cánh tay
Với các cơ sở trong chương 2, hệ thống điều khiển mô phỏng chuyển động
cánh tay sẽ gồm 2 khối chính là khối cảm biến và khối xử lý. Ngoài ra, trong khuôn
khổ mục đích mô phỏng, chúng ta cũng thực hiện xây dựng mô hình mô phỏng cánh
tay với một số yêu cầu kỹ thuật nhất định. Dưới đây là sơ đồ khối của hệ thống điều
khiển mô phỏng chuyển động cánh tay:
Chuyển động của

cánh tay
Cử động nhỏ của ngón tay Cử động lớn của cánh tay
PHỎNG CHUYỂN ĐỘNG CÁNH

HỆ THỐNG ĐIỀU KHIỂN MÔ
Bộ cảm biến biến trở trượt Cảm biến la bàn MPU6050

TAY
VI XỬ LÝ/VI ĐIỀU KHIỂN
Tín hiệu được điều chế độ rông xung

PWM
Hình 3-1. Sơ đồ khối của Hệ thống điều khiển mô phỏng chuyển động cánh tay.
- 29 -
Các lựa chọn cụ thể với các khối như sau:
 Khối cảm biến và nhận dạng chuyển động: Bộ cảm biến biến trở trượt và
cảm biến la bàn MPU6050
 Khối xử lý: Kit Arduino Uno R3 và module Arduino Nano.
3.2 Khối cảm biến và nhận dạng chuyển động- Bộ cảm biến biến trở trượt và
MPU6050
Là khối chức năng chính để thu nhận các chuyển động và cử động của cơ
thể, khối cảm biến và nhận dạng chuyển động thu thập dữ liệu trạng thái của các
ngón tay và trạng thái của cẳng tay, cánh.
3.2.1 Bộ cảm biến biến trở thanh trượt
Hình 3-2. Hình minh họa cho bộ cảm biến biến trở thanh trượt
Bộ cảm biến biến trở trượt được hình thành theo yêu cầu đo biến dạng của
ngón tay. Với nguyên lý chính dựa vào nguyên lý của cảm biến biến dạng và cảm
biến di chuyển. Với việc đồng nhất hiệu điện thế của 5 biến trở, ta có thể xác định
được chuyển động tương đương của các ngón tay thông quá trình phân áp trên điện
trở thanh trượt
- 30 -




  (3.1)

Trong đó, Rx là điện trở hiện thời của biến trở và Rbiến trở là giá trị định danh
của biến trở.
Hình 3-3. Nguyên lý hoạt động của biến trở. 1 là vị trí của thanh biến trở, 2 là vị
trí của con chạy trên biến trở
Nguyên lý hoạt động của bộ cảm biến biến trở
Biến trở trượt được cố định 1 đầu hoàn toàn vào cổ tay hoặc 1 điểm nằm trên
cánh tay, con chạy của biến trở được nối vào đầu ngón tay bằng một dây không có
tính đàn hồi và được giằng cố định bằng một dây cao su có độ đàn hồi co dãn xác
định.
Hình 3-4. Trạng thái của ngón tay và trạng thái tương ứng của con chạy biến
trở.
Chọn gốc giá trị R x= Rbiến trở (giá trị biến trở trong nghiên cứu này là 10k
Ohm, do dòng điện khi đi qua biến trở luôn được duy trì ở ngưỡng an toàn với vi
- 31 -
điều khiển); khi đó điện thế phân áp trên biến trở = 0V tương ứng với khi ngón tay
duỗi thoải mái. Khi ngón tay co duỗi, do tính chất không đàn hồi của dây nối và tính
chất đàn hồi của da, con chạy của biến trở sẽ bị dây nối dịch chuyển một đoạn Δl
tương đương với độ biến dạng theo chiều chuyển động của bề mặt ngón tay. Thực
nghiệm đo cho thấy, Δl và U ngón tay tỷ lệ thuận với nhau tương đương với giá trị
theo công thức phân áp. Qua đó, chúng ta hoàn toàn có thể xác định được giá trị
điện thế tương đương với các cử động của ngón tay. Ngoài ra,
Chức năng của dây nối không biến dạng là xác định khoảng biến dạng của
ngón tay là Δl; dây cao su có chức năng duy trì hình dáng và vị trí của con chạy
biến trở, đồng thời tái tạo tính đàn hồi của hệ thống mô phỏng. Dây cao su này cho
phép hệ thống cảm biến con trở duy trì độ dịch chuyển của biến trở tương đương
với biến dạng dọc của ngón tay trong các chuyển động co duỗi.
3.2.2 Cảm biến gia tốc góc/ la bàn MPU6050
MPU6050 là cảm biến của hãng InvenSense. MPU6050 là một trong những
giải pháp cảm biến chuyển động đầu tiên trên thế giới có tới 6 (mở rộng tới 9) trục
vảm biến tích hợp trong 1 chip duy nhất. Với kích thước nhỏ gọn thích hợp với việc
gắn cố định lên cánh tay. Đây là một lựa chọn hợp lý cho hệ thống mô phỏng
MPU6050 sử dụng công nghệ độc quyền MotionFusion của InvenSense có

thể chạy trên các thiết bị di động, tay điều khiển,... Nó được điều hành bằng một
nguồn cung cấp 3.3V/5V và giao tiếp thông qua I2C với tốc độ tối đa 400kHz. Chip
này cũng có sẵn một gói SPI được gọi là MPU6000 cho tốc độ giao tiếp lên tới
10Mbs.
Thông số truyền động:
 Có thể lựa chọn ±2/4/8/16g phạm vi gia tốc.

 Có thể lựa chọn ±250/500/1000/2000 độ /s phạm vi con quay hồi chuyển.
 16 bit đầu ra
- 32 -
 Con quay nhạy cảm của gia tốc tuyến tính 0.1 độ/s, một cải tiến lớn so với
con quay hồi chuyển trục tri của các công ty khác.
 Chịu ảnh hưởng của nhiễu thấp trên cả hai kết quả đầu ra (xem thông số kỹ
thuật).
 Tỷ lệ sản lượng dữ liệu lên đến 1000Hz, mặc dù được xây dựng bằng kỹ
thuật số thấp vượt qua bộ lọc có tần số góc tối đa là 256Hz.
 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, MPU6050 còn có 1 đơn vị tang tốc phần cứng chuyên xử lý
chuyển động số (Digital Motion Processor – DMP) 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, giảm độ trễ của tốc độ đáp ứng, cải thiện tốc độ xử lý; và kết quả
là tốc độ phản hồi của khối cảm biến sẽ được cải thiện đáng kể. Đó chính là ưu
điểm lớn của MPU6050 so với khá nhiều cảm biến gia tốc khác.
Hinh 3-5. Sơ đô khối của cảm biến la bàn MPU6050.
- 33 -
Các cảm biến bên trong MPU6050 sử dụng bộ chuyển đổi tương tự - số
( Analog to Digital Converter – ADC) 16-bit cho ra kết quả chi tiết về góc quay với
độ phân giải cao. Với 16-bit, chúng ta có giải giá trị thu được lên tới 65536 mức, tất
cả chỉ cần 1 cảm biến
Một tính năng của chip này là bộ xử lý chuyển động kỹ thuật số trên máy bay
(DMP). Trong lý thuyết này có thể được sử dụng để sản xuất trực tiếp các góc
Euler, Quaternions, hoặc một hướng cosin ma trận, và thậm chí thực hiện lọc cùng
với việc tích hợp dữ liệu một la bàn I2C bên ngoài.
MPU6050 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.
Hơn nữa, MPU6050 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 dữ liệu sau khi MPU6050 tính toán xong.
Chân giao tiếp MPU6050.
Hình 3-6. Module cảm biến MPU6050
VCC 2.375V – 3.46V
GND 0V
- 34 -
SCL Chân SCL trong giao tiếp I2C
SDA Chân SDA trong giao tiếp I2C
XDA Chân dữ liệu (kết nối với cảm biến khác)
XCL Chân xung (kết nối với cảm biến khác)
AD0 Bito của địa chỉ I2C
INT Chân ngắt
MPU-6050 sẽ giúp ta xác định được góc nghiêng của thiết bị với phương
nằm ngang mặt đất (Accelerometer) cũng như xác định được vận tốc góc theo các
trục tọa độ( Gyro). Việc tích hợp 2 bộ cảm biến trên vào trong con MPU-6050 sẽ
tận dụng ưu điểm và hạn chế nhược điểm của 2 bộ cảm biến gia tốc và cảm biến
góc.
Cảm biến gia tốc giúp ta đo được chính xác góc nghiêng, nhưng lại bị ảnh
hưởng rất mạnh bởi nhiễu,rung động của thết bị, còn cảm biến góc thì giúp ta xác
định được vận tốc góc, từ đó suy ra được góc nghiêng, và không bị ảnh hưởng bởi
rung lắc của thiết bị, nhưng lại bị trôi điểm 0, và hoạt động sau một thời gian thì
tích lũy sai số rất lớn. Cho nên việc kết hợp 2 bộ trên là rất hợp lý.
MPU-6050 có rất nhiều thanh ghi, có những thanh ghi là để thiết lập chế độ
hoạt động cho nó, và có những thanh ghi chứa các dữ liệu cảm biến mà ta cần phải
đọc được giá trị của nó qua I2C để từ đó tính toán được góc nghiêng.
Việc giao tiếp I2C với MPU-6050 được nói đến trong datasheet của MPU-
6050 như sau:
 Để đọc một byte:
- 35 -
 Để đọc nhiều byte một lúc:
Tương tự đó là việc ghi 1 hay nhiều byte vào các thanh ghi của MPU-6050:
Trong đó:
 S: bit Start
 AD :địa chỉ của MPU-6050
 W: bit xác định là đọc ghi liệu
 R: bit xác địnhlà đọc dữ liệu
 ACK: bit Acknowledge
 N-ACK :Not-Acknowledge
 RA: địa chỉ thanh ghi mà ta muốn đọc
 DATA: dữ liệu truyền di hoặc đọc được
 P: bit Stop
- 36 -
Rõ ràng là để giao tiếp I2C với các chip khác nhau thì cách thức là khác
nhau, nhưng đều dựa trên chuân I2C, ở đây ta thấy là có thể phải gửi 2 lần bit Start,
trong một phiên giao tiếp phải gửi cả bit Write đầu tiên sau đó lại lại bit Read.
Ta chỉ có thể biết rõ điều này khi tham khảo datasheets.
3.3 Khối xử lý- Arduino Uno
Một mạch Arduino bao gồm một vi điều khiển AVR với nhiều linh kiện bổ
sung giúp dễ dàng lập trình và có thể mở rộng với các mạch khác. Một khía cạnh
quan trọng của Arduino là các kết nối tiêu chuẩn của nó, cho phép người dùng kết
nối với CPU của board với các module thêm vào có thể dễ dàng chuyển đổi, được
gọi là shield. Vài shield truyền thông với board Arduino trực tiếp thông qua các
chân khách nhau, nhưng nhiều shield được định địa chỉ thông qua hệ thống bus nối
tiếp I2C-nhiều shield có thể được xếp chồng và sử dụng dưới dạng song song.
Arduino chính thức thường sử dụng các dòng chip megaAVR, đặc biệt là ATmega8,
ATmega168, ATmega328, ATmega1280, và ATmega2560. Một vài các bộ vi xử lý
khác cũng được sử dụng bởi các mạch Aquino tương thích. Hầu hết các mạch gồm
một bộ điều chỉnh tuyến tính 5V và một thạch anh dao động 16 MHz (hoặc bộ cộng
hưởng ceramic trong một vài biến thể), mặc dù một vài thiết kế như LilyPad chạy
tại 8 MHz và bỏ qua bộ điều chỉnh điện áp onboard do hạn chế về kích cỡ thiết bị.
Một vi điều khiển Arduino cũng có thể được lập trình sẵn với một boot loader cho
phép đơn giản là upload chương trình vào bộ nhớ flash on-chip, so với các thiết bị
khác thường phải cần một bộ nạp bên ngoài. Điều này giúp cho việc sử dụng
Arduino được trực tiếp hơn bằng cách cho phép sử dụng 1 máy tính gốc như là một
bộ nạp chương trình.
Theo nguyên tắc, khi sử dụng ngăn xếp phần mềm Arduino, tất cả các board
được lập trình thông qua một kết nối RS-232, nhưng cách thức thực hiện lại tùy
thuộc vào đời phần cứng. Các board Serial Arduino có chứa một mạch chuyển đổi
giữa RS232 sang TTL. Các board Arduino hiện tại được lập trình thông qua cổng
- 37 -
USB, thực hiện thông qua chip chuyển đổi USB-to-serial như là FTDI FT232. Vài
biến thể, như Arduino Mini và Boarduino không chính thức, sử dụng một board
adapter hoặc cáp nối USB-to-serial có thể tháo rời được, Bluetooth hoặc các
phương thức khác. (Khi sử dụng một công cụ lập trình vi điều khiển truyền thống
thay vì ArduinoIDE, công cụ lập trình AVR ISP tiêu chuẩn sẽ được sử dụng.)
Nhắc tới dòng mạch Arduino dùng để lập trình, cái đầu tiên mà người ta
thường nói tới chính là dòng Arduino UNO. Hiện dòng mạch này đã phát triển tới
thế hệ thứ 3 (R3).
3.3.1 Thông số của Arduino UNO R3
Vi điều khiển ATmega328 (họ 8bit)
Điện áp hoạt động 5V – DC (chỉ được cấp qua cổng USB)
Tần số hoạt động 16 MHz
Dòng tiêu thụ 30mA
Điện áp vào khuyên dùng 7-12V – DC
Điện áp vào giới hạn 6-20V – DC
Số chân Digital I/O 14 (6 chân PWM)
Số chân Analog 6 (độ phân giải 10bit)
Dòng tối đa trên mỗi chân I/O 30 mA
Dòng ra tối đa (5V) 500 mA
- 38 -
Dòng ra tối đa (3.3V) 50 mA
32 KB (ATmega328) với 0.5KB dùng bởi

Bộ nhớ flash bootloader
SRAM 2 KB (ATmega328)
EEPROM 1 KB (ATmega328)
Vi điều khiển
Hinh 3-7. Vi điều khiển ATMEGA328
Arduino UNO có thể sử dụng 3 vi điều khiển họ 8bit AVR là ATmega8,

ATmega168, ATmega328. Bộ não này có thể xử lí những tác vụ đơn giản như điều
khiển đèn LED nhấp nháy, xử lí tín hiệu cho xe điều khiển từ xa, làm một trạm đo
nhiệt độ - độ ẩm và hiển thị lên màn hình LCD,… Thiết kế tiêu chuẩn của Arduino
UNO sử dụng vi điều khiển ATmega328. Tuy nhiên nếu yêu cầu phần cứng không
cao, có thể sử dụng các loại vi điều khiển khác có chức năng tương đương như
ATmega8 (bộ nhớ flash 8KB) hoặc ATmega168 (bộ nhớ flash 16KB):
- 39 -
Chế tạo thủ công Sử dụng mạch in
Năng lượng
Arduino UNO có thể được cấp nguồn 5V thông qua cổng USB hoặc cấp
nguồn ngoài với điện áp khuyên dùng là 7-12V DC và giới hạn là 6-20V. Thường
thì cấp nguồn bằng pin vuông 9V là hợp lí nhất nếu không có sẵn nguồn từ cổng
USB. Nếu cấp nguồn vượt quá ngưỡng giới hạn trên sẽ làm hỏng Arduino UNO.
Các chân năng lượng
 GND (Ground): cực âm của nguồn điện cấp cho Arduino UNO. Khi dùng
các thiết bị sử dụng những nguồn điện riêng biệt thì những chân này phải
được nối với nhau.
 5V: cấp điện áp 5V đầu ra. Dòng tối đa cho phép ở chân này là 500mA.
 3.3V: cấp điện áp 3.3V đầu ra. Dòng tối đa cho phép ở chân này là 50mA.
 Vin (Voltage Input): để cấp nguồn ngoài cho Arduino UNO, nối cực dương
của nguồn với chân này và cực âm của nguồn với chân GND.
 IOREF: điện áp hoạt động của vi điều khiển trên Arduino UNO có thể được
đo ở chân này. Lưu ý hạn chế lấy nguồn 5V từ chân này để sử dụng bởi chức
năng của nó không phải là cấp nguồn.
- 40 -
 RESET: việc nhấn nút Reset trên board để reset vi điều khiển tương đương
với việc chân RESET được nối với GND qua 1 điện trở 10KΩ.
Lưu ý:
 Arduino UNO không có bảo vệ cắm ngược nguồn vào. Do đó phải hết sức
cẩn thận, kiểm tra các cực âm – dương của nguồn trước khi cấp cho Arduino
UNO. Việc làm chập mạch nguồn vào của Arduino UNO sẽ biến nó thành
một miếng nhựa chặn giấy. Nên dùng nguồn từ cổng USB nếu có thể.
 Các chân 3.3V và 5V trên Arduino là các chân dùng để cấp nguồn ra cho các
thiết bị khác, không phải là các chân cấp nguồn vào. Việc cấp nguồn sai vị trí
có thể làm hỏng board. Điều này không được nhà sản xuất khuyến khích.
 Cấp nguồn ngoài không qua cổng USB cho Arduino UNO với điện áp dưới
6V có thể làm hỏng board.
 Cấp điện áp trên 13V vào chân RESET trên board có thể làm hỏng vi điều
khiển ATmega328.
 Cường độ dòng điện vào/ra ở tất cả các chân Digital và Analog của Arduino
UNO nếu vượt quá 200mA sẽ làm hỏng vi điều khiển.
 Cấp điệp áp trên 5.5V vào các chân Digital hoặc Analog của Arduino UNO
sẽ làm hỏng vi điều khiển.
 Cường độ dòng điện qua một chân Digital hoặc Analog bất kì của Arduino
UNO vượt quá 40mA sẽ làm hỏng vi điều khiển. Do đó nếu không dùng để
truyền nhận dữ liệu, nên sử dụng một điện trở hạn dòng.
Bộ nhớ
Vi điều khiển Atmega328 tiêu chuẩn cung cấp cho người dùng:
- 41 -
 32KB bộ nhớ Flash: những đoạn lệnh lập trình sẽ được lưu trữ trong bộ nhớ
Flash của vi điều khiển. Thường thì sẽ có khoảng vài KB trong số này sẽ
được dùng cho bootloader và bộ nhớ này là đủ cho các ứng dụng điều khiển
thông thường.
 2KB cho SRAM (Static Random Access Memory): giá trị các biến khai báo
khi lập trình sẽ lưu ở đây. Khai báo càng nhiều biến thì càng cần nhiều bộ
nhớ RAM. Tuy nhiên 2KB là đủ đối với các ứng dụng sử dụng trong nghiên
cứu này.
 1KB cho
EFROM (Electrically Eraseble Programmable Read Only Memory): đây
giống như một chiếc ổ cứng mini, nơi có thể đọc và ghi dữ liệu của mình vào
đây mà không phải lo bị mất khi cúp điện giống như dữ liệu trên SRAM.
Các cổng vào/ra
Hình 3-8. Các cổng vào của Arduino Uno.
Arduino UNO có 14 chân digital dùng để đọc hoặc xuất tín hiệu. Chúng chỉ
có 2 mức điện áp là 0V và 5V với dòng vào/ra tối đa trên mỗi chân là 40mA. Ở mỗi
- 42 -
chân đều có các điện trở pull-up từ được cài đặt ngay trong vi điều khiển
ATmega328 (mặc định thì các điện trở này không được kết nối).
Một số chân digital có các chức năng đặc biệt như sau:
 2 chân nối tiếp Serial: 0 (RX) và 1 (TX): dùng để gửi (transmit – TX) và
nhận (receive – RX) dữ liệu TTL Serial. Arduino Uno có thể giao tiếp với
thiết bị khác thông qua 2 chân này. Kết nối bluetooth thường thấy nói nôm na
chính là kết nối Serial không dây. Nếu không cần giao tiếp Serial, không nên
sử dụng 2 chân này nếu không cần thiết
 Chân PWM (~): 3, 5, 6, 9, 10, và 11: cho phép xuất ra xung PWM với độ
phân giải 8bit (giá trị từ 0 → 28-1 tương ứng với 0V → 5V) bằng hàm
analogWrite(). Nói một cách đơn giản, có thể điều chỉnh được điện áp ra ở
chân này từ mức 0V đến 5V thay vì chỉ cố định ở mức 0V và 5V như những
chân khác. Các chân PWM phù hợp với yêu cầu điều khiển
 Chân giao tiếp SPI: 10 (SS), 11 (MOSI), 12 (MISO), 13 (SCK). Ngoài các
chức năng thông thường, 4 chân này còn dùng để truyền phát dữ liệu bằng
giao thức SPI với các thiết bị khác.
 LED 13: trên Arduino UNO có 1 đèn led màu cam (kí hiệu chữ L). Khi bấm
nút Reset, sẽ thấy đèn này nhấp nháy để báo hiệu. Nó được nối với chân số
13. Khi chân này được người dùng sử dụng, LED sẽ sáng.
Arduino UNO có 6 chân analog (A0 → A5) cung cấp độ phân giải tín hiệu
10bit (0 → 2 10-1) để đọc giá trị điện áp trong khoảng 0V → 5V. Với
chân AREF trên board, có thể để đưa vào điện áp tham chiếu khi sử dụng các chân
analog. Tức là nếu cấp điện áp 2.5V vào chân này ta có thể dùng các chân analog để
đo điện áp trong khoảng từ 0V → 2.5V với độ phân giải vẫn là 10bit.
- 43 -
Đặc biệt, Arduino UNO có 2 chân A4 (SDA) và A5 (SCL) hỗ trợ giao tiếp
I2C/TWI với các thiết bị khác.
3.3.2. Chương trình điều khiển:
Các thiết bị dựa trên nền tảng Arduino được lập trình bằng ngôn riêng. Ngôn
ngữ này dựa trên ngôn ngữ Wiring. Và Wiring lại là một biến thể của C/C++. Một
số người gọi nó là Wiring, một số khác thì gọi là C hay C/C++. Nhưng thực tế, nhà
sản xuất định danh ngôn ngữ này là ngôn ngữ Arduino. Ngôn ngữ Arduino bắt
nguồn từ C/C++ phổ biến hiện nay do đó rất dễ học, dễ hiểu. Nếu học tốt chương
trình Tin học 11 thì việc lập trình Arduino là hoàn toàn khả thi.
Trong luận văn này, chúng ta sẽ đi vào chi tiết 3 vấn đề. Vấn đề thứ nhất là
thiết lập giao tiếp I2C giữa cảm biến và vi điều khiển cũng như xác định giá trị
tương đương của cảm biến biến trở, vấn đề thứ 2 là xử lý dữ liệu từ cảm biến
MPU6050, vấn đề thứ 3 là xây dựng ma trận cử động tương ứng giữa cử động đầu
vào và giá trị đầu ra. Sau đây, chúng ta sẽ đi vào chi tiết lần lượt từng vấn đề.
Thiết lập giao tiếp I2C với MPU6050 và xác định giá trị đầu vào của biến
trở.
Để thiết lập được giao tiếp I2C (Interface Intergrated Circuit) giữa MPU6050
và Arduino, ta thực hiện kết nối giữa chân SCL của MPU6050 với chân SCL (chân
A5) của vi điều khiển Arduino và chân SDA của MPU6050 và chân SDA (chân A4)
của vi điều khiển Arduino, đồng thời cấp nguồn cho MPU6050 ở ngưỡng 5V /
3.3V. Lúc này đã thực hiện xong việc thiết lập phần cứng cho giao thức I2C giữa
Arduino Uno R3 va MPU6050.
- 44 -
Hình 3-9. Kết nối giữa Arduino và MPU6050 theo giao thức I2C.
Lúc này MPU6050 sẽ đóng vai trò như một mạch cấp dưới ( Slave ) của
Arduino và các giá trị của MPU6050 sẽ được gọi ra theo các hàm truyền đạt và sử
sẵn có trong thư viện Mpu6050.h được viết sẵn. Khi sử dụng thư viện MPU6050.h.
Các giá trị của gia tốc các trục x, y, z cũng như giá trị gyro của các trục này được
thư viện MPU6050.h khởi tạo sẵn và thực hiện xử lý ngay trong khối xử lý chuyển
động của MPU6050.
Dưới đây là các giá trị được tạo sẵn mà ta có thể sử dụng khi khai thác thư
viện MPU6050.h
uint8_t dmpSendQuaternion(uint_fast16_t accuracy);
uint8_t dmpSendGyro(uint_fast16_t elements, uint_fast16_t accuracy);
uint8_t dmpSendAccel(uint_fast16_t elements, uint_fast16_t accuracy);
uint8_t dmpSendLinearAccel(uint_fast16_t elements, uint_fast16_t accuracy);
uint8_t dmpSendLinearAccelInWorld(uint_fast16_t elements, uint_fast16_t

accuracy);
- 45 -
uint8_t dmpSendControlData(uint_fast16_t elements, uint_fast16_t accuracy);
uint8_t dmpSendSensorData(uint_fast16_t elements, uint_fast16_t accuracy);
uint8_t dmpSendExternalSensorData(uint_fast16_t elements, uint_fast16_t
accuracy);
uint8_t dmpSendGravity(uint_fast16_t elements, uint_fast16_t accuracy);

uint8_t dmpSendPacketNumber(uint_fast16_t accuracy);
uint8_t dmpSendQuantizedAccel(uint_fast16_t elements, uint_fast16_t accuracy);
uint8_t dmpSendEIS(uint_fast16_t elements, uint_fast16_t accuracy);
hàm xác định chuyển động xoay.
void getRotation(int16_t* x, int16_t* y, int16_t* z);
int16_t getRotationX();
int16_t getRotationY();
int16_t getRotationZ();
hàm xác định gia tốc
void getMotion9(int16_t* ax, int16_t* ay, int16_t* az, int16_t* gx, int16_t* gy,
int16_t* gz, int16_t* mx, int16_t* my, int16_t* mz);
void getMotion6(int16_t* ax, int16_t* ay, int16_t* az, int16_t* gx, int16_t* gy,
int16_t* gz);
void getAcceleration(int16_t* x, int16_t* y, int16_t* z);
int16_t getAccelerationX();
int16_t getAccelerationY();
- 46 -
int16_t getAccelerationZ();
Dựa vao các kết quả trên ở phần thân chương trình, code được rút ngắn như
sau:
#include "Wire.h"
#include "I2Cdev.h"
#include "MPU6050.h"
#include "Servo.h"
MPU6050 mpu;
int16_t ax, ay, az;
int16_t gx, gy, gz;
Servo truc_xoay,truc_gap;
int xoay,gap;
void setup()
Wire.begin();
Serial.begin(9200);
Serial.println("Initialize MPU");
mpu.initialize();
Serial.println(mpu.testConnection() ? "Connected" : "Connection failed");
- 47 -
truc_xoay.attach(9);
truc_gap.attach(8);
void loop()
mpu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
gap = map (ax, -16500,16500,0, 179);
xoay = map(ay, -16500, 16500, 0, 179);
truc_xoay.write(xoay);
truc_gap.write(gap);
delay(50);
Với các giá trị có thể lấy được từ hàm getMotion6, các chuyển động được
lấy tỉ lệ theo phép toán ánh xạ map với biên giới hạn là giá trị off-set của MPU6050
là .
Thiết lập giao tiếp kết nối với bộ cảm biến biến trở.
Thực hiện kết nối biến trở là bước đầu tiên trong quá trình đo và xác định
giá trị chuyển động của ngón tay.Kết nối này được minh họa bằng hình 3-10, qua đó
ta thấy được các kết nối của một cảm biến với Arduino. Đầu vào của biến trở có thể
là một trong số các cổng vào bất ky từ A0 đến A5 của kit Arduino. Khi đó, giá trị
phân áp trên biến trở sẽ cung cấp trị đầu vào cho phép đều chế độ rộng xung PWM.
Chương trình để xử lý giá trị của biến trở như sau
- 48 -
Hình 3-10. Minh họa cho kết nối giữa biến trở và Arduino.
#include <Servo.h>
Servo ngon_tro,ngon_giua,ngon_aput,ngon_ut,ngon_cai; // create servo object to
control a servo
int ts_tro = 0;// analog pin used to connect the potentiometer
int ts_giua = 1;
int ts_aput= 2;
int ts_ut= 3;
int ts_cai=4;
int tro;// variable to read the value from the analog pin
int giua;
int aput;
int ut;
int cai;
- 49 -
void setup()
{
ngon_tro.attach(9); // attaches the servo on pin 9 to the servo object
ngon_giua.attach(10);
ngon_aput.attach(11);
ngon_ut.attach(6);
ngon_cai.attach(5);
}
void loop()
{
tro = analogRead(ts_tro); // reads the value of the potentiometer (value
between 0 and 255)
tro = map(tro, 0, 127, 0, 180); // scale it to use it with the servo (value between 0
and 180)
ngon_tro.write(tro); // sets the servo position according to the scaled
value
giua = analogRead(ts_giua); // reads the value of the potentiometer (value
between 0 and 255)
giua = map(giua, 0, 127, 0, 180); // scale it to use it with the servo (value
between 0 and 180)
ngon_giua.write(giua); // sets the servo position according to the scaled
value
aput = analogRead(ts_aput); // reads the value of the potentiometer (value
between 0 and 255)
aput = map(aput, 0, 127, 0, 180); // scale it to use it with the servo (value
between 0 and 180)
ngon_aput.write(aput); // sets the servo position according to the scaled
value
- 50 -
ut = analogRead(ts_ut); // reads the value of the potentiometer (value
between 0 and 255)
ut = map(ut, 0, 127, 0, 180); // scale it to use it with the servo (value between 0
and 180)
ngon_ut.write(ut); // sets the servo position according to the scaled
value
cai = analogRead(ts_cai); // reads the value of the potentiometer (value
between 0 and 255)
cai = map(cai, 0, 127, 0, 180); // scale it to use it with the servo (value between
0 and 180)
ngon_cai.write(cai); // sets the servo position according to the scaled
value
delay(15); // waits for the servo to get there
}
Trong chương trình này, giá trị của đầu vào là dạng 8 bit được xử lý trong
phép ánh xạ quy đổi (val,0,127,0,180). Các giá trị từ 0 đến 127 cung cấp dải quy đổi
tương đương gồm 128 giá trị với độ phận giải của về góc tương ứng của hệ thống là
1.40. 128 giá trị tương đương này tương đương với 2 8 trạng thái trong hệ nhị phân,
qua đó chuyển đổi giá trị tương đương của biến trở thành giá trị góc gần đúng.
Với hàm analogRead() và hàm servo.write(); giá trị được đọc trực tiếp từ
chân kết nối với biến trở thông qua hàm analogRead( đầu vào), với đầu vào tương
ứng với các chân A0 đến A5. Sau đó, hệ thống sẽ thực hiện việc quy đổi giá trị
tương đương giá trị điều chế D với giá trị mà hệ thống đọc được từ đầu vào. Một
xung vuông sẽ được hình thành dựa vào giá trị điều chế D theo biểu thức nêu ở mục
2.3.2, sau đó đượcc chuyển đến hàm servo.write() để thực thi điều chế PWM với cơ
chế servo.
Đó là toàn bộ những điểm cần chú ý trong chương trình xử lý dữ liệu từ cảm
biến của khối vi điều khiển. Bên cạnh những điểm đã được khắc phục và xử lý; hệ
thống điều khiển mô phỏng chuyển động cánh tay vẫn cần rất nhiều thời gian để
- 51 -
chỉnh lý và hoàn thiện nhằm phục vụ các ứng dụng thực tiễn hơn, không chỉ bó gọn
trong chức năng mô phỏng điều khiển.
- 52 -
Chương 4: Kết quả và Bàn luận.
Hệ thống mô phỏng chuyển động mô phỏng lại được các chuyển động lớn
của cánh tay và chuyển động nhỏ của ngón tay. Độ chính xác của hệ thống điều
khiển mô phỏng chuyển động chưa được tiến hành kiểm chứng trong khuôn khổ
nghiên cứu. Với hệ thống điều khiển có kiểm soát bằng điều chế biên độ rộng xung,
hệ thống hoàn toàn kiểm soát được độ chính xác của tín hiệu đầu ra khi sử dụng
điều khiển cơ cấu chấp hành servo. Các nhân tố ảnh hưởng đến độ chính xác, và tốc
độ đáp ứng của cơ cấu chấp hành là các nhân tố cơ khí như các khớp mô phỏng, độ
chính xác khi thiết kế và chế tạo cơ cấu chấp hành.
Hình 4-1. Các cử động mô phỏng qua cơ cấu chấp hành của ngón tay giữa.
Với kết quả cử động của ngón tay được thể hiện ở hình trên, việc phát triển
hệ thống cánh tay mô phỏng chuyển động của con người là bước tiếp theo của luận
văn này. Bằng việc kết hợp các nghiên cứu về cơ cấu chấp hành cũng như về vật
liệu, nghiên cứu tiếp tục mở rộng của hệ thống điều khiển sẽ gồm việc nâng cấp
nhận dạng tín hiệu sinh học của con người – nhận diện sinh trắc học -. Bằng việc
tích hợp sử dụng nguyên lý “chứa trong”, hệ thống điều khiển sẽ được chuyển đổi
thành một cánh tay robot cơ khí thực sự với khả năng chuyển động đầy đủ các bậc
tự do (22 bậc tự do).
- 53 -
Trong các vấn đề còn tồn tại của hệ thống, việc giảm tuổi thọ trong quá trình
sử dụng của biến trở thanh trượt là vấn đề khá nan giải. Với biến trở thanh trượt,
cách tiếp cận này bộc lộ rất nhiều ưu điểm. Trong đó phải kể đến sự đơn giản hóa
đến mức tối đa các chuyển đổi tín hiệu chuyển động sang tín hiệu điện. So với
những cảm biến chuyển động khác sử dụng nguyên lý vi cơ điện tử hay sử dụng vật
liệu áp điện. Do đó, xác định phương án thay thế tương đương cho biến trở thanh
trượt hoặc đề xuất cách nâng cấp và phát triển biến trở thanh trượt có tuổi thọ cao là
điều rất quan trọng đối với nghiên cứu này.
Một vấn đề nảy sinh trong quá trình nghiên cứu hệ thống điều khiển mô
phỏng chuyển động cánh tay người là đặc điểm sinh học của bàn tay người. Mỗi
người đều sở hữu các nét đặc biệt trên bàn tay như chiều dài, đường kính ngón tay,
cũng như giới hạn chuyển động của ngón tay mỗi người là khác nhau. Điều này dẫn
đến 1 vấn đề khá nghiêm trọng là việc áp dụng hệ thống này trên diện rộng là rất
khó nếu không phát triển hệ thống theo hướng sử dụng các hệ thống có luật học
kiểm soát sử dụng trí tuệ nhân tạo. Bởi các chương trình được viết sẵn không thể
nhận biết sai khác ngay từ đối tượng đầu vào. Nếu đối tượng đầu vào thay đổi, việc
thay đổi tín hiện đo ở khối cảm biến là hệ quả tất nhiên. Nó sẽ khiến hệ thống nhận
định sai về giá trị cần mô phỏng nếu ta xử lý trên miền biến dạng dọc. Do đó, giải
đáp cho vấn đề này là việc sử dụng cảm biến biến dạng dạng thanh. Khi đó toàn bộ
các biến dạng dọc sẽ được chuyển thành các biến dạng gập, lúc này vấn đề chiều dài
không đồng bộ của ngón tay sẽ được triệt tiêu và thay vào đó là việc xử lý độ biến
thiên của giá trị dựa vào độ biến thiên góc trên ngón tay.
Thêm một vấn đề khác nữa là việc nâng cấp hệ thống chấp hành cho hệ
thống mô phỏng chuyển động cánh tay. Do đặc điểm cấu tạo của do có độ đàn hồi
biến dạng nên cánh tay con người là cơ cấu rất khó thực hiện mô phỏng tái tạo với
vật liệu thông thường. Việc sử dụng các vật liệu thông thường như giấy, nhựa, cao
su đều gặp phải trở ngại. Nếu giấy tetra pad cho phép xây dựng hệ thống chấp hành
nhẹ, có độ chịu lực cao thì vấn đề xảy ra là độ đàn hồi biến dạng rất thấp. Dẫn đến
tính mô phỏng hầu như hạn chế rất nhiều khi tay thực hiện các động tác có sự biến
- 54 -
dạng bề mặt lớn như nắm tay hoặc xoay cổ tay. Nếu sử dụng nhựa PVC thì hệ thống
có ưu điểm chịu lực cao xong độ đàn hồi biến dạng là không có, việc này đưa đến
một vấn đề lớn trong thiết kế cánh tay chấp hành. Người thiết kế phải xử lý các
khớp nối vô cùng phức tạp để đạt được hiệu quả tương tự như các khớp ở tay người
nhưng độ mềm dẻo của cử động rất thấp. Quá đó ta có thể thấy, cách tiếp cận xây
dựng cơ cấp chấp hành cho hệ thống mô phỏng chuyển động là rất khó và đòi hỏi
nghiên cứu chuyên sâu và bài bản hơn. Với việc phát triển xây dựng cánh tay chấp
hành dựa trên cấu trúc giải phẫu sinh lý của cánh tay người đưa ra một ý tưởng rất
khả thi cho phương án nâng cấp của hệ thống. Với đầu vào tín hiệu là tín hiệu điện
não đồ EEG cùng với các hệ thống cảm biến áp lực và hệ thống tạo phản hồi cho hệ
thần kinh, hệ thống hoàn toàn có thể nâng cấp trở thành thiết bị thay thế cho chi cho
người khuyết tật.
Qua nghiên cứu này, chúng ta cũng thấy rõ được sự hạn chế của cảm biến đề
xuất, do thiết kế cơ cấu đo chuyển động nhỏ đề nghị cho ngón tay không được
nghiên cứu kế thừa mà được sử dụng dựa vào tính sẵn có và độ đơn giản khi tiếp
cận nên rất nhiều chỉ số của cơ cấu đo bị bỏ qua do không có khả năng đáp ứng như
độ phân giải, độ tin cậy, .v.v…. Điều này dẫn đến một sự đòi hỏi một nghiên cứu
bài bản và lâu dài để có thể hoàn thiện thiết kế này, và nâng cấp chúng lên các thiết
bị có thể gắn trên cơ thể nhằm tăng tính tiện dụng. Ngoài ra, việc mở rộng các giao
thức kết nối khác như SPI hoặc các chuẩn kết nối công nghiệp và an toàn công
nghiệp để đưa hệ thống điều khiển phục vụ sản xuất công nghiệp, trong bốc dỡ hàng
hóa tại các điểm trung chuyển hàng hóa là hướng đi hoàn toàn hứa hẹn. Trên đây là
các điểm còn tồn tại và cần nghiên cứu thêm của hệ thống điều khiển. Tuy còn hạn
chế, xong hệ thống điều khiển mô phỏng chuyển động cánh tay đã cung cấp một
cách tiếp cận đơn giản, dễ thực hiện. Nó có ý nghĩa ứng dụng cao trong việc xây
dựng mô hình học tập trong các trường có giảng dạy về cơ sinh, cơ sinh ứng dụng,
cũng như điều khiển học.
- 55 -
Tài Liệu tham khảo
[1] Arduino Reference, [Online]. Availabe:
http://www.ele.uri.edu/Courses/ele205/ELE205Lab/ELE205_Lab_files/Arduino
%20-%20Reference.pdf
[2] Arduino MPU6050 [Oniline] Availabe:
http://playground.arduino.cc/Main/MPU-6050
[3] Nhóm tác giả Dieutri.vn, Giải phẫu chi trên [Online]
Availabe:http://www.dieutri.vn/giaiphaunguoi/31-3-2015/S6816/Giai-phau-co-
chi-tren.htm
[4] Philipp Lang, Daniel Steines, Hacene Bouadi, David Miller, Barry J. Linder,
Cecily Anne Snyder (2003), Minimally invasive Joint implant with 3
dimensional geometry matching the articular surfaces [Online] Availabe:
http://www.google.com/patents/WO2004032806A1?cl=en#backward-citations
- 56 -
Phụ Lục
Thư viện hàm “MPU6050.h”
#ifndef _MPU6050_H_
#define _MPU6050_H_
#include "I2Cdev.h"
// supporting link: http://forum.arduino.cc/index.php?&topic=143444.msg1079517#msg1079517

// also: http://forum.arduino.cc/index.php?&topic=141571.msg1062899#msg1062899s
#ifndef __arm__
#include <avr/pgmspace.h>
#else
#define PROGMEM /* empty */
#define pgm_read_byte(x) (*(x))
#define pgm_read_word(x) (*(x))
#define pgm_read_float(x) (*(x))
#define PSTR(STR) STR
#endif
#define MPU6050_ADDRESS_AD0_LOW 0x68 // address pin low (GND), default for InvenSense
evaluation board
#define MPU6050_ADDRESS_AD0_HIGH 0x69 // address pin high (VCC)
#define MPU6050_DEFAULT_ADDRESS MPU6050_ADDRESS_AD0_LOW
#define MPU6050_RA_XG_OFFS_TC 0x00 //[7] PWR_MODE, [6:1] XG_OFFS_TC, [0]

OTP_BNK_VLD
#define MPU6050_RA_YG_OFFS_TC 0x01 //[7] PWR_MODE, [6:1] YG_OFFS_TC, [0]
OTP_BNK_VLD
#define MPU6050_RA_ZG_OFFS_TC 0x02 //[7] PWR_MODE, [6:1] ZG_OFFS_TC, [0]
OTP_BNK_VLD
#define MPU6050_RA_X_FINE_GAIN 0x03 //[7:0] X_FINE_GAIN
#define MPU6050_RA_Y_FINE_GAIN 0x04 //[7:0] Y_FINE_GAIN
#define MPU6050_RA_Z_FINE_GAIN 0x05 //[7:0] Z_FINE_GAIN
#define MPU6050_RA_XA_OFFS_H 0x06 //[15:0] XA_OFFS
#define MPU6050_RA_XA_OFFS_L_TC 0x07
#define MPU6050_RA_YA_OFFS_H 0x08 //[15:0] YA_OFFS
#define MPU6050_RA_YA_OFFS_L_TC 0x09
#define MPU6050_RA_ZA_OFFS_H 0x0A //[15:0] ZA_OFFS
#define MPU6050_RA_ZA_OFFS_L_TC 0x0B
#define MPU6050_RA_XG_OFFS_USRH 0x13 //[15:0] XG_OFFS_USR
#define MPU6050_RA_XG_OFFS_USRL 0x14
#define MPU6050_RA_YG_OFFS_USRH 0x15 //[15:0] YG_OFFS_USR
#define MPU6050_RA_YG_OFFS_USRL 0x16
#define MPU6050_RA_ZG_OFFS_USRH 0x17 //[15:0] ZG_OFFS_USR
#define MPU6050_RA_ZG_OFFS_USRL 0x18
#define MPU6050_RA_SMPLRT_DIV 0x19
#define MPU6050_RA_CONFIG 0x1A
#define MPU6050_RA_GYRO_CONFIG 0x1B
#define MPU6050_RA_ACCEL_CONFIG 0x1C
#define MPU6050_RA_FF_THR 0x1D
#define MPU6050_RA_FF_DUR 0x1E
#define MPU6050_RA_MOT_THR 0x1F
- 57 -
#define MPU6050_RA_MOT_DUR 0x20
#define MPU6050_RA_ZRMOT_THR 0x21
#define MPU6050_RA_ZRMOT_DUR 0x22
#define MPU6050_RA_FIFO_EN 0x23
#define MPU6050_RA_I2C_MST_CTRL 0x24
#define MPU6050_RA_I2C_SLV0_ADDR 0x25
#define MPU6050_RA_I2C_SLV0_REG 0x26
#define MPU6050_RA_I2C_SLV0_CTRL 0x27
#define MPU6050_RA_I2C_SLV1_CTRL 0x2A
#define MPU6050_RA_I2C_SLV2_ADDR 0x2B
#define MPU6050_RA_I2C_SLV2_REG 0x2C
#define MPU6050_RA_I2C_SLV2_CTRL 0x2D
#define MPU6050_RA_I2C_SLV3_ADDR 0x2E
#define MPU6050_RA_I2C_SLV3_REG 0x2F
#define MPU6050_RA_I2C_SLV4_DO 0x33
#define MPU6050_RA_I2C_SLV4_DI 0x35
#define MPU6050_RA_I2C_MST_STATUS 0x36
#define MPU6050_RA_INT_PIN_CFG 0x37
#define MPU6050_RA_INT_ENABLE 0x38
#define MPU6050_RA_DMP_INT_STATUS 0x39
#define MPU6050_RA_INT_STATUS 0x3A
#define MPU6050_RA_ACCEL_XOUT_H 0x3B
#define MPU6050_RA_ACCEL_XOUT_L 0x3C
#define MPU6050_RA_ACCEL_YOUT_H 0x3D
#define MPU6050_RA_ACCEL_YOUT_L 0x3E
#define MPU6050_RA_ACCEL_ZOUT_H 0x3F
#define MPU6050_RA_ACCEL_ZOUT_L 0x40
#define MPU6050_RA_TEMP_OUT_H 0x41
#define MPU6050_RA_TEMP_OUT_L 0x42
#define MPU6050_RA_GYRO_XOUT_H 0x43
#define MPU6050_RA_GYRO_XOUT_L 0x44
#define MPU6050_RA_GYRO_YOUT_H 0x45
#define MPU6050_RA_GYRO_YOUT_L 0x46
#define MPU6050_RA_GYRO_ZOUT_H 0x47
#define MPU6050_RA_GYRO_ZOUT_L 0x48
#define MPU6050_RA_EXT_SENS_DATA_00 0x49
#define MPU6050_RA_EXT_SENS_DATA_01 0x4A
#define MPU6050_RA_EXT_SENS_DATA_02 0x4B
#define MPU6050_RA_EXT_SENS_DATA_03 0x4C
#define MPU6050_RA_EXT_SENS_DATA_04 0x4D
#define MPU6050_RA_EXT_SENS_DATA_05 0x4E
#define MPU6050_RA_EXT_SENS_DATA_06 0x4F
- 58 -
#define MPU6050_RA_EXT_SENS_DATA_17 0x5A
#define MPU6050_RA_EXT_SENS_DATA_18 0x5B
#define MPU6050_RA_EXT_SENS_DATA_19 0x5C
#define MPU6050_RA_EXT_SENS_DATA_20 0x5D
#define MPU6050_RA_EXT_SENS_DATA_21 0x5E
#define MPU6050_RA_EXT_SENS_DATA_22 0x5F
#define MPU6050_RA_MOT_DETECT_STATUS 0x61
#define MPU6050_RA_I2C_MST_DELAY_CTRL 0x67
#define MPU6050_RA_SIGNAL_PATH_RESET 0x68
#define MPU6050_RA_MOT_DETECT_CTRL 0x69
#define MPU6050_RA_USER_CTRL 0x6A
#define MPU6050_RA_PWR_MGMT_1 0x6B
#define MPU6050_RA_PWR_MGMT_2 0x6C
#define MPU6050_RA_BANK_SEL 0x6D
#define MPU6050_RA_MEM_START_ADDR 0x6E
#define MPU6050_RA_MEM_R_W 0x6F
#define MPU6050_RA_DMP_CFG_1 0x70
#define MPU6050_RA_DMP_CFG_2 0x71
#define MPU6050_RA_FIFO_COUNTH 0x72
#define MPU6050_RA_FIFO_COUNTL 0x73
#define MPU6050_RA_FIFO_R_W 0x74
#define MPU6050_RA_WHO_AM_I 0x75
#define MPU6050_TC_PWR_MODE_BIT 7
#define MPU6050_TC_OFFSET_BIT 6
#define MPU6050_TC_OFFSET_LENGTH 6
#define MPU6050_TC_OTP_BNK_VLD_BIT 0
#define MPU6050_VDDIO_LEVEL_VLOGIC 0
#define MPU6050_VDDIO_LEVEL_VDD 1
#define MPU6050_CFG_EXT_SYNC_SET_BIT 5
#define MPU6050_CFG_EXT_SYNC_SET_LENGTH 3
#define MPU6050_CFG_DLPF_CFG_BIT 2
#define MPU6050_CFG_DLPF_CFG_LENGTH 3
#define MPU6050_EXT_SYNC_DISABLED 0x0

#define MPU6050_EXT_SYNC_TEMP_OUT_L 0x1
#define MPU6050_EXT_SYNC_GYRO_XOUT_L 0x2
#define MPU6050_EXT_SYNC_GYRO_YOUT_L 0x3
#define MPU6050_EXT_SYNC_GYRO_ZOUT_L 0x4
#define MPU6050_EXT_SYNC_ACCEL_XOUT_L 0x5
#define MPU6050_EXT_SYNC_ACCEL_YOUT_L 0x6
#define MPU6050_EXT_SYNC_ACCEL_ZOUT_L 0x7
#define MPU6050_DLPF_BW_256 0x00

- 59 -
#define MPU6050_GCONFIG_FS_SEL_BIT 4
#define MPU6050_GCONFIG_FS_SEL_LENGTH 2
#define MPU6050_GYRO_FS_250 0x00

#define MPU6050_ACONFIG_XA_ST_BIT 7
#define MPU6050_ACONFIG_YA_ST_BIT 6
#define MPU6050_ACONFIG_ZA_ST_BIT 5
#define MPU6050_ACONFIG_AFS_SEL_BIT 4
#define MPU6050_ACONFIG_AFS_SEL_LENGTH 2
#define MPU6050_ACONFIG_ACCEL_HPF_BIT 2
#define MPU6050_ACONFIG_ACCEL_HPF_LENGTH 3
#define MPU6050_ACCEL_FS_2 0x00

#define MPU6050_DHPF_RESET 0x00

#define MPU6050_DHPF_5 0x01
#define MPU6050_DHPF_2P5 0x02
#define MPU6050_DHPF_HOLD 0x07
#define MPU6050_TEMP_FIFO_EN_BIT 7
#define MPU6050_XG_FIFO_EN_BIT 6
#define MPU6050_YG_FIFO_EN_BIT 5
#define MPU6050_ZG_FIFO_EN_BIT 4
#define MPU6050_ACCEL_FIFO_EN_BIT 3
#define MPU6050_SLV2_FIFO_EN_BIT 2
#define MPU6050_MULT_MST_EN_BIT 7
#define MPU6050_WAIT_FOR_ES_BIT 6
#define MPU6050_SLV_3_FIFO_EN_BIT 5
#define MPU6050_I2C_MST_P_NSR_BIT 4
#define MPU6050_I2C_MST_CLK_BIT 3
#define MPU6050_I2C_MST_CLK_LENGTH 4
#define MPU6050_CLOCK_DIV_348 0x0

#define MPU6050_CLOCK_DIV_471 0xA
- 60 -
#define MPU6050_CLOCK_DIV_444 0xB
#define MPU6050_CLOCK_DIV_421 0xC
#define MPU6050_CLOCK_DIV_400 0xD
#define MPU6050_CLOCK_DIV_381 0xE
#define MPU6050_CLOCK_DIV_364 0xF
#define MPU6050_I2C_SLV_RW_BIT 7
#define MPU6050_I2C_SLV_ADDR_BIT 6
#define MPU6050_I2C_SLV_ADDR_LENGTH 7
#define MPU6050_I2C_SLV_EN_BIT 7
#define MPU6050_I2C_SLV_BYTE_SW_BIT 6
#define MPU6050_I2C_SLV_REG_DIS_BIT 5
#define MPU6050_I2C_SLV_GRP_BIT 4
#define MPU6050_I2C_SLV_LEN_BIT 3
#define MPU6050_I2C_SLV_LEN_LENGTH 4
#define MPU6050_I2C_SLV4_RW_BIT 7
#define MPU6050_I2C_SLV4_ADDR_BIT 6
#define MPU6050_I2C_SLV4_ADDR_LENGTH 7
#define MPU6050_I2C_SLV4_EN_BIT 7
#define MPU6050_I2C_SLV4_INT_EN_BIT 6
#define MPU6050_I2C_SLV4_REG_DIS_BIT 5
#define MPU6050_I2C_SLV4_MST_DLY_BIT 4
#define MPU6050_I2C_SLV4_MST_DLY_LENGTH 5
#define MPU6050_MST_PASS_THROUGH_BIT 7
#define MPU6050_MST_I2C_SLV4_DONE_BIT 6
#define MPU6050_MST_I2C_LOST_ARB_BIT 5
#define MPU6050_MST_I2C_SLV4_NACK_BIT 4
#define MPU6050_INTCFG_INT_LEVEL_BIT 7
#define MPU6050_INTCFG_INT_OPEN_BIT 6
#define MPU6050_INTCFG_LATCH_INT_EN_BIT 5
#define MPU6050_INTCFG_INT_RD_CLEAR_BIT 4
#define MPU6050_INTCFG_FSYNC_INT_LEVEL_BIT 3
#define MPU6050_INTCFG_FSYNC_INT_EN_BIT 2
#define MPU6050_INTCFG_I2C_BYPASS_EN_BIT 1
#define MPU6050_INTCFG_CLKOUT_EN_BIT 0
#define MPU6050_INTMODE_ACTIVEHIGH 0x00

#define MPU6050_INTMODE_ACTIVELOW 0x01
#define MPU6050_INTDRV_PUSHPULL 0x00

#define MPU6050_INTDRV_OPENDRAIN 0x01
#define MPU6050_INTLATCH_50USPULSE 0x00

#define MPU6050_INTLATCH_WAITCLEAR 0x01
#define MPU6050_INTCLEAR_STATUSREAD 0x00

#define MPU6050_INTCLEAR_ANYREAD 0x01
#define MPU6050_INTERRUPT_FF_BIT 7
#define MPU6050_INTERRUPT_MOT_BIT 6
- 61 -
#define MPU6050_INTERRUPT_ZMOT_BIT 5
#define MPU6050_INTERRUPT_FIFO_OFLOW_BIT 4
#define MPU6050_INTERRUPT_I2C_MST_INT_BIT 3
#define MPU6050_INTERRUPT_PLL_RDY_INT_BIT 2
#define MPU6050_INTERRUPT_DMP_INT_BIT 1
#define MPU6050_INTERRUPT_DATA_RDY_BIT 0
// TODO: figure out what these actually do

// UMPL source code is not very obivous
#define MPU6050_DMPINT_5_BIT 5
#define MPU6050_MOTION_MOT_XNEG_BIT 7
#define MPU6050_MOTION_MOT_XPOS_BIT 6
#define MPU6050_MOTION_MOT_YNEG_BIT 5
#define MPU6050_MOTION_MOT_YPOS_BIT 4
#define MPU6050_MOTION_MOT_ZNEG_BIT 3
#define MPU6050_MOTION_MOT_ZPOS_BIT 2
#define MPU6050_MOTION_MOT_ZRMOT_BIT 0
#define MPU6050_DELAYCTRL_DELAY_ES_SHADOW_BIT 7
#define MPU6050_DELAYCTRL_I2C_SLV4_DLY_EN_BIT 4
#define MPU6050_PATHRESET_GYRO_RESET_BIT 2
#define MPU6050_PATHRESET_ACCEL_RESET_BIT 1
#define MPU6050_PATHRESET_TEMP_RESET_BIT 0
#define MPU6050_DETECT_ACCEL_ON_DELAY_BIT 5
#define MPU6050_DETECT_ACCEL_ON_DELAY_LENGTH 2
#define MPU6050_DETECT_FF_COUNT_BIT 3
#define MPU6050_DETECT_FF_COUNT_LENGTH 2
#define MPU6050_DETECT_MOT_COUNT_BIT 1
#define MPU6050_DETECT_MOT_COUNT_LENGTH 2
#define MPU6050_DETECT_DECREMENT_RESET 0x0

#define MPU6050_DETECT_DECREMENT_1 0x1
#define MPU6050_USERCTRL_DMP_EN_BIT 7
#define MPU6050_USERCTRL_FIFO_EN_BIT 6
#define MPU6050_USERCTRL_I2C_MST_EN_BIT 5
#define MPU6050_USERCTRL_I2C_IF_DIS_BIT 4
#define MPU6050_USERCTRL_DMP_RESET_BIT 3
#define MPU6050_USERCTRL_FIFO_RESET_BIT 2
#define MPU6050_USERCTRL_I2C_MST_RESET_BIT 1
#define MPU6050_USERCTRL_SIG_COND_RESET_BIT 0
#define MPU6050_PWR1_DEVICE_RESET_BIT 7
- 62 -
#define MPU6050_PWR1_SLEEP_BIT 6
#define MPU6050_PWR1_CYCLE_BIT 5
#define MPU6050_PWR1_TEMP_DIS_BIT 3
#define MPU6050_PWR1_CLKSEL_BIT 2
#define MPU6050_PWR1_CLKSEL_LENGTH 3
#define MPU6050_CLOCK_INTERNAL 0x00

#define MPU6050_CLOCK_PLL_XGYRO 0x01
#define MPU6050_CLOCK_PLL_YGYRO 0x02
#define MPU6050_CLOCK_PLL_ZGYRO 0x03
#define MPU6050_CLOCK_PLL_EXT32K 0x04
#define MPU6050_CLOCK_PLL_EXT19M 0x05
#define MPU6050_CLOCK_KEEP_RESET 0x07
#define MPU6050_PWR2_LP_WAKE_CTRL_BIT 7
#define MPU6050_PWR2_LP_WAKE_CTRL_LENGTH 2
#define MPU6050_PWR2_STBY_XA_BIT 5
#define MPU6050_PWR2_STBY_YA_BIT 4
#define MPU6050_PWR2_STBY_ZA_BIT 3
#define MPU6050_PWR2_STBY_XG_BIT 2
#define MPU6050_PWR2_STBY_YG_BIT 1
#define MPU6050_PWR2_STBY_ZG_BIT 0
#define MPU6050_WAKE_FREQ_1P25 0x0

#define MPU6050_WAKE_FREQ_2P5 0x1
#define MPU6050_WAKE_FREQ_5 0x2
#define MPU6050_WAKE_FREQ_10 0x3
#define MPU6050_BANKSEL_PRFTCH_EN_BIT 6
#define MPU6050_BANKSEL_CFG_USER_BANK_BIT 5
#define MPU6050_BANKSEL_MEM_SEL_BIT 4
#define MPU6050_BANKSEL_MEM_SEL_LENGTH 5
#define MPU6050_WHO_AM_I_BIT 6
#define MPU6050_WHO_AM_I_LENGTH 6
#define MPU6050_DMP_MEMORY_BANKS 8
#define MPU6050_DMP_MEMORY_BANK_SIZE 256
#define MPU6050_DMP_MEMORY_CHUNK_SIZE 16
// note: DMP code memory blocks defined at end of header file
class MPU6050 {
public:
MPU6050();
MPU6050(uint8_t address);
void initialize();
bool testConnection();
// AUX_VDDIO register
uint8_t getAuxVDDIOLevel();
void setAuxVDDIOLevel(uint8_t level);
// SMPLRT_DIV register
uint8_t getRate();
void setRate(uint8_t rate);
- 63 -
// CONFIG register
uint8_t getExternalFrameSync();
void setExternalFrameSync(uint8_t sync);
uint8_t getDLPFMode();
void setDLPFMode(uint8_t bandwidth);
// GYRO_CONFIG register
uint8_t getFullScaleGyroRange();
void setFullScaleGyroRange(uint8_t range);
// ACCEL_CONFIG register
bool getAccelXSelfTest();
void setAccelXSelfTest(bool enabled);
bool getAccelYSelfTest();
void setAccelYSelfTest(bool enabled);
bool getAccelZSelfTest();
void setAccelZSelfTest(bool enabled);
uint8_t getFullScaleAccelRange();
void setFullScaleAccelRange(uint8_t range);
uint8_t getDHPFMode();
void setDHPFMode(uint8_t mode);
// FF_THR register
uint8_t getFreefallDetectionThreshold();
void setFreefallDetectionThreshold(uint8_t threshold);
// FF_DUR register
uint8_t getFreefallDetectionDuration();
void setFreefallDetectionDuration(uint8_t duration);
// MOT_THR register
uint8_t getMotionDetectionThreshold();
void setMotionDetectionThreshold(uint8_t threshold);
// MOT_DUR register
uint8_t getMotionDetectionDuration();
void setMotionDetectionDuration(uint8_t duration);
// ZRMOT_THR register
uint8_t getZeroMotionDetectionThreshold();
void setZeroMotionDetectionThreshold(uint8_t threshold);
// ZRMOT_DUR register
uint8_t getZeroMotionDetectionDuration();
void setZeroMotionDetectionDuration(uint8_t duration);
// FIFO_EN register
bool getTempFIFOEnabled();
void setTempFIFOEnabled(bool enabled);
bool getXGyroFIFOEnabled();
void setXGyroFIFOEnabled(bool enabled);
bool getYGyroFIFOEnabled();
void setYGyroFIFOEnabled(bool enabled);
bool getZGyroFIFOEnabled();
void setZGyroFIFOEnabled(bool enabled);
bool getAccelFIFOEnabled();
- 64 -
void setAccelFIFOEnabled(bool enabled);
bool getSlave2FIFOEnabled();
void setSlave2FIFOEnabled(bool enabled);
// I2C_MST_CTRL register
bool getMultiMasterEnabled();
void setMultiMasterEnabled(bool enabled);
bool getWaitForExternalSensorEnabled();
void setWaitForExternalSensorEnabled(bool enabled);
bool getSlaveReadWriteTransitionEnabled();
void setSlaveReadWriteTransitionEnabled(bool enabled);
uint8_t getMasterClockSpeed();
void setMasterClockSpeed(uint8_t speed);
// I2C_SLV* registers (Slave 0-3)

uint8_t getSlaveAddress(uint8_t num);
void setSlaveAddress(uint8_t num, uint8_t address);
uint8_t getSlaveRegister(uint8_t num);
void setSlaveRegister(uint8_t num, uint8_t reg);
bool getSlaveEnabled(uint8_t num);
void setSlaveEnabled(uint8_t num, bool enabled);
bool getSlaveWordByteSwap(uint8_t num);
void setSlaveWordByteSwap(uint8_t num, bool enabled);
bool getSlaveWriteMode(uint8_t num);
void setSlaveWriteMode(uint8_t num, bool mode);
bool getSlaveWordGroupOffset(uint8_t num);
void setSlaveWordGroupOffset(uint8_t num, bool enabled);
uint8_t getSlaveDataLength(uint8_t num);
void setSlaveDataLength(uint8_t num, uint8_t length);
// I2C_SLV* registers (Slave 4)

uint8_t getSlave4Address();
void setSlave4Address(uint8_t address);
uint8_t getSlave4Register();
void setSlave4Register(uint8_t reg);
void setSlave4OutputByte(uint8_t data);
bool getSlave4Enabled();
void setSlave4Enabled(bool enabled);
bool getSlave4InterruptEnabled();
void setSlave4InterruptEnabled(bool enabled);
bool getSlave4WriteMode();
void setSlave4WriteMode(bool mode);
uint8_t getSlave4MasterDelay();
void setSlave4MasterDelay(uint8_t delay);
uint8_t getSlate4InputByte();
// I2C_MST_STATUS register
bool getPassthroughStatus();
bool getSlave4IsDone();
bool getLostArbitration();
bool getSlave4Nack();
- 65 -
// INT_PIN_CFG register
bool getInterruptMode();
void setInterruptMode(bool mode);
bool getInterruptDrive();
void setInterruptDrive(bool drive);
bool getInterruptLatch();
void setInterruptLatch(bool latch);
bool getInterruptLatchClear();
void setInterruptLatchClear(bool clear);
bool getFSyncInterruptLevel();
void setFSyncInterruptLevel(bool level);
bool getFSyncInterruptEnabled();
void setFSyncInterruptEnabled(bool enabled);
bool getI2CBypassEnabled();
void setI2CBypassEnabled(bool enabled);
bool getClockOutputEnabled();
void setClockOutputEnabled(bool enabled);
// INT_ENABLE register
uint8_t getIntEnabled();
void setIntEnabled(uint8_t enabled);
bool getIntFreefallEnabled();
void setIntFreefallEnabled(bool enabled);
bool getIntMotionEnabled();
void setIntMotionEnabled(bool enabled);
bool getIntZeroMotionEnabled();
void setIntZeroMotionEnabled(bool enabled);
bool getIntFIFOBufferOverflowEnabled();
void setIntFIFOBufferOverflowEnabled(bool enabled);
bool getIntI2CMasterEnabled();
void setIntI2CMasterEnabled(bool enabled);
bool getIntDataReadyEnabled();
void setIntDataReadyEnabled(bool enabled);
// INT_STATUS register
uint8_t getIntStatus();
bool getIntFreefallStatus();
bool getIntMotionStatus();
bool getIntZeroMotionStatus();
bool getIntFIFOBufferOverflowStatus();
bool getIntI2CMasterStatus();
bool getIntDataReadyStatus();
// ACCEL_*OUT_* registers
void getMotion9(int16_t* ax, int16_t* ay, int16_t* az, int16_t* gx, int16_t* gy, int16_t* gz, int16_t*
mx, int16_t* my, int16_t* mz);
void getMotion6(int16_t* ax, int16_t* ay, int16_t* az, int16_t* gx, int16_t* gy, int16_t* gz);
void getAcceleration(int16_t* x, int16_t* y, int16_t* z);
int16_t getAccelerationX();
int16_t getAccelerationY();
int16_t getAccelerationZ();
- 66 -
// TEMP_OUT_* registers
int16_t getTemperature();
// GYRO_*OUT_* registers
void getRotation(int16_t* x, int16_t* y, int16_t* z);
int16_t getRotationX();
int16_t getRotationY();
int16_t getRotationZ();
// EXT_SENS_DATA_* registers
uint8_t getExternalSensorByte(int position);
uint16_t getExternalSensorWord(int position);
uint32_t getExternalSensorDWord(int position);
// MOT_DETECT_STATUS register
bool getXNegMotionDetected();
bool getXPosMotionDetected();
bool getYNegMotionDetected();
bool getYPosMotionDetected();
bool getZNegMotionDetected();
bool getZPosMotionDetected();
bool getZeroMotionDetected();
// I2C_SLV*_DO register
void setSlaveOutputByte(uint8_t num, uint8_t data);
// I2C_MST_DELAY_CTRL register
bool getExternalShadowDelayEnabled();
void setExternalShadowDelayEnabled(bool enabled);
bool getSlaveDelayEnabled(uint8_t num);
void setSlaveDelayEnabled(uint8_t num, bool enabled);
// SIGNAL_PATH_RESET register
void resetGyroscopePath();
void resetAccelerometerPath();
void resetTemperaturePath();
// MOT_DETECT_CTRL register
uint8_t getAccelerometerPowerOnDelay();
void setAccelerometerPowerOnDelay(uint8_t delay);
uint8_t getFreefallDetectionCounterDecrement();
void setFreefallDetectionCounterDecrement(uint8_t decrement);
uint8_t getMotionDetectionCounterDecrement();
void setMotionDetectionCounterDecrement(uint8_t decrement);
// USER_CTRL register
bool getFIFOEnabled();
void setFIFOEnabled(bool enabled);
bool getI2CMasterModeEnabled();
void setI2CMasterModeEnabled(bool enabled);
void switchSPIEnabled(bool enabled);
void resetFIFO();
void resetI2CMaster();
void resetSensors();
// PWR_MGMT_1 register
void reset();
- 67 -
bool getSleepEnabled();
void setSleepEnabled(bool enabled);
bool getWakeCycleEnabled();
void setWakeCycleEnabled(bool enabled);
bool getTempSensorEnabled();
void setTempSensorEnabled(bool enabled);
uint8_t getClockSource();
void setClockSource(uint8_t source);
// PWR_MGMT_2 register
uint8_t getWakeFrequency();
void setWakeFrequency(uint8_t frequency);
bool getStandbyXAccelEnabled();
void setStandbyXAccelEnabled(bool enabled);
bool getStandbyYAccelEnabled();
void setStandbyYAccelEnabled(bool enabled);
bool getStandbyZAccelEnabled();
void setStandbyZAccelEnabled(bool enabled);
bool getStandbyXGyroEnabled();
void setStandbyXGyroEnabled(bool enabled);
bool getStandbyYGyroEnabled();
void setStandbyYGyroEnabled(bool enabled);
bool getStandbyZGyroEnabled();
void setStandbyZGyroEnabled(bool enabled);
// FIFO_COUNT_* registers
uint16_t getFIFOCount();
// FIFO_R_W register
uint8_t getFIFOByte();
void setFIFOByte(uint8_t data);
void getFIFOBytes(uint8_t *data, uint8_t length);
// WHO_AM_I register
uint8_t getDeviceID();
void setDeviceID(uint8_t id);
// ======== UNDOCUMENTED/DMP REGISTERS/METHODS ========
// XG_OFFS_TC register
uint8_t getOTPBankValid();
void setOTPBankValid(bool enabled);
int8_t getXGyroOffsetTC();
void setXGyroOffsetTC(int8_t offset);
// YG_OFFS_TC register
int8_t getYGyroOffsetTC();
void setYGyroOffsetTC(int8_t offset);
// ZG_OFFS_TC register
int8_t getZGyroOffsetTC();
void setZGyroOffsetTC(int8_t offset);
// X_FINE_GAIN register
int8_t getXFineGain();
void setXFineGain(int8_t gain);
- 68 -
// Y_FINE_GAIN register
int8_t getYFineGain();
void setYFineGain(int8_t gain);
// Z_FINE_GAIN register
int8_t getZFineGain();
void setZFineGain(int8_t gain);
// XA_OFFS_* registers
int16_t getXAccelOffset();
void setXAccelOffset(int16_t offset);
// YA_OFFS_* register
int16_t getYAccelOffset();
void setYAccelOffset(int16_t offset);
// ZA_OFFS_* register
int16_t getZAccelOffset();
void setZAccelOffset(int16_t offset);
// XG_OFFS_USR* registers
int16_t getXGyroOffset();
void setXGyroOffset(int16_t offset);
// YG_OFFS_USR* register
int16_t getYGyroOffset();
void setYGyroOffset(int16_t offset);
// ZG_OFFS_USR* register
int16_t getZGyroOffset();
void setZGyroOffset(int16_t offset);
// INT_ENABLE register (DMP functions)

bool getIntPLLReadyEnabled();
void setIntPLLReadyEnabled(bool enabled);
bool getIntDMPEnabled();
void setIntDMPEnabled(bool enabled);
// DMP_INT_STATUS
bool getDMPInt5Status();
// INT_STATUS register (DMP functions)

bool getIntPLLReadyStatus();
bool getIntDMPStatus();
// USER_CTRL register (DMP functions)

bool getDMPEnabled();
void setDMPEnabled(bool enabled);
void resetDMP();
// BANK_SEL register
void setMemoryBank(uint8_t bank, bool prefetchEnabled=false, bool userBank=false);
- 69 -
// MEM_START_ADDR register
void setMemoryStartAddress(uint8_t address);
// MEM_R_W register
uint8_t readMemoryByte();
void writeMemoryByte(uint8_t data);
void readMemoryBlock(uint8_t *data, uint16_t dataSize, uint8_t bank=0, uint8_t address=0);
bool writeMemoryBlock(const uint8_t *data, uint16_t dataSize, uint8_t bank=0, uint8_t address=0, bool
verify=true, bool useProgMem=false);
bool writeProgMemoryBlock(const uint8_t *data, uint16_t dataSize, uint8_t bank=0, uint8_t address=0,
bool verify=true);
bool writeDMPConfigurationSet(const uint8_t *data, uint16_t dataSize, bool useProgMem=false);

bool writeProgDMPConfigurationSet(const uint8_t *data, uint16_t dataSize);
// DMP_CFG_1 register
uint8_t getDMPConfig1();
void setDMPConfig1(uint8_t config);
// DMP_CFG_2 register
uint8_t getDMPConfig2();
void setDMPConfig2(uint8_t config);
// special methods for MotionApps 2.0 implementation

#ifdef MPU6050_INCLUDE_DMP_MOTIONAPPS20
uint8_t *dmpPacketBuffer;
uint16_t dmpPacketSize;
uint8_t dmpInitialize();
bool dmpPacketAvailable();
uint8_t dmpSetFIFORate(uint8_t fifoRate);

uint8_t dmpGetFIFORate();
uint8_t dmpGetSampleStepSizeMS();
uint8_t dmpGetSampleFrequency();
int32_t dmpDecodeTemperature(int8_t tempReg);
// Register callbacks after a packet of FIFO data is processed

//uint8_t dmpRegisterFIFORateProcess(inv_obj_func func, int16_t priority);
//uint8_t dmpUnregisterFIFORateProcess(inv_obj_func func);
uint8_t dmpRunFIFORateProcesses();
// Setup FIFO for various output

uint8_t dmpSendLinearAccelInWorld(uint_fast16_t elements, uint_fast16_t accuracy);
uint8_t dmpSendExternalSensorData(uint_fast16_t elements, uint_fast16_t accuracy);
- 70 -
// Get Fixed Point data from FIFO
uint8_t dmpGetAccel(int32_t *data, const uint8_t* packet=0);
uint8_t dmpGetAccel(VectorInt16 *v, const uint8_t* packet=0);
uint8_t dmpGetQuaternion(int32_t *data, const uint8_t* packet=0);
uint8_t dmpGetQuaternion(Quaternion *q, const uint8_t* packet=0);
uint8_t dmpGet6AxisQuaternion(int32_t *data, const uint8_t* packet=0);
uint8_t dmpGet6AxisQuaternion(Quaternion *q, const uint8_t* packet=0);
uint8_t dmpGetRelativeQuaternion(int32_t *data, const uint8_t* packet=0);
uint8_t dmpGetRelativeQuaternion(Quaternion *data, const uint8_t* packet=0);
uint8_t dmpGetGyro(int32_t *data, const uint8_t* packet=0);
uint8_t dmpGetGyro(VectorInt16 *v, const uint8_t* packet=0);
uint8_t dmpSetLinearAccelFilterCoefficient(float coef);
uint8_t dmpGetLinearAccel(int32_t *data, const uint8_t* packet=0);
uint8_t dmpGetLinearAccel(VectorInt16 *v, const uint8_t* packet=0);
uint8_t dmpGetLinearAccel(VectorInt16 *v, VectorInt16 *vRaw, VectorFloat *gravity);
uint8_t dmpGetLinearAccelInWorld(int32_t *data, const uint8_t* packet=0);
uint8_t dmpGetLinearAccelInWorld(VectorInt16 *v, const uint8_t* packet=0);
uint8_t dmpGetLinearAccelInWorld(VectorInt16 *v, VectorInt16 *vReal, Quaternion *q);
uint8_t dmpGetGyroAndAccelSensor(int32_t *data, const uint8_t* packet=0);
uint8_t dmpGetGyroAndAccelSensor(VectorInt16 *g, VectorInt16 *a, const uint8_t* packet=0);
uint8_t dmpGetGyroSensor(int32_t *data, const uint8_t* packet=0);
uint8_t dmpGetGyroSensor(VectorInt16 *v, const uint8_t* packet=0);
uint8_t dmpGetControlData(int32_t *data, const uint8_t* packet=0);
uint8_t dmpGetTemperature(int32_t *data, const uint8_t* packet=0);
uint8_t dmpGetGravity(int32_t *data, const uint8_t* packet=0);
uint8_t dmpGetGravity(VectorInt16 *v, const uint8_t* packet=0);
uint8_t dmpGetGravity(VectorFloat *v, Quaternion *q);
uint8_t dmpGetUnquantizedAccel(int32_t *data, const uint8_t* packet=0);
uint8_t dmpGetUnquantizedAccel(VectorInt16 *v, const uint8_t* packet=0);
uint8_t dmpGetQuantizedAccel(int32_t *data, const uint8_t* packet=0);
uint8_t dmpGetQuantizedAccel(VectorInt16 *v, const uint8_t* packet=0);
uint8_t dmpGetExternalSensorData(int32_t *data, uint16_t size, const uint8_t* packet=0);
uint8_t dmpGetEIS(int32_t *data, const uint8_t* packet=0);
uint8_t dmpGetEuler(float *data, Quaternion *q);

uint8_t dmpGetYawPitchRoll(float *data, Quaternion *q, VectorFloat *gravity);
// Get Floating Point data from FIFO

uint8_t dmpGetAccelFloat(float *data, const uint8_t* packet=0);
uint8_t dmpGetQuaternionFloat(float *data, const uint8_t* packet=0);
uint8_t dmpProcessFIFOPacket(const unsigned char *dmpData);

uint8_t dmpReadAndProcessFIFOPacket(uint8_t numPackets, uint8_t *processed=NULL);
uint8_t dmpSetFIFOProcessedCallback(void (*func) (void));
- 71 -
uint8_t dmpInitFIFOParam();
uint8_t dmpCloseFIFO();
uint8_t dmpSetGyroDataSource(uint8_t source);
uint8_t dmpDecodeQuantizedAccel();
uint32_t dmpGetGyroSumOfSquare();
uint32_t dmpGetAccelSumOfSquare();
void dmpOverrideQuaternion(long *q);
uint16_t dmpGetFIFOPacketSize();
#endif
// special methods for MotionApps 4.1 implementation

#ifdef MPU6050_INCLUDE_DMP_MOTIONAPPS41
uint8_t *dmpPacketBuffer;
uint16_t dmpPacketSize;
uint8_t dmpInitialize();
bool dmpPacketAvailable();
uint8_t dmpSetFIFORate(uint8_t fifoRate);

uint8_t dmpGetFIFORate();
uint8_t dmpGetSampleStepSizeMS();
uint8_t dmpGetSampleFrequency();
int32_t dmpDecodeTemperature(int8_t tempReg);
// Register callbacks after a packet of FIFO data is processed

//uint8_t dmpRegisterFIFORateProcess(inv_obj_func func, int16_t priority);
//uint8_t dmpUnregisterFIFORateProcess(inv_obj_func func);
uint8_t dmpRunFIFORateProcesses();
// Setup FIFO for various output

uint8_t dmpSendLinearAccelInWorld(uint_fast16_t elements, uint_fast16_t accuracy);
uint8_t dmpSendExternalSensorData(uint_fast16_t elements, uint_fast16_t accuracy);
// Get Fixed Point data from FIFO

uint8_t dmpGetAccel(VectorInt16 *v, const uint8_t* packet=0);
uint8_t dmpGetQuaternion(Quaternion *q, const uint8_t* packet=0);
uint8_t dmpGet6AxisQuaternion(Quaternion *q, const uint8_t* packet=0);
uint8_t dmpGetRelativeQuaternion(Quaternion *data, const uint8_t* packet=0);
- 72 -
uint8_t dmpGetGyro(VectorInt16 *v, const uint8_t* packet=0);
uint8_t dmpGetMag(int16_t *data, const uint8_t* packet=0);
uint8_t dmpSetLinearAccelFilterCoefficient(float coef);
uint8_t dmpGetLinearAccel(VectorInt16 *v, const uint8_t* packet=0);
uint8_t dmpGetLinearAccel(VectorInt16 *v, VectorInt16 *vRaw, VectorFloat *gravity);
uint8_t dmpGetLinearAccelInWorld(VectorInt16 *v, const uint8_t* packet=0);
uint8_t dmpGetLinearAccelInWorld(VectorInt16 *v, VectorInt16 *vReal, Quaternion *q);
uint8_t dmpGetGyroAndAccelSensor(VectorInt16 *g, VectorInt16 *a, const uint8_t* packet=0);
uint8_t dmpGetGyroSensor(VectorInt16 *v, const uint8_t* packet=0);
uint8_t dmpGetControlData(int32_t *data, const uint8_t* packet=0);
uint8_t dmpGetTemperature(int32_t *data, const uint8_t* packet=0);
uint8_t dmpGetGravity(VectorInt16 *v, const uint8_t* packet=0);
uint8_t dmpGetGravity(VectorFloat *v, Quaternion *q);
uint8_t dmpGetUnquantizedAccel(VectorInt16 *v, const uint8_t* packet=0);
uint8_t dmpGetQuantizedAccel(VectorInt16 *v, const uint8_t* packet=0);
uint8_t dmpGetExternalSensorData(int32_t *data, uint16_t size, const uint8_t* packet=0);
uint8_t dmpGetEIS(int32_t *data, const uint8_t* packet=0);
uint8_t dmpGetEuler(float *data, Quaternion *q);
uint8_t dmpGetYawPitchRoll(float *data, Quaternion *q, VectorFloat *gravity);
// Get Floating Point data from FIFO

uint8_t dmpGetAccelFloat(float *data, const uint8_t* packet=0);
uint8_t dmpGetQuaternionFloat(float *data, const uint8_t* packet=0);
uint8_t dmpProcessFIFOPacket(const unsigned char *dmpData);

uint8_t dmpReadAndProcessFIFOPacket(uint8_t numPackets, uint8_t *processed=NULL);
uint8_t dmpSetFIFOProcessedCallback(void (*func) (void));
uint8_t dmpInitFIFOParam();
uint8_t dmpCloseFIFO();
uint8_t dmpSetGyroDataSource(uint8_t source);
uint8_t dmpDecodeQuantizedAccel();
uint32_t dmpGetGyroSumOfSquare();
uint32_t dmpGetAccelSumOfSquare();
void dmpOverrideQuaternion(long *q);
uint16_t dmpGetFIFOPacketSize();
#endif
private:
uint8_t devAddr;
uint8_t buffer[14];
- 73 -
};
#endif /* _MPU6050_H_ */
- 74 -

Thiết Kế Hệ Thống Điều Khiển Mô Phỏng Chuyển Động Cánh Tay.

Uploaded by

Document Information

Copyright

Available Formats

Share this document

Share or Embed Document

Sharing Options

Did you find this document useful?

Is this content inappropriate?

Copyright:

Available Formats

Thiết Kế Hệ Thống Điều Khiển Mô Phỏng Chuyển Động Cánh Tay.

Uploaded by

Copyright:

Available Formats

BỘ GIÁO DỤC VÀ ĐÀO TẠO

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

Phạm Quốc Anh

THIẾT KẾ HỆ THỐNG ĐIỀU KHIỂN MÔ PHỎNG CHUYỂN ĐỘNG

Chuyên ngành : KỸ THUẬT Y SINH

LUẬN VĂN THẠC SĨ KỸ THUẬT

NGƯỜI HƯỚNG DẪN KHOA HỌC :

Phạm Quốc Anh.

1.1 Định nghĩa về mô phỏng và điều khiên mô phỏng:

2.1.1 Cấu tạo cánh tay

Hình 2-1. Cấu tạo xương cánh tay.

Hình 2-2. Cấu tạo xương cẳng tay.

Hình 2-3. Cấu tạo xương bàn tay.

1 Bẻ cổ tay – Khớp cổ tay (khớp động) 900

2 Gập cổ tay – Khớp cổ tay (khớp động) 990

5 Gập cẳng tay – Khớp cẳng tay ( khớp động ) 1190

6 Xoay cẳng tay – Khớp cẳng tay (khớp động) 770

7 Xoay cánh tay – Khớp vai ( khớp động) 1420

2.2 Cảm biến ghi nhận chuyển dộng.

Hình 2-4. Mô tả nguyên lý làm việc chung của cảm biến.

 Tốc độ đáp ứng

2.2.2 Cảm biến gia tốc góc

Hệ thống vi cơ điện tử (Micro Electro Mechanicals Systems – MEMS) là

Các ưu điểm và nhược điểm của MEMS:

+ Có thể bị ảnh hưởng bởi các nhiễu điện từ…

Ứng dụng của cảm biến gia tốc

 Cảm biến góc Roll –Pitch, góc Euler, góc

 Định hướng 3D trong không gian

 Đo và điều khiển mức rung

 Đo một số thông số sinh học trong cơ thể con người.

Nguyên tắc hoạt động của cảm biến vi cơ MEMS dạng tụ

Hệ thống có thể được mô tả bởi phương trình vi phân sau:

hay có thể viết lại là:

2.3 Giải thuật xử lý dữ liệu và lý thuyết điều khiển.

2.3.1 Giải thuật xử lý dữ liệu:

Hình 2-8. Tiến trình xử lý tín hiệu của hệ thống.

2.3.2 Lý thuyết điều khiển

Chuyển động của

PHỎNG CHUYỂN ĐỘNG CÁNH

Bộ cảm biến biến trở trượt Cảm biến la bàn MPU6050

VI XỬ LÝ/VI ĐIỀU KHIỂN

Tín hiệu được điều chế độ rông xung

3.2.1 Bộ cảm biến biến trở thanh trượt

Nguyên lý hoạt động của bộ cảm biến biến trở

3.2.2 Cảm biến gia tốc góc/ la bàn MPU6050

MPU6050 sử dụng công nghệ độc quyền MotionFusion của InvenSense có

Thông số truyền động:

 Có thể lựa chọn ±2/4/8/16g phạm vi gia tốc.

Hinh 3-5. Sơ đô khối của cảm biến la bàn MPU6050.

Chân giao tiếp MPU6050.

Hình 3-6. Module cảm biến MPU6050

VCC 2.375V – 3.46V

SDA Chân SDA trong giao tiếp I2C

XDA Chân dữ liệu (kết nối với cảm biến khác)

XCL Chân xung (kết nối với cảm biến khác)

AD0 Bito của địa chỉ I2C

INT Chân ngắt

 Để đọc một byte:

Ta chỉ có thể biết rõ điều này khi tham khảo datasheets.

3.3 Khối xử lý- Arduino Uno

3.3.1 Thông số của Arduino UNO R3

Vi điều khiển ATmega328 (họ 8bit)

Điện áp hoạt động 5V – DC (chỉ được cấp qua cổng USB)

Tần số hoạt động 16 MHz

uint8_t dmpGetEuler(float data, Quaternion q);