IMUセンサーICM-20948とRaspberry Pi Pico(Arduino) を使って、PCで3Dのキューブを回転表示させてみました。
以前、MPU6050でやったものの、ICM-20948版です。
MPU6050 + Raspberry Pi Pico(Arduino) -> PCで3Dのキューブを回転表示
ICM-20948は「SparkFun 9DoF IMU Breakout – ICM-20948」ボードを使用しています。せんごくネット通販で購入しました。
ICM-20948は電源電圧は1.71V～3.6Vですが、I/O電圧が1.71V～1.95Vとなっており、Picoなど3.3Vで使用するにはI/O電圧の変換が必要です。このボードはI/O電圧の変換を行ってくれるので使いやすいです。
姿勢推定はMPU6050の時と同じくMadgwickAHRSフィルターを使用していますが、ICM-20948では加速度・ジャイロに加えて磁気を追加しています。
MPU6050は加速度3軸・ジャイロ3軸の6軸ですが、ICM-20948は磁気3軸が追加されて9軸となります。（加えて温度も取れます）
IMUセンサーICM-20948とRaspberry Pi Pico(Arduino) を使って、PCで3Dのキューブを回転表示させてみました。
姿勢推定はMadgwickAHRSフィルターを使用しています。 pic.twitter.com/StWU90mWd9— PONTA@電子工作⚡️ロボット制作⚡️プログラミング (@Elec_Robot) August 29, 2024
PC側のプログラムは前回と同じです。
https://github.com/ponta1/MPU6050/blob/main/MPU6050.py
Pico側のプログラムは以下になります。Arduino環境(earlephilhower版)です。
SparkFun 9DoF IMU Breakout – ICM 20948 – Arduino Libraryを使用しているので、あらかじめインストールしてください。
// SparkFun 9DoF IMU Breakout - ICM-20948 から値を取得する
// 接続 Pico ICM-20948
// Pin3 GND -> GND
// Pin36 3V3(OUT) -> VCC(3.3)
// Pin21 GP16 -> SDA
// Pin22 GP17 -> SCL
// ライブラリ
// SparkFun 9DoF IMU Breakout - ICM 20948 - Arduino Library
// ver 1.2.13
#include "Wire.h"
#include "ICM_20948.h"
#include "MadgwickAHRS.h"
#include "RPi_Pico_TimerInterrupt.h"
Madgwick MadgwickFilter;
#define TIMER_INTERVAL_US 20000L // 20ms=50Hz
//#define TIMER_INTERVAL_US 10000L // 10ms=100Hz
// Init RPI_PICO_Timer
RPI_PICO_Timer ITimer(1);
float roll = 0;
float pitch = 0;
float yaw = 0;
// The value of the last bit of the I2C address.
// On the SparkFun 9DoF IMU breakout the default is 1, and when the ADR jumper is closed the value becomes 0
#define AD0_VAL 1
ICM_20948_I2C myICM;
int led = 1;
bool TimerHandler(struct repeating_timer *t)
{
if (myICM.dataReady()) {
myICM.getAGMT();
// 加速度(mg)
float acc_x = myICM.accX() / 16384.0;
float acc_y = myICM.accY() / 16384.0;
float acc_z = myICM.accZ() / 16384.0;
// 角速度(度/s)
float gyro_x = myICM.gyrX();
float gyro_y = myICM.gyrY();
float gyro_z = myICM.gyrZ();
// 磁気(uT)
float mag_x = myICM.magX();
float mag_y = myICM.magY();
float mag_z = myICM.magZ();
//Madgwickフィルターを用いて、PRY（pitch, roll, yaw）を計算
MadgwickFilter.update(gyro_x, gyro_y, gyro_z, acc_x, acc_y, acc_z, mag_x, mag_y, mag_z);
//PRYの計算結果を取得する
roll = MadgwickFilter.getRoll();
pitch = MadgwickFilter.getPitch();
yaw = MadgwickFilter.getYaw();
}
else {
Serial.println("Waiting for data");
}
return true;
}
void setup() {
Serial.begin(115200);
while (!Serial){};
Serial.println("START---");
pinMode(LED_BUILTIN, OUTPUT);
digitalWrite(LED_BUILTIN, led);
Wire.setSDA(16); //I2C0 SDA
Wire.setSCL(17); //I2C0 SCL
Wire.setClock(400000);
Wire.begin();
bool initialized = false;
while (!initialized) {
myICM.begin(Wire, AD0_VAL);
Serial.print(F("Initialization of the sensor returned: "));
Serial.println(myICM.statusString());
if (myICM.status != ICM_20948_Stat_Ok) {
Serial.println("Trying again...");
delay(500);
}
else {
initialized = true;
}
}
MadgwickFilter.begin(1000000 / TIMER_INTERVAL_US); //Hz
//MadgwickFilter.setGain(1.0); // これを利用するにはMadgwickFilterのソース修正が必要 [参考] https://blog2.studiok-i.net/2489.html
ITimer.attachInterruptInterval(TIMER_INTERVAL_US, TimerHandler);
delay(100);
}
void loop() {
led ^= 1;
digitalWrite(LED_BUILTIN, led);
Serial.print(roll); Serial.print(",");
Serial.print(pitch); Serial.print(",");
Serial.println(yaw);
delay(20);
}
