3.2.1 Module cảm biến MPU6050[1][8]
Cảm biến MPU-6050 là dòng cảm biến chuyển động tích hợp bên trong 3 cảm biến gia tốc và 3 cảm biến con quay hồi chuyển, cho phép tính toán sự chuyển động của cảm biến hoặc hệ vật được gắn cảm biến. Dữ liệu được số hóa thông qua các bộ ADCs và có thể xử lý chuyển động (Digital Motion Processor) trước khi đưa vào các thanh ghi. MPU-6050 có thể được cài đặt các thông số từ vi điều khiển bên ngoài thông qua giao thức I2C hoặc SPI.
Trong đề tài này MPU6050 được sử dụng để đo giá trị góc nghiêng (Trục Elevation) của hệ thống.
Hình 3.2: Hình ảnh thực tế Module MPU-6050
Một số đặc tính của module cảm biến MPU-6050:
Dải điện áp hoạt động: 2.3V – 3.4V Nhiệt độ hoạt động tốt nhất: 250C Dải nhiệt hoạt động: -40 đến +850C
Chuẩn giao tiếp I2C (Fast mode và Normal mode) hoặc SPI Khả năng chịu chấn động: 10000g
Khoảng đo lường của cảm biến gia tốc: 2g – 4g – 8g – 16g
Khoảng đo lường của con quay hồi chuyển: 250 – 500 – 1000 – 2000 (Degree/sec)
19
Hình 3.3: Sơ đồ nguyên lý của module MPU-6050 Bảng 3.1: Chức năng các chân của module MPU-6050
Chân Tên Chức năng
PIN1 VCC Chân nguồn 3.3V
PIN2 GND Chân mass
PIN3 INT Chân Interrupt
PIN4 SCL Serial Clock
PIN5 SDA Serial Data
PIN6 DA0 Chọn địa chỉ I2C (0x68, 0x69)
PIN7 ASCL Auxilary Serial Clock
PIN8 ASDA Auxilary Serial Data
Bảng 3.2: Các thanh ghi cơ bản thiết lập hoạt động MPU-6050
Địa chỉ (Hex) Tên thanh ghi Chức năng
0x19 SMPLRT_DIV Thiết lập hệ số chia tính tốc độ lấy mẫu
0x1A CONFIG Cài đặt bộ lọc hạ thông, đồng bộ dữ liệu
0x1B GYRO_CONFIG Thiết lập tốc độ lấy mẫu cho con quay hồi chuyển
0x1C ACCEL_CONFIG Thiết lập tốc độ lấy mẫu cho cảm biến gia tốc
0x6C PWR_MGMT_1 Khởi động cảm biến
0x75 WHO_I_AM Kiểm tra kết nối với cảm biến
3.2.1.1 Thiết lập cảm biến hoạt động :
**Thiết lập thanh ghi PWR_MGMT_1 (địa chỉ 0x68)
Thanh ghi này cho phép người dung cấu hình chế độ nguồn và xung clock của cảm biến. Nó cũng hỗ trợ bit Reset thiết bị hoặc khóa hoạt động của cảm biến nhiệt độ.
20 Bit DEVECE_RESET: khi bit này có giá trị là 1, tất car thanh ghi sẽ đưa về trạng thái mặc định ban đầu, bit sẽ tự động chuyển giá trị 0 khi quá trình reset kết thúc.
Bit SLEEP: khi bit này được thiết lập lên 1, cảm biến sẽ vào chế độ ngủ Bit CYCLE: khi thiết lập bit này lên 1 và khóa chế độ SLEEP, cảm biến sẽ
thay dổi liên tục giữa 2 trạng thái ngủ và thức để đọc dữ liệu
Bit TEMP_DI: khi thiết lập bit này lên 1, cảm biến nhiệt độ sẽ bị khóa. Bit CLKSEL: dung để lựa chọn nguồn xung clock cho cảm biến
Bảng 3.3: Nguồn xung clock cho con quay hồi chuyển
CLKSEL Clock Source
0 Internal 8MHz oscillator
1 PLL with X axis gyroscope reference
2 PLL with Y axis gyroscope reference
3 PLL with Z axis gyroscope reference
4 PLL with external 32.768kHz reference
5 PLL with external 19.2MHz reference
6 Reserved
7 Stops the clock and keeps the timing generator in reset
Ta sẽ ghi giá trị 0x08 và thanh ghi PWR_MGMT_1 nhằm khóa cảm biến nhiệt độ khi module hoạt động và đưa cảm biến vào chế độ hoạt động liên tục.
**Thiết lập thanh ghi CONFIG (địa chỉ 0x1A)
Thanh ghi CONFIG dung để cấu hình chân đồng bộ khung mở rộng và bộ lọc hạ thông số cho cảm biến gia tốc và con quay hồi chuyển.
Hình 3.5: Thanh ghi CONFIG
Bit EXT_SYNC_SET: thiết lập lấy mẫu chân FSYNC theo bảng tham chiếu đồng bộ mở rộng.
Bảng 3.4: Bảng tham chiếu đồng bộ mở rộng
EXT_SYNC_SET FSYNC Bit Localtion
0 Input disabled 1 TEMP_OUT_L[0] 2 GYRO_XOUT_L[0] 3 GYRO_YOUT_L[0] 4 GYRO_ZOUT_L[0] 5 ACCEL_XOUT_L[0] 6 ACCEL_YOUT_L[0] 7 ACCEL_ZOUT_L[0]
21 Bit DLPF_CFG: thiết lập bộ lọc hạ thông số theo bảng
Bảng 3.5: Chỉ số bộ lọc hạ thông DLPF_CF G Accelerometer (Fs = 1kHz) Gyroscope
Bandwidth(Hz) Delay(ms) Bandwidth(Hz) Delay(ms) Fs(kHz)
0 260 0 256 0.98 8 1 184 2.0 188 1.9 1 2 94 3.0 98 2.8 1 3 44 4.9 42 4.8 1 4 21 8.5 20 8.2 1 5 10 13.8 10 13.4 1 6 5 19.0 5 18.6 1 7 RESERVED RESERVED 8
Ghi giá trị 0x03 vào thanh ghi CONFIG để sử dụng bộ lọc hạ thông 42Hz cho cảm biến gia tốc và 44Hz cho con quay hồi chuyển.
**Thiết lập thanh ghi SMPLRT_DIV (địa chỉ 0x19):
Thanh ghi này chứa thông số cho bộ chia dùng để thiết lập tốc độ lấy mẫu dựa vào tốc độ ngõ ra của con quay hồi chuyển MPU-6050.
Tốc độ ngõ ra của cảm biến được xác định bằng công thức (1) sau:
𝑆𝑎𝑚𝑝𝑙𝑒 𝑅𝑎𝑡𝑒 = 𝐺𝑦𝑟𝑜𝑠𝑐𝑜𝑝𝑒 𝑂𝑢𝑡𝑝𝑢𝑡 𝑅𝑎𝑡𝑒
1 + 𝑆𝑀𝑃𝐿𝑅𝑇_𝐷𝐼𝑉 (1)
Hình 3.6: Cấu trúc thanh ghi SMPLRT_DIV
Để thiết lập tốc độ lấy mẫu của MPU-6050 là 100Hz thì ta cần chọn SMPLRT_DIV là 9, vậy ghi giá trị 0x09 vào thanh ghi SMPLRT_DIV.
**Thanh ghi GYRO_CONFIG:
Thanh ghi này cho phép kích hoạt quá trình tự kiểm tra cấu hình khoảng đo đạc của con quay hồi chuyển. Quá trình tự kiểm tra sẽ cho phép người dùng kiểm tra phần cơ khí cũng như phần điện của con quay hồi chuyển. Tự kiểm tra trên mỗi trục có thể được kích hoạt bằng việc điều khiển các bit XG_ST, YG_ST và ZG_ST trong thanh ghi. Quá trình tự kiểm tra của mỗi trục có thể hoạt động độc lập hoặc cùng một lúc, kết quả của tự kiểm tra được xác định như sau:
Đáp ứng quá trình tự kiểm tra = giá trị đo đạc khi tự kiểm tra được cho phép - giá trị đo đạc khi tự kiểm tra không được cho phép.
22 Bit XG_ST: khi nhận giá trị 1, cho phép tự kiểm tra trục x
Bit YG_ST: khi nhận giá trị 1, cho phép tự kiểm tra trục y Bit ZG_ST : khi nhận giá trị 1, cho phép tự kiểm tra trục z Bit FS_SEL: lựa chọn khoảng đo đạc cho con quay hồi chuyển
Bảng 3.6: Các khoảng đo đạc của con quay hồi chuyển
FS_SEL Full Scale Range
0 ± 250 o/s
1 ± 500 o/s
2 ± 1000 o/s
3 ± 2000 o/s
Đối với thanh ghi này, ta không cần quá trình tự kiểm tra, chỉ cần thiết lập khoảng đo đạc cho con quay hồi chuyển là 1000 độ/s. Ghi giá trị 0x10 vào thanh ghi GYRO_CONFIG.
**Thanh ghi ACCEL_CONFIG:
Thanh ghi này cho phép kích hoạt quá trình tự kiểm tra và cấu hình khoảng đo đạc của cảm biến gia tốc. Quá trình tự kiểm tra sẽ cho phép người dùng kiểm tra phần cơ khí cũng như phần điện của cảm biến gia tốc. Tự kiểm tra trên mỗi trục có thể được kích hoạt bằng việc điều khiển các bit XA_ST, YA_ST và ZA_ST trong thanh ghi. Quá trình tự kiểm tra của mỗi trục có thể hoạt động độc lập hoặc cùng một lúc, kết quả của tự kiểm tra được xác định như sau:
Đáp ứng quá trình tự kiểm tra = giá trị đo đạc khi tự kiểm tra được cho phép – giá trị đo đạc khi tự kiểm tra không được cho phép.
Hình 3.8: Cấu trúc thanh ghi ACCEL_CONFIG
Bit XA_ST: khi nhận giá trị 1, cho phép tự kiểm tra trục x Bit YA_ST: khi nhận giá trị 1, cho phép tự kiểm tra trục y Bit ZA_ST : khi nhận giá trị 1, cho phép tự kiểm tra trục z Bit AFS_SEL: lựa chọn khoảng đo đạc cho cảm biến gia tốc
Bảng 3.7: Các khoảng đo đạc của gia tốc kế
AFS_SEL Full Scale Range
0 ± 2g
1 ± 4g
2 ± 8g
3 ± 16g
Đối với thanh ghi này, ta không cần quá trình tự kiểm tra, chỉ cần thiết lập khoảng đo đạc cho cảm biến gia tốc là 2 g/s. Ghi giá trị 0x00 vào thanh ghi ACCEL_CONFIG.
23
Bảng 3.8: Bảng các thanh ghi chứ dữ liệu cảm biến MPU-6050
Addr (Hex) Addr (Dec) Register Name 3B 59 ACCEL_XOUT_H ACCEL_XOUT[15:8] 3C 60 ACCEL_XOUT_L ACCEL_XOUT[7:0] 3D 61 ACCEL_YOUT_H ACCEL_YOUT[15:8] 3E 62 ACCEL_YOUT_L ACCEL_YOUT[7:0] 3F 63 ACCEL_ZOUT_H ACCEL_ZOUT[15:8] 40 64 ACCEL_ZOUT_L ACCEL_ZOUT[7:0] 43 67 GYRO_XOUT_H GYRO_XOUT[15:8] 44 68 GYRO_XOUT_L GYRO_XOUT[7:0] 45 69 GYRO_YOUT_H GYRO_YOUT[15:8] 46 70 GYRO_YOUT_L GYRO_YOUT[7:0] 47 71 GYRO_ZOUT_H GYRO_ZOUT[15:8] 48 72 GYRO_ZOUT_L GYRO_ZOUT[7:0]
3.2.1.2 Đọc giá trị từ cảm biến MPU-6050:
Cảm biến sau khi đo đạc với tần số lấy mẫu đã thiết lập sẽ lưu các giá trị vào thanh ghi 8 bit. Do bộ ADC của cảm biến có 16 bit nên dữ liệu gia tốc và vận tốc gốc mỗi trục sẽ được chứa trong 2 thanh ghi: 1 thanh ghi chứa dữ liệu 8 bit cao và 1 thanh ghi chứa dữ liệu 8 bit thấp (bảng 3.8). Để có được dữ liệu thì ta phải ghép các bit ở thanh ghi chứa dữ liệu mức cao và thanh ghi chứa dữ liệu mức thấp.
3.2.2 Module cảm biến la bàn số HMC5883L[7]
HMC5883L được thiết kế cho cảm biến từ trường thấp với giao diện kỹ thuật số cho các ứng dụng như xác định hướng nhờ vào từ trường. HMC5883L gồm cảm biến trở-từ (HMC118X) hiện đại và có độ phân dải cao, tự động triệt từ bằng các trình điều khiển, và một bộ ADC 12bit cho phép độ chính xác la bàn từ 1°-2°.
Đây là module thường được dùng trong các ứng dụng cần xác định tọa độ, vị trí, phương hướng, định vị.... HMC5883L Honeywell sử dụng kỹ thuật Anisotropic Magnetoresistive (AMR) (từ trở dị hướng) của hãng Honeywell, nó có rất nhiều ưu điểm so với công nghệ cảm biến từ khác. Tính chất không đẳng hướng, nét đặc biệt trên từng hướng cảm biến là độ chính xác về độ nhạy và độ tuyến tính trên từng trục. Với khả năng phù hợp cho hệ thống nên nhóm lựa chọn module HMC5883L để xác định trục phương vị (Azimuth).
24
Các đặt tính của module HMC5883L:
Điện áp hoạt động : 2.16 – 3.6V, tiêu thụ điện áp thấp (100uA) Chuẩn giao tiếp : I2C
Độ rộng dải từ trường : (±8Oe)
Tần số tối đa mà vẫn cho đầu ra ổn định: 160Hz
Hình 3.10: Sơ đồ nguyên lý của module HMC5883L
Bảng 3.9: Chức năng các chân của Module HMC5883L
Chân Tên Chức năng
PIN1 VCC Chân nguồn 3.3V
PIN2 GND Chân mass
PIN3 SCL Serial Clock
PIN4 SDA Serial Data
PIN5 DRDY Data Ready, Interrupt Pin
Ta sử dụng chân 3, 4 để giao tiếp với module, cấp nguồn và mass chân 1, 2 chân 5 bỏ trống.
Bảng 3.10: Danh sách các thanh ghi của HMC5883L
Address Location Name Access
00 Configuration Register A Read/Write
01 Configuration Register B Read/Write
02 Mode Register Read/Write k
03 Data Output X MSB Register Read
04 Data Output X LSB Register Read
05 Data Output Z MSB Register Read
06 Data Output Z LSB Register Read
07 Data Output Y MSB Register Read
08 Data Output Y LSB Register Read
09 Status Register Read
10 Identification Register A Read
11 Identification Register B Read
25
3.2.2.1 Thiết lập các thanh ghi
**Thanh ghi cấu hình A: CRA
Địa chỉ của thanh ghi CRA (0x00)
Hình 3.11: Sơ đồ thanh ghi CRA
Các bit trong thanh ghi CRA được miêu tả như sau:
Bảng 3.11: Miêu tả các bit của thanh ghi CRA
Bit Tên Miêu tả chức năng
CRA7 CRA7 Bit này được dùng trong tương lại, đặt ở mức 0 khi thiết lập
CRA6 - CRA5 MA1 - MA0 Chọn số lượng mẫu trung bình (1 đến 8) cho mỗi ngõ ra. 00 = 1(mặc định); 01 = 2; 10 = 4; 11 = 8
CRA4 - CRA2 DO2 – DO0 Tốc độ bit ngõ ra. Thiết lập tốc độ mà dữ liệu được ghi vào thanh ghi.
CRA1 – CRA0 MS1 – MS0 Bit cấu hình đo lường
Hình 3.12: Tốc độ bit ngõ ra
Hình 3.13: Thiết lập cho thanh ghi CRA
Thiết lập thanh ghi: Chọn số lượng lấy mẫu trung bình là 8, tốc độ 15 Hz, Chế độ đo lường là bình thường. Kết quả ta ghi vào thanh ghi giá trị 0x70.
26
**Thanh ghi cấu hình B: CRB
Địa chỉ của thanh ghi CRB (0x01)
Hình 3.14: Thanh ghi CRB
Các bit của thanh ghi CRB được miêu tả như sau:
Bảng 3.12: Miêu tả các bit trong thanh ghi CRB
Địa chỉ Tên Miêu tả chức năng
CRB7 – CRB5 GN2 – GN0 Cấu hình bit độ lợi cho thiết bị
CRB4 – CRB0 0 Đưa về 0
Chỉ có 3 bit GN2, GN1, GN0 được sử dụng để thiết lập, các bit còn lại phải xóa về 0. Hình bên dưới là bảng setting gain dựa vào giá trị của GN2-GN0.
Hình 3.15: Thiết lập phạm vi cảm biến
Chọn gain = 001 phạm vi ở mức ±1.3 Ga, ghi vào thanh ghi CRB giá trị là 0x20.
**Thanh ghi chọn chế độ hoạt động MODE
Thanh ghi MODE có địa chỉ 0x02
Các bit trong thanh ghi MODE được miêu tả trong hình sau:
27
Bảng 3.13: Miêu tả các bit trong thanh ghi MODE
Địa chỉ Tên Miêu tả chức năng
MR7 – MR2 HS Thiết lập ở chế độ I2C mức cao, 3400kHz.
MR1 – MR0 MD1 – MD0 Bit thiết lập chế độ cho thiết bị
2 Bit MD1, MD0 được dùng để chọn chế độ đo liên tục hay chỉ đo một lần. - 00 : Chế độ đo liên tục
- 01 : Chế độ đo chỉ 1 lần
Hệ thống sử dụng đo liên tục nên ta chọn MD1, MD0 là 00, ta ghi vào thanh ghi MODE giá trị là 0x00 (Continuous-measurement mode).
3.2.2.2 Hiệu chỉnh cảm biến la bàn[5]
Module HMC5883L với giá thành rẻ, sự ảnh hưởng của các linh kiện, board mạch sản xuất và nơi sản xuất nên không tránh khỏi việc sai số. Để giảm thiểu sai số đó chúng ta có thể hiệu chỉnh lại cảm biến.
Những gì cần làm là nhận ra được các giá trị bồi thường của x,y,z từ cảm biến. Phương pháp đơn giản đó là xoay cảm biến ở mọi phương hướng, biễu diễn dữ liệu thu được trên Matlab hoặc phần mềm mà bạn thích, từ đó rút ra những sai số và bồi thường vào để có được dữ liệu chính xác hơn.
Dựa vào đồ thị vẽ ra, ta có thể xác định được tâm dữ liệu lệch bao nhiêu so với tâm của đồ thị, ước lượng được khoảng sai số. Từ đó ta thêm các giá trị bù trừ vào chương trình, tiếp tục lặp lại công việc lấy mẫu và vẽ đồ thị cho đến khi đạt như mong muốn.
**Hiệu chỉnh bằng cách trực quan dựa vào đồ thị
Hình 3.17: Dữ liệu ban đầu
Như (Hình 3.17) đây là dữ liệu ban đầu của module HMC5883L mà nhóm sử dụng, ta nhận thấy tâm hình tròn của dữ liệu là điểm A có tọa độ khoảng (-200,-90) lệch rất lớn so với tâm đồ thị (0,0). Ta thêm 2 giá trị bù trừ này vào hệ thống , tiếp tục lấy mẫu và vẽ lại đồ thị mới.
28
Hình 3.18: Dữ liệu sau khi thay đổi x + 185, y + 80
Dữ liệu sau khi hiệu chỉnh, cụ thể các giá trị bù trừ như sau: x = x + 185, y = y + 80, ta được hình tròn có tâm gần trùng với gốc O(0,0) hơn.
Để tìm ra các giá trị bù trừ chính xác hơn, ta có thể dùng phần mềm để tìm giá trị sai số.
**Hiệu chỉnh bằng phần mềm
Bên cạnh việc vẽ ra hình ảnh của dữ liệu cảm biến, chúng ta có thể lập trình để tìm ra các giá trị bù trừ, ở phương pháp này thì các giá trị tìm được chính xác hơn. Bằng cách tìm các giá trị Max và Min của 3 trục X, Y, Z sau khi quay module HMC5883L ở mọi hướng khác nhau, giá trị bù trừ của từng trục sẽ bằng tổng của MAX và MIN của trục đó chia cho 2. Ví dụ tìm giá trị bù trừ của trục X (X offset) được tính theo công thức (2):
𝑋 𝑜𝑓𝑓𝑠𝑒𝑡 = (𝑀𝐴𝑋𝑥 + 𝑀𝐼𝑁𝑥 )
2 (2)
3.3 ĐỘNG CƠ BƯỚC
Động cơ bước là một loại động cơ điện có nguyên lý và ứng dụng khác biệt với đa số các động cơ điện thông thường. chúng không quay theo cơ chế thông thường mà quay theo từng bước nên có độ chính xác rất cao về mặt điều khiển.
Động cơ bước được ứng dụng nhiều trong ngành Tự động hóa, chúng được ứng dụng trong các thiết bị cần điều khiển chính xác. Ví dụ: Robot, điều khiển tiêu cự trong các hệ quang học, điều khiển định vị trong hệ quang trắc, điều khiển bắt, bám mục tiêu trong các khí tài quan sát, điều khiển lập trình trong các thiết bị gia công cắt gọt,…
Ở trên thị trường có rất nhiều động cơ bước có góc bước khác nhau, nhưng