M5Stack Core2で180度計測する2D Lidarっぽいものを作る

公式のサンプルはUIFlowだったので、Arduino互換のC++に書き直した。

だいぶ雑だけど、雰囲気はでている。が、実用は難しいかな。

#include <M5Core2.h>
#include <VL53L0X.h>

#define SV_FREQ 50
#define MIN_SV_PULSE 0.5
#define MAX_SV_PULSE 2.4
#define NUM_SERVO 4
#define STEP_UNIT 1

#define TOF_NA_VAL 0x1FFE

VL53L0X tof;

const int servoPins[NUM_SERVO] = {26};
int adjust[NUM_SERVO] = {-7};
uint16_t point_map[180];

int getPulseWidth(int angle) {
  float pulseMs = MIN_SV_PULSE + (MAX_SV_PULSE - MIN_SV_PULSE) * angle / 180;
  return (int) (65536 * (pulseMs * SV_FREQ / 1000.0));
}

void vibration() {
  M5.Axp.SetLDOEnable(3, true);
  delay(100);
  M5.Axp.SetLDOEnable(3, false);
}

void setup() {
  // put your setup code here, to run once:
  M5.begin();

  int i, angle;
  
  angle = 90;

  for (i = 0; i < NUM_SERVO; ++i) {
    ledcSetup(i, SV_FREQ, 16);
    ledcAttachPin(servoPins[i], i);
  }

  for (i = 0; i < NUM_SERVO; ++i) {
    ledcWrite(i, getPulseWidth(angle + adjust[i]));
  }

  Serial.begin(9600);
  Wire.begin();

  tof.setTimeout(500);
  if (!tof.init())
  {
    Serial.println("Failed to detect and initialize sensor!");
    while (1) {}
  }
  for (int i = 0; i < 180; ++i) {
    point_map[i] = TOF_NA_VAL;
  }

  // Start continuous back-to-back mode (take readings as
  // fast as possible).  To use continuous timed mode
  // instead, provide a desired inter-measurement period in
  // ms (e.g. sensor.startContinuous(100)).
  tof.startContinuous();
}

void updateLcd() {
  int i;

  M5.Lcd.clearDisplay();

  int x0 = 320 / 2;
  int y0 = 240;

  double scale = 240.0 / 2000.0;

  for (int d = 500; d < 3000; d += 500) {
    double l = d * scale;
    for (i = 0; i < 180; ++i) {
      double rad = i * PI / 180.0;
      int32_t x = x0 + l * cos(rad);
      int32_t y = y0 - l * sin(rad);
      M5.Lcd.drawPixel(x, y, GREEN);
    }
  }

  uint16_t to_center = 20; // [mm]

  for (i = 0; i < 180; ++i) {
    uint16_t d = point_map[i];
    if (d == TOF_NA_VAL) continue;
    d += to_center;
    double l = scale * d;

    double rad = i * PI / 180.0;
    int32_t x = x0 + l * cos(rad);
    int32_t y = y0 - l * sin(rad);
    M5.Lcd.drawPixel(x, y, WHITE);
  }
}

void loop() {
  // put your main code here, to run repeatedly:
  static int count = 0;
  static int angle = 90;
  static int step = STEP_UNIT;
  static bool mode = false;
  int i;

  M5.update();

  if (M5.BtnC.isPressed()) {
    mode = !mode;
    vibration();
    delay(500);

    if (angle < 10) {
      step = STEP_UNIT;
    } else {
      step = -STEP_UNIT;
    }
  }
  if (!mode) {
    if (M5.BtnA.isPressed()) {
      adjust[0] += 1;
      vibration();
    }
    if (M5.BtnB.isPressed()) {
      adjust[0] -= 1;
      vibration();
    }
    angle = 90;
    step = 0;
    delay(500);
  }

  uint16_t distance = tof.readRangeContinuousMillimeters();
  if (tof.timeoutOccurred() || distance > TOF_NA_VAL) {
    distance = TOF_NA_VAL;
  }
  point_map[angle] = distance;

  bool update = false;
  if (++count > 15) {
    count = 0;
    update = true;
  }

  if (update) {
    updateLcd();
  }

  M5.Lcd.setCursor(0, 0);
  M5.Lcd.printf("Angle %3d%+2d\n", angle, adjust[0]);
  M5.Lcd.printf("Step %+d\n", step);
  if (distance == TOF_NA_VAL) {
    M5.Lcd.print("Distance N/A\n");
  } else {
    M5.Lcd.printf("Distance %+.2fm\n", distance * 0.001);
  }

  if (mode) {
    angle += step;
    if (angle > 170) {
      step = -STEP_UNIT;
    } else if (angle < 10) {
      step = STEP_UNIT;
    }
  }

  for (i = 0; i < NUM_SERVO; ++i) {
    ledcWrite(i, getPulseWidth(angle + adjust[i]));
  }

  delay(1);
}

platform.iniはこんな感じ。

[env:m5stack-core2]
platform = espressif32
board = m5stack-core2
framework = arduino
lib_deps = 
    m5stack/M5Core2@^0.0.3
    pololu/VL53L0X@^1.3.0

サーボキットの動画で紹介されていたのと構成はほぼ同じで、サーボキットを使った。 別にSG90とかでできると思うけど、LEGO互換の固定ブラケットだとかフレームだとかはとても便利だった。

Core2は、AWS Edukit版なので、サーボはPORT Bに接続して、26ピンで制御している。 無印の場合は、GPIO直接つかえばいいかな。