M5StickCで角度を取得し，振動デバイスに出力する。
// M5StickC 初期型（MPU6886） Pitch / Roll 取得デモ
// 50Hz更新，相補フィルタ(α=0.98)
// 画面表示 + シリアルCSV出力（Pitch,Roll）
#include <M5StickC.h>
const float alpha = 0.98f; // 相補フィルタ係数（0.95〜0.99で調整）
const uint32_t dt_ms = 20; // サンプリング周期 20ms → 50Hz
const float dt = dt_ms / 1000.0f;
float pitch = 0.0f; // 融合後の角度[deg]
float roll = 0.0f;
void setup() {
M5.begin();
M5.IMU.Init(); // MPU6886 初期化
M5.Lcd.setRotation(3); // 横向き
M5.Lcd.fillScreen(BLACK);
M5.Lcd.setTextSize(2);
M5.Lcd.setCursor(0, 0);
M5.Lcd.println("M5StickC IMU");
M5.Lcd.println("Pitch/Roll");
Serial.begin(115200);
while (!Serial) { delay(10); }
// 初期姿勢を加速度ベースで推定して，初期値に採用
float ax, ay, az;
M5.IMU.getAccelData(&ax, &ay, &az); // [g]
float pitchAcc = atan2f(-ax, sqrtf(ay*ay + az*az)) * 180.0f / PI;
float rollAcc = atan2f( ay, az ) * 180.0f / PI;
pitch = pitchAcc;
roll = rollAcc;
delay(500);
}
void loop() {
// センサ取得
float ax, ay, az; // 加速度[g]
float gx, gy, gz; // 角速度[deg/s]
M5.IMU.getAccelData(&ax, &ay, &az);
M5.IMU.getGyroData(&gx, &gy, &gz);
// 加速度からの姿勢角（重力のみ仮定）
float pitchAcc = atan2f(-ax, sqrtf(ay*ay + az*az)) * 180.0f / PI;
float rollAcc = atan2f( ay, az ) * 180.0f / PI;
// ジャイロ積分（軸対応：MPU6886はおおむね gx→Roll, gy→Pitch）
float pitchGyro = pitch + gy * dt;
float rollGyro = roll + gx * dt;
// 相補フィルタ融合
pitch = alpha * pitchGyro + (1.0f - alpha) * pitchAcc;
roll = alpha * rollGyro + (1.0f - alpha) * rollAcc;
// 画面表示
M5.Lcd.fillRect(0, 40, 160, 40, BLACK);
M5.Lcd.setCursor(0, 40);
M5.Lcd.printf("Pitch:%6.2f deg\n", pitch);
M5.Lcd.printf("Roll :%6.2f deg\n", roll);
// シリアルCSV（Pitch,Roll）
Serial.print(pitch, 3);
Serial.print(",");
Serial.println(roll, 3);
delay(dt_ms);
}
// M5StickC 初期型（MPU6886） Pitch / Roll 取得＋Pitch>10°でG26出力
// 相補フィルタ(α=0.98) / 50Hz / ヒステリシス: ON 10°, OFF 8°
#include <M5StickC.h>
const int MOTOR_PIN = 26; // G26 にモータードライバの入力を接続（直結は不可）
const float alpha = 0.98f; // 相補フィルタ係数
const uint32_t dt_ms = 20; // 50Hz
const float dt = dt_ms / 1000.0f;
const float PITCH_ON_DEG = 10.0f; // これを超えたらON
const float PITCH_OFF_DEG = 8.0f; // これを下回ったらOFF（ヒステリシス）
float pitch = 0.0f;
float roll = 0.0f;
bool motorOn = false;
void setup() {
M5.begin();
M5.IMU.Init();
M5.Lcd.setRotation(3);
M5.Lcd.fillScreen(BLACK);
M5.Lcd.setTextSize(2);
M5.Lcd.setCursor(0, 0);
M5.Lcd.println("M5StickC IMU");
M5.Lcd.println("Pitch->G26");
Serial.begin(115200);
while (!Serial) delay(10);
// モーターピン初期化
pinMode(MOTOR_PIN, OUTPUT);
digitalWrite(MOTOR_PIN, LOW); // 初期はOFF
// 初期姿勢（加速度ベース）
float ax, ay, az;
M5.IMU.getAccelData(&ax, &ay, &az);
float pitchAcc = atan2f(-ax, sqrtf(ay*ay + az*az)) * 180.0f / PI;
float rollAcc = atan2f( ay, az ) * 180.0f / PI;
pitch = pitchAcc;
roll = rollAcc;
delay(300);
}
void loop() {
// センサ取得
float ax, ay, az; // [g]
float gx, gy, gz; // [deg/s]
M5.IMU.getAccelData(&ax, &ay, &az);
M5.IMU.getGyroData(&gx, &gy, &gz);
// 姿勢角（加速度）
float pitchAcc = atan2f(-ax, sqrtf(ay*ay + az*az)) * 180.0f / PI;
float rollAcc = atan2f( ay, az ) * 180.0f / PI;
// ジャイロ積分
float pitchGyro = pitch + gy * dt; // gy ≈ pitch軸
float rollGyro = roll + gx * dt; // gx ≈ roll軸
// 相補フィルタ融合
pitch = alpha * pitchGyro + (1.0f - alpha) * pitchAcc;
roll = alpha * rollGyro + (1.0f - alpha) * rollAcc;
// —— Pitchしきい値でG26制御（ヒステリシスあり）——
if (!motorOn && pitch > PITCH_ON_DEG) {
motorOn = true;
digitalWrite(MOTOR_PIN, HIGH);
} else if (motorOn && pitch < PITCH_OFF_DEG) {
motorOn = false;
digitalWrite(MOTOR_PIN, LOW);
}
// 表示＆ログ
M5.Lcd.fillRect(0, 40, 160, 50, BLACK);
M5.Lcd.setCursor(0, 40);
M5.Lcd.printf("Pitch:%6.2f deg\n", pitch);
M5.Lcd.printf("Roll :%6.2f deg\n", roll);
M5.Lcd.printf("MOTOR:%s \n", motorOn ? "ON " : "OFF");
Serial.print(pitch, 3);
Serial.print(",");
Serial.print(roll, 3);
Serial.print(",");
Serial.println(motorOn ? 1 : 0);
delay(dt_ms);
}
// M5StickC 初期型（MPU6886） Pitch / Roll 取得＋角度帯域でG26出力パターン
#include <M5StickC.h>
const int MOTOR_PIN = 26; // モータードライバ入力（直結不可）
const float alpha = 0.98f; // 相補フィルタ係数
const uint32_t dt_ms = 20; // 50Hz
const float dt = dt_ms / 1000.0f;
// 角度しきい値（度）：必要に応じて設定してください
float TH_A = 5.0f;
float TH_B = 10.0f;
float TH_C = 15.0f;
float TH_D = 20.0f;
// パターン定義（ONは常に100ms固定）
const uint16_t ON_MS = 100;
const uint16_t OFF_A = 500; // A〜B未満
const uint16_t OFF_B = 300; // B〜C未満
const uint16_t OFF_C = 100; // C〜D未満
float pitch = 0.0f;
float roll = 0.0f;
// -------- サブルーチン：角度に応じて出力を駆動（非ブロッキング）--------
enum Band { BAND_OFF, BAND_A, BAND_B, BAND_C, BAND_ON };
Band currentBand = BAND_OFF;
bool phaseOn = false;
unsigned long phaseStartMs = 0;
Band classifyBand(float ang) {
if (ang < TH_A) return BAND_OFF;
else if (ang < TH_B) return BAND_A;
else if (ang < TH_C) return BAND_B;
else if (ang < TH_D) return BAND_C;
else return BAND_ON;
}
// 指定した角度に従って G26 を点滅/常時ON/OFF させる
void driveOutputByAngle(float angDeg) {
Band band = classifyBand(angDeg);
unsigned long now = millis();
// 帯域が変わったらフェーズをリセット（各帯域は100ms ON から開始）
if (band != currentBand) {
currentBand = band;
phaseOn = true; // まずはON
phaseStartMs = now;
if (band == BAND_OFF) {
digitalWrite(MOTOR_PIN, LOW);
} else if (band == BAND_ON) {
digitalWrite(MOTOR_PIN, HIGH);
} else {
digitalWrite(MOTOR_PIN, HIGH); // 点滅帯域の初回ON
}
}
// 各帯域の動作
switch (currentBand) {
case BAND_OFF:
// ずっとOFF
digitalWrite(MOTOR_PIN, LOW);
break;
case BAND_ON:
// ずっとON
digitalWrite(MOTOR_PIN, HIGH);
break;
case BAND_A: {
// 100ms ON / 500ms OFF
uint16_t offMs = OFF_A;
if (phaseOn && (now - phaseStartMs >= ON_MS)) {
phaseOn = false; phaseStartMs = now; digitalWrite(MOTOR_PIN, LOW);
} else if (!phaseOn && (now - phaseStartMs >= offMs)) {
phaseOn = true; phaseStartMs = now; digitalWrite(MOTOR_PIN, HIGH);
}
} break;
case BAND_B: {
// 100ms ON / 300ms OFF
uint16_t offMs = OFF_B;
if (phaseOn && (now - phaseStartMs >= ON_MS)) {
phaseOn = false; phaseStartMs = now; digitalWrite(MOTOR_PIN, LOW);
} else if (!phaseOn && (now - phaseStartMs >= offMs)) {
phaseOn = true; phaseStartMs = now; digitalWrite(MOTOR_PIN, HIGH);
}
} break;
case BAND_C: {
// 100ms ON / 100ms OFF
uint16_t offMs = OFF_C;
if (phaseOn && (now - phaseStartMs >= ON_MS)) {
phaseOn = false; phaseStartMs = now; digitalWrite(MOTOR_PIN, LOW);
} else if (!phaseOn && (now - phaseStartMs >= offMs)) {
phaseOn = true; phaseStartMs = now; digitalWrite(MOTOR_PIN, HIGH);
}
} break;
}
}
// -------- サブルーチンここまで --------
void setup() {
M5.begin();
M5.IMU.Init();
M5.Lcd.setRotation(3);
M5.Lcd.fillScreen(BLACK);
M5.Lcd.setTextSize(2);
M5.Lcd.setCursor(0, 0);
M5.Lcd.println("M5StickC IMU");
M5.Lcd.println("Angle->G26");
Serial.begin(115200);
while (!Serial) delay(10);
pinMode(MOTOR_PIN, OUTPUT);
digitalWrite(MOTOR_PIN, LOW);
// 初期姿勢（加速度ベース）
float ax, ay, az;
M5.IMU.getAccelData(&ax, &ay, &az);
float pitchAcc = atan2f(-ax, sqrtf(ay*ay + az*az)) * 180.0f / PI;
float rollAcc = atan2f( ay, az ) * 180.0f / PI;
pitch = pitchAcc;
roll = rollAcc;
delay(300);
}
void loop() {
// センサ取得
float ax, ay, az; // [g]
float gx, gy, gz; // [deg/s]
M5.IMU.getAccelData(&ax, &ay, &az);
M5.IMU.getGyroData(&gx, &gy, &gz);
// 姿勢角（加速度）
float pitchAcc = atan2f(-ax, sqrtf(ay*ay + az*az)) * 180.0f / PI;
float rollAcc = atan2f( ay, az ) * 180.0f / PI;
// ジャイロ積分
float pitchGyro = pitch + gy * dt; // gy ≈ pitch軸
float rollGyro = roll + gx * dt; // gx ≈ roll軸
// 相補フィルタ融合
pitch = alpha * pitchGyro + (1.0f - alpha) * pitchAcc;
roll = alpha * rollGyro + (1.0f - alpha) * rollAcc;
// —— ここで角度に応じた出力を実行（Pitchを使用）——
driveOutputByAngle(pitch);
// driveOutputByAngle(roll); // Rollで判定したい場合はこちら
// 画面表示＆ログ
M5.Lcd.fillRect(0, 40, 160, 60, BLACK);
M5.Lcd.setCursor(0, 40);
M5.Lcd.printf("Pitch:%6.2f deg\n", pitch);
M5.Lcd.printf("Roll :%6.2f deg\n", roll);
Serial.print(pitch, 3);
Serial.print(",");
Serial.print(roll, 3);
Serial.print(",");
Serial.println((currentBand==BAND_ON) ? 2 : (currentBand==BAND_OFF ? 0 : 1)); // 2=ON連続,1=点滅,0=OFF
delay(dt_ms);
}