公式のサンプルは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直接つかえばいいかな。