
Arduino入門
#include <M5Unified.h>
#include <math.h>
float accX = 0.0F;
float accY = 0.0F;
float accZ = 0.0F;
float pitch = 0.0F;
float roll = 0.0F;
float yaw = 0.0F;
uint32_t lastUpdate = 0;
constexpr float RAD_TO_DEGREE = 57.2957795F;
constexpr float ALPHA = 0.98F;
void printRow(int16_t y, const char* axisLabel, float axisValue,
const char* angleLabel, float angleValue) {
M5.Display.setCursor(5, y);
M5.Display.print(axisLabel);
M5.Display.setCursor(30, y);
M5.Display.printf("%6.2f", axisValue);
M5.Display.setCursor(95, y);
M5.Display.print(angleLabel);
M5.Display.setCursor(155, y);
M5.Display.printf("%6.2f", angleValue);
}
void setup() {
auto cfg = M5.config();
M5.begin(cfg);
M5.Display.setRotation(1);
M5.Display.fillScreen(TFT_BLACK);
M5.Display.setTextFont(4);
M5.Display.setTextSize(1);
M5.Display.setTextColor(TFT_WHITE, TFT_BLACK);
M5.Display.setCursor(100, 2);
M5.Display.print("IMU");
lastUpdate = millis();
}
void loop() {
if (!M5.Imu.update()) {
delay(10);
return;
}
auto data = M5.Imu.getImuData();
accX = data.accel.x;
accY = data.accel.y;
accZ = data.accel.z;
uint32_t now = millis();
float deltaTime = (now - lastUpdate) * 0.001F;
lastUpdate = now;
deltaTime = constrain(deltaTime, 0.0F, 0.5F);
const float accelPitch =
atan2f(-accX, sqrtf(accY * accY + accZ * accZ)) * RAD_TO_DEGREE;
const float accelRoll =
atan2f(accY, accZ) * RAD_TO_DEGREE;
pitch = ALPHA * (pitch + data.gyro.y * deltaTime)
+ (1.0F - ALPHA) * accelPitch;
roll = ALPHA * (roll + data.gyro.x * deltaTime)
+ (1.0F - ALPHA) * accelRoll;
yaw += data.gyro.z * deltaTime;
if (yaw > 180.0F) yaw -= 360.0F;
if (yaw < -180.0F) yaw += 360.0F;
printRow(36, "X:", accX, "Pitch:", pitch);
printRow(69, "Y:", accY, "Roll:", roll);
printRow(102, "Z:", accZ, "Yaw:", yaw);
delay(100);
}このプログラムは、X/Y/Zの3軸加速度と、加速度・ジャイロデータから算出したPitch、Roll、Yaw姿勢角を大きなフォントで行ごとに画面表示し、約100 msごとに更新します。
