راه اندازی ماژول شتاب سنج و زاویه سنج ICM42688 با آردوینو

سنسورهای IMU (واحد اندازه‌گیری اینرسی) امروزه به عنوان یکی از اجزای کلیدی در بسیاری از سیستم‌های مدرن استفاده می‌شوند. این سنسورها قابلیت اندازه‌گیری شتاب (با استفاده از شتاب‌سنج) و چرخش زاویه‌ای (با استفاده از ژیروسکوپ) در سه محور مختصات را دارند، به طوری که ترکیب این دو قابلیت، امکان ردیابی دقیق حرکت را فراهم می‌کند.

سنسور IMU

معرفی ICM42688-P

ICM42688 سنسور IMU (واحد اندازه‌گیری اینرسی) از شرکت TDK InvenSense هستند که MEMS می باشد و برای ردیابی حرکت در دستگاه‌های مختلف استفاده می شود. این سنسور قابلیت اندازه‌گیری شتاب و زاویه در سه محور (6 محور کلی) را دارد و در برنامه‌ هایی مانند رباتیک، پهپادها، دستگاه‌های واقعیت افزوده و مجازی (AR/VR) و الکترونیک مصرفی به کار می رود. در واقع این سنسور نسخه بروز شده سنسور معروف MPU6050 هست که دیگر تولید نمی شود. همچنین این سنسور دقت بالاتر ، نویز پذیری کمتر و نرخ داده بالاتری نسبت به نسل قبلی خود یعنی MPU6050 دارد.

ICM42688-P

در این مطلب ما از ماژول این تراشه استفاده میکنیم که از فروشگاه امبددتک قابل تهیه است.

راه اندازی ICM42688 با آردوینو UNO و نمایش Roll , Pitch , Yaw روی نمایشگر Nokia 1202

سخت افزار مورد نیاز

LCD Nokia 1202
آردوینو UNO
ماژول ICM42688

نحوه اتصال سخت افزار

راه اندازی ماژول شتاب سنج و زاویه سنج ICM42688 با آردوینو

برای استفاده از سنسور ICM42688 ابتدا باید کتابخانه آردوینوی آن فراخوانی شود. از طریق این لینک می توانید به کتابخانه ها و برنامه نوشته شده دسترسی داشته باشید. برای اضافه کردن کتابخانه مورد نظر ابتدا در نرم افزار آردوینو به مسیر Sketch -> Include Library -> Add .ZIP Library رفته و فایل ICM42688.zip و u8g2-ste2007-main.zip را اضافه می کنیم. همچنین می توانید برای آشنایی بیشتر با LCD نوکیا و نحوه کار کردن با آن از این لینک دیدن کنید.

راه اندازی ماژول شتاب سنج و زاویه سنج ICM42688 با آردوینو

بررسی برنامه :

#include <Arduino.h>
#include <U8g2lib.h>
#include "ICM42688.h"


#ifdef U8X8_HAVE_HW_SPI
#include <SPI.h>
#endif
#ifdef U8X8_HAVE_HW_I2C
#include <Wire.h>
#endif
///LCD////////////
#define LCD_BACKLIGHT 7
#define LCD_CS 10
//////////////////


U8G2_STE2007_96X68_F_3W_SW_SPI u8g2(U8G2_R0, /* clock=*/ 13, /* data=*/ 11, /* cs=*/ 10, /* reset=*/ 8);

ICM42688 IMU(Wire, 0x68);


void u8g2_prepare(void) {
  u8g2.setFont(u8g2_font_6x10_tf);
  u8g2.setFontRefHeightExtendedText();
  u8g2.setDrawColor(1);
  u8g2.setFontPosTop();
  u8g2.setFontDirection(0);
}



void interrupt_setup(){
  
cli();//stop interrupts

//set timer1 interrupt at 5Hz
  TCCR1A = 0;// set entire TCCR1A register to 0
  TCCR1B = 0;// same for TCCR1B
  TCNT1  = 0;//initialize counter value to 0
  // set compare match register for 5hz increments
  OCR1A = 49999;// = (16*10^6) / (1*1024) - 1 (must be <65536)
  // turn on CTC mode
  TCCR1B |= (1 << WGM12);
  // Set CS12, CS11 and CS10 bits for 64 prescaler
  TCCR1B |= (0 << CS12) | (1 << CS11) | (1 << CS10);
  // enable timer compare interrupt
  TIMSK1 |= (1 << OCIE1A);


sei();//allow interrupts
}



void setup(void) {
  pinMode(LCD_BACKLIGHT, OUTPUT);
  digitalWrite(LCD_BACKLIGHT, HIGH);
  u8g2.begin();

  Serial.begin(115200);
  while(!Serial) {}

  // start communication with IMU
  int status = IMU.begin();
  if (status < 0) {
    Serial.println("IMU initialization unsuccessful");
    Serial.println("Check IMU wiring or try cycling power");
    Serial.print("Status: ");
    Serial.println(status);
    while(1) {}
  }

  // setting the accelerometer full scale range to +/-8G
  IMU.setAccelFS(ICM42688::gpm8);
  // setting the gyroscope full scale range to +/-500 deg/s
  IMU.setGyroFS(ICM42688::dps500);
  
  // set output data rate to 12.5 Hz
  IMU.setAccelODR(ICM42688::odr12_5);
  IMU.setGyroODR(ICM42688::odr12_5);

  interrupt_setup();
}

unsigned long timer = 0;
float roll = 0.00, pitch = 0.00 ,yaw = 0.00;  
float timeStep = 0.01;

char temp[10];
char temp1[10];
char temp2[10];
int i;


void loop(void) {

  // read the sensor
  timer = millis();
  IMU.getAGT();

  // display the data
  roll = atan2(IMU.accY() , IMU.accZ()) * 57.3;
  pitch = atan2((- IMU.accX()) , sqrt(IMU.accY() * IMU.accY() + IMU.accZ() * IMU.accZ())) * 57.3;
  yaw = (yaw + IMU.gyrZ()/100);
  
delay((timeStep*1000) - (millis() - timer));

}


ISR(TIMER1_COMPA_vect){


  u8g2.clearBuffer();

  u8g2_prepare();

  sprintf(temp, "roll = %d ",(int)roll);
  u8g2.drawStr( 0, 0, temp);

  sprintf(temp1, "pitch = %d ", (int)pitch);
  u8g2.drawStr( 0, 20, temp1);

  sprintf(temp2, "yaw = %d ", (int)yaw);
  u8g2.drawStr( 0, 40, temp2);


  u8g2.sendBuffer();
   
}

100

101

102

103

104

105

106

107

108

109

110

111

112

113

114

115

116

117

118

119

120

121

122

123

124

125

126

127

128

129

130

131

#include <Arduino.h>

#include <U8g2lib.h>

#include "ICM42688.h"

#ifdef U8X8_HAVE_HW_SPI

#include <SPI.h>

#endif

#ifdef U8X8_HAVE_HW_I2C

#include <Wire.h>

#endif

///LCD////////////

#define LCD_BACKLIGHT 7

#define LCD_CS 10

//////////////////

U8G2_STE2007_96X68_F_3W_SW_SPI u8g2(U8G2_R0, /* clock=*/ 13, /* data=*/ 11, /* cs=*/ 10, /* reset=*/ 8);

ICM42688 IMU(Wire, 0x68);

void u8g2_prepare(void) {

u8g2.setFont(u8g2_font_6x10_tf);

u8g2.setFontRefHeightExtendedText();

u8g2.setDrawColor(1);

u8g2.setFontPosTop();

u8g2.setFontDirection(0);

}

void interrupt_setup(){

cli();//stop interrupts

//set timer1 interrupt at 5Hz

TCCR1A = 0;// set entire TCCR1A register to 0

TCCR1B = 0;// same for TCCR1B

TCNT1 = 0;//initialize counter value to 0

// set compare match register for 5hz increments

OCR1A = 49999;// = (16*10^6) / (1*1024) - 1 (must be <65536)

// turn on CTC mode

TCCR1B |= (1 << WGM12);

// Set CS12, CS11 and CS10 bits for 64 prescaler

TCCR1B |= (0 << CS12) | (1 << CS11) | (1 << CS10);

// enable timer compare interrupt

TIMSK1 |= (1 << OCIE1A);

sei();//allow interrupts

}

void setup(void) {

pinMode(LCD_BACKLIGHT, OUTPUT);

digitalWrite(LCD_BACKLIGHT, HIGH);

u8g2.begin();

Serial.begin(115200);

while(!Serial) {}

// start communication with IMU

int status = IMU.begin();

if (status < 0) {

Serial.println("IMU initialization unsuccessful");

Serial.println("Check IMU wiring or try cycling power");

Serial.print("Status: ");

Serial.println(status);

while(1) {}

}

// setting the accelerometer full scale range to +/-8G

IMU.setAccelFS(ICM42688::gpm8);

// setting the gyroscope full scale range to +/-500 deg/s

IMU.setGyroFS(ICM42688::dps500);

// set output data rate to 12.5 Hz

IMU.setAccelODR(ICM42688::odr12_5);

IMU.setGyroODR(ICM42688::odr12_5);

interrupt_setup();

}

unsigned long timer = 0;

float roll = 0.00, pitch = 0.00 ,yaw = 0.00;

float timeStep = 0.01;

char temp[10];

char temp1[10];

char temp2[10];

int i;

void loop(void) {

// read the sensor

timer = millis();

IMU.getAGT();

// display the data

roll = atan2(IMU.accY() , IMU.accZ()) * 57.3;

pitch = atan2((- IMU.accX()) , sqrt(IMU.accY() * IMU.accY() + IMU.accZ() * IMU.accZ())) * 57.3;

yaw = (yaw + IMU.gyrZ()/100);

delay((timeStep*1000) - (millis() - timer));

}

ISR(TIMER1_COMPA_vect){

u8g2.clearBuffer();

u8g2_prepare();

sprintf(temp, "roll = %d ",(int)roll);

u8g2.drawStr( 0, 0, temp);

sprintf(temp1, "pitch = %d ", (int)pitch);

u8g2.drawStr( 0, 20, temp1);

sprintf(temp2, "yaw = %d ", (int)yaw);

u8g2.drawStr( 0, 40, temp2);

u8g2.sendBuffer();

}

در این برنامه خواندن اطلاعات IMU با استفاده از حلقه loop و نمایش اطلاعات در غالب Roll , Pitch , Yaw بر روی LCD با استفاده از وقفه تایمر انجام شده است. خط 18 شئ u8g2 را تعریف می کند که با استفاده از این شئ پایه متصل شده به LCD تغییر می کند. خط 20 شئ IMU را تعریف می کند که مشخص می کند که پروتکل ارتباطی ماژول I2C و آدرس آن 0X68 می باشد. تابع u8g2_prepare تنظیمات فونت اندازه رشته مورد نظر بر روی LCD را تعیین می کند. تابع interrupt_setup تنظیمات مربوط به وقفه تایمر می باشد که بر روی 5Hz تنظیم شده است. خط 64 تا 72 بررسی می کند چنانچه ماژول به آردوینو وصل نبود برنامه در این نقطه می ایستد. خط 75 محدوده مقیاس کامل برای شتاب سنج را مشخص می کند. محدوده مقیاس کامل برای شتاب سنج ها به طور کلی بسته به پلت فرم سخت افزاری ± 2/±4/±8/±16 گرم قابل برنامه ریزی است. هرچه رنج کمتر باشد، شتاب سنج به سیگنال های دامنه کمتر حساس تر خواهد بود. به عنوان مثال، برای اندازه‌گیری ارتعاشات کوچک روی میز، استفاده از یک FSR 2g داده‌های دقیق‌تری را ارائه می‌کند، زیرا به هر شتاب جزئی بسیار حساس است، در حالی که استفاده از محدوده 16 گرمی ممکن است برای اندازه‌گیری ارتعاشات فردی که راه می‌رود مناسب‌تر باشد.

خط 77 نرخ نمونه برداری (ODR) را مشخص می کند. ODR سرعتی است که حسگر اندازه‌گیری‌ها یا نمونه‌های جدیدی را به دست می‌آورد. ODR بر حسب تعداد نمونه در ثانیه (Hz) اندازه گیری می شود. تنظیمات ODR بالاتر منجر به نمونه های بیشتری در ثانیه می شود. بسته‌های حسگر مختلف اغلب با چندین ODR موجود عرضه می‌شوند، و معمولاً این به توسعه‌دهنده برنامه بستگی دارد که بر اساس نیازهای برنامه، از کدام ODR استفاده کند.

خط 100 عملیات خواندن از سنسور IMU را در محور های X , Y انجام می دهد و مقادیر خام خوانده شده را در دو متغیر IMU.accX , IMU.accY ذخیره می کند. برای خواندن محور Z نمی توان صرفا از شتاب سنج استفاده کرد چرا که چرخش حول محور Z (Yaw) به معنای دوران جسم به دور محور Z است. این نوع چرخش زاویه‌ای است، و به تغییرات موقعیت زاویه‌ای مرتبط است، نه تغییرات در شتاب خطی. شتاب‌سنج تنها می‌تواند تغییرات در شتاب خطی ناشی از حرکت یا نیروی گرانش را حس کند. وقتی جسم حول محور Z می‌چرخد، هیچ تغییر شتاب خطی خاصی در محورهای X، Y یا Z رخ نمی‌دهد که شتاب‌سنج بتواند آن را به‌طور مستقیم تشخیص دهد. بنابراین، ژیروسکوپ برای تشخیص و اندازه‌گیری Yaw لازم است، زیرا ژیروسکوپ می‌تواند نرخ تغییر زاویه حول محور Z را اندازه‌گیری کند، که برای محاسبه زاویه Yaw ضروری است.

راه اندازی ماژول شتاب سنج و زاویه سنج ICM42688 با آردوینو

در خطوط 103 و 104 اعداد خام roll و Pitch در خروجی سنسور به زوایای واقعی بین 0-360 درجه تبدیل شده اند. برای اطلاعات بیشتر میتوان به این لینک اشاره کرد.

نحوه محاسبه YAW با دو محور دیگر همانطور که گفته شد متفاوت است و این کار با استفاده از ژیروسکوپ انجام می شود. توجه شود که در این برنامه محاسبه YAW به صورت کاملا تقریبی می باشد. در محاسبه دقیق YAW باید از سنسور قطب نما (Compass) یا از روش های پیچیده دیگری همچون GPS ، INS یا روش های پیچیده ریاضی همچون فیلتر کالمن استفاده کرد.