10 | 11月 | 2025 | E-152教室

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);
}

月	火	水	木	金	土	日
					1	2
3	4	5	6	7	8	9
10	11	12	13	14	15	16
17	18	19	20	21	22	23
24	25	26	27	28	29	30