M5AtomS3とToioで倒立振子に挑戦

main.cpp

#include <Arduino.h>
#include <M5Unified.h>
#include <MadgwickAHRS.h>

#define Serial   USBSerial

Madgwick madgwick = Madgwick();

float roll = 0;
float pitch = 0;
float yaw = 0;

static void imu_task(void*)
{
  int freq = 100;
  madgwick.begin(freq);
  while(true) {
    float val[6];
    M5.Imu.getGyro(&val[0], &val[1], &val[2]);
    M5.Imu.getAccel(&val[3], &val[4], &val[5]);
    madgwick.updateIMU(val[0], val[1], val[2], val[3], val[4], val[5]);
    roll = madgwick.getRoll();
    pitch = madgwick.getPitch();
    yaw = madgwick.getYaw();
    delay(1000 / freq);
    Serial.printf("roll: %f, pitch: %f, yaw: %f\r\n", roll, pitch, yaw);
  }
}

void setup() {
  // M5Stack の初期化
  auto cfg = M5.config();

  cfg.clear_display = true;  // default=true. clear the screen when begin.
  cfg.output_power  = true;  // default=true. use external port 5V output.
  cfg.internal_imu  = true;  // default=true. use internal IMU.
  cfg.internal_rtc  = true;  // default=true. use internal RTC.
  cfg.internal_spk  = true;  // default=true. use internal speaker.
  cfg.internal_mic  = true;  // default=true. use internal microphone.
  cfg.external_imu  = true;  // default=false. use Unit Accel & Gyro.
  cfg.external_rtc  = true;  // default=false. use Unit RTC.
  cfg.led_brightness = 64;   // default= 0. system LED brightness (0=off / 255=max) (※ not NeoPixel)

  M5.begin(cfg);

  Serial.begin(115200);

  xTaskCreatePinnedToCore(imu_task, "imu_task", 4096, NULL, 25, NULL, APP_CPU_NUM);
}

void loop() {
  int h = M5.Display.height() / 8;
  int w = M5.Display.width() / 2;


  int ox = (M5.Display.width()+h)>>1;
  static int prev_xpos[3];
  int xpos[3];
  float val[3];
  int color[3] = { TFT_RED, TFT_GREEN, TFT_BLUE };

  xpos[0] = roll * w / 180;
  xpos[1] = pitch * w / 180;
  xpos[2] = yaw * w / 180;

  M5.Display.startWrite();
  M5.Display.setClipRect(0, h, M5.Display.width(), M5.Display.height());
  M5.Display.waitDisplay();
  for (int i = 0; i < 3; ++i)
  {
    if (xpos[i] == prev_xpos[i]) continue;

    int px = prev_xpos[i];
    if ((xpos[i] < 0) != (px < 0))
    {
      if (px)
      {
        M5.Display.fillRect(ox, h * (i+2), px, h, M5.Display.getBaseColor());
      }
      px = 0;
    }
    if (xpos[i] != px)
    {
      if ((xpos[i] > px) != (xpos[i] < 0))
      {
        M5.Display.setColor(color[i]);
      }
      else
      {
        M5.Display.setColor(M5.Display.getBaseColor());
      }
      M5.Display.fillRect(xpos[i] + ox, h * (i+2), px - xpos[i], h);
    }
    prev_xpos[i] = xpos[i];
  }
  M5.Display.clearClipRect();

  M5.Display.endWrite();
  M5.Display.display();
}

platformio.ini

[env:ATOMS3]
platform = espressif32@5.2.0
framework = arduino
platform_packages = platformio/framework-arduinoespressif32@^3.20005.220925
board = esp32-s3-devkitc-1
lib_ldf_mode = deep
monitor_speed = 115200
upload_speed = 1500000
board_build.f_cpu = 240000000L
board_build.f_flash = 80000000L
board_build.flash_mode = dio
build_flags = 
	-DCORE_DEBUG_LEVEL=3
	-DUSE_USEBSERIAL
	-Iinclude
lib_deps = 
	M5Unified
	arduino-libraries/Madgwick@^1.2.0