IoT化したTAMIYAのロボットに追っかけ機能を追加した

夏なのでホラー企画として、IoT化したTAMIYAのロボットに追っかけ機能を追加してみました。レーザー測距センサモジュールで捕捉した相手にずずずんっと迫って来ます！

真ん中のがレーザー測距モジュールです。

でも、レーザーは肉眼で見ても平気な出力のものだし、20cmまで近づくと止まるようにしていて、10cm以下になると後ずさります！

とりあえずビデオを上げます！

距離センサー

今回使用した距離センサーはレーザー方式ですが、一般によく知られているのは超音波方式のものだと思います。ぼくも最初は超音波方式のUS-015を使ったのですが、超音波方式のセンサーは対象物の素材や角度による影響が大きく、汎用性に乏しいことがわかったため使うのを止めました。

レーザー方式のVL53L0XはUS-015の倍くらいの値段ですが、それでも1,000円程度なので、距離を測りたい対象物が決まったものでなかったり、角度が斜めになったりする場合はレーザー方式の距離センサーを選んだ方がいいと思います。

レーザー方式の弱点は肉眼で見ても安全な出力にすると、計れる距離が短くなることですね。VL53L0Xは2メートル程度しか測ることができません。US-015はVL53L0Xの半額ですが測れる距離は倍の4メートルなのでお得です。使用状況に応じて使い分けるといいと思います。

VL53L0XとUS-015

VL53L0Xの接続

VL53L0Xには付属のコネクタ付きケーブルが付いていて、このようなピン配置になっています。付属マニュアルによると5:XSHUTと6:GPIO1は繋がなくても動作すると書いてあったので、繋いでいません。I2CインターフェースのSDAとSCLはESP8266のデフォルトのGPIO4とGPIO5に繋いで、V+を3.3VにGNDはGNDに繋いでいます。

回路図

バッテリーはニッケル水素電池3本で3.6Vで動作させています。

距離センサーは秋月電子製のfritzing素材が無かったので、Adafruit製のもので代用しています。

BLYNK

実行画面

FOLLOW：追っかけ機能ON/OFFスイッチ

DISTANCE X10CM：追っかけ機能有効距離。X10なので10だと100cm。

JOYSTICK：マニュアル操作用のジョイスティック

FOLLOW Button設定画面

DISTANCE Slider設定画面

Joystick設定画面

スケッチ

もったいぶっていた訳ではありませんが、お待ちかねのスケッチです。

※2019/5/21 追っかけモードをOFFにしたときにモーターを止めるように修正

/*
 * Robot.ino (with follow function)
 * May 21, 2019
 * By Hiroyuki ITO
 * http://intellectualcuriosity.hatenablog.com/  
 * MIT Licensed.
*/
#define BLYNK_PRINT Serial

#include <ESP8266WiFi.h>
#include <BlynkSimpleEsp8266.h>
#include <Wire.h>
#include <Adafruit_VL53L0X.h>

// Set Blynk Auth Token and WiFi credentials.
const char *auth = "YourAuthToken";
const char *ssid = "YourNetworkName";
const char *pass = "YourPassword";

// DRV8835
// GPIO
#define PIN_AIN1 14  // D5 Right Motor Blue
#define PIN_AIN2 12  // D6 Right Motor Yellow
#define PIN_BIN1 13  // D7 Left  Motor Yellow
#define PIN_BIN2 15  // D8 Left  Motor Blue
// PWM
#define PWM_FREQ 1000 // PWM frequency: 1000Hz(1kHz)
#define PWM_RANGE 100 // PWM range: 100

// LED GPIO
#define PIN_EYER 0    // D3 Right LED
#define PIN_EYEL 16   // D0 Left  LED

// VL53LOX
Adafruit_VL53L0X lox = Adafruit_VL53L0X();

// Follow me mode
#define FARAWAY   0
#define FORWARD   1
#define REVERSE   2
#define STOP      3

// Global
int distance_mode = STOP;
int mr_pwm = 0;       // Right motor speed
int ml_pwm = 0;       // Left  motor speed
int follow;
int distance_limit;
BlynkTimer timer;
VL53L0X_RangingMeasurementData_t measure;

/*
 * ABS Function
 */
int setABS(int pwm_value) {
  pwn_value = abs(pwm_value);
  if (pwm_value> 100) {
    pwm_value= 100;
  }
  return pwm_value;
}

/*
 * Set Motor
 */
void setMotor() {
  // Right Motor
  if (mr_pwm > -1) {
    // CW Forward
    analogWrite(PIN_AIN1, 0);
    analogWrite(PIN_AIN2, setABS(mr_pwm));
  } else {
    // CCW Reverse
    analogWrite(PIN_AIN1, setABS(mr_pwm));
    analogWrite(PIN_AIN2, 0);
  }

  // Left Motor
  if (ml_pwm > -1) {
    // CW Forward
    analogWrite(PIN_BIN1, 0);
    analogWrite(PIN_BIN2, setABS(ml_pwm));
  } else {
    // CCW Reverse
    analogWrite(PIN_BIN1, setABS(ml_pwm));
    analogWrite(PIN_BIN2, 0);
  }
}

/*
 * Keep Distance
 */
void keepDistance() {
  if (!follow) {
    return;
  }
  
  lox.rangingTest(&measure, false); // pass in 'true' to get debug data printout!
  int distance = measure.RangeMilliMeter;
  BLYNK_LOG2("distance: ", distance);

  if (distance < 100) {             // Less than 10cm
    // Reverse
    distance_mode = REVERSE;
    mr_pwm = -100;
    ml_pwm = -100;
    digitalWrite(PIN_EYEL, HIGH);
    digitalWrite(PIN_EYER, HIGH);
    BLYNK_LOG("keepDistance: REVERSE");
  } else if (distance < 200) {     // 10cm - 20cm
    // Stop
    distance_mode = STOP;
    mr_pwm = 0;
    ml_pwm = 0;
    digitalWrite(PIN_EYEL, HIGH);
    digitalWrite(PIN_EYER, HIGH);
    BLYNK_LOG("keepDistance: STOP");
  } else if (distance < distance_limit) { // 20cm - Limit
    // Forward
    distance_mode = FORWARD;
    mr_pwm = 100;
    ml_pwm = 100;
    digitalWrite(PIN_EYEL, HIGH);
    digitalWrite(PIN_EYER, HIGH);
    BLYNK_LOG("keepDistance: FORWARD");
  } else {                          // Greater than 100cm
    // FarAway
    if (distance_mode != FARAWAY) {
      mr_pwm = 0;
      ml_pwm = 0;
    }
    distance_mode = FARAWAY;
    digitalWrite(PIN_EYEL, LOW);
    digitalWrite(PIN_EYER, LOW);
    BLYNK_LOG("keepDistance: FARAWAY");
  }
}

/*
 * Receive Data from Cloud at connected
 */
BLYNK_CONNECTED() {
  Blynk.syncAll();
}

/*
 * Follow Button
 */
BLYNK_WRITE(V2) {
  follow = param.asInt();
  if (!follow) {
    digitalWrite(PIN_EYEL, LOW);
    digitalWrite(PIN_EYER, LOW);
    mr_pwm = 0;       // Right motor stop
    ml_pwm = 0;       // Left  motor stop
  }
  BLYNK_LOG2("Follow Button:", follow);
}

/*
 * Distance Slider
 */
BLYNK_WRITE(V3) {
  distance_limit = param.asInt() * 100;   // mm
  BLYNK_LOG2("Distance Slider:", distance_limit);
}

/*
 * Joystick x axis, y axis
 */
BLYNK_WRITE(V1) {
  if (follow && (distance_mode != FARAWAY)) {
    return;
  }

  int x = param[0].asInt(); // Joystick x axis
  int y = param[1].asInt(); // Joystick y axis
  BLYNK_LOG2("Joystick x:", x);
  BLYNK_LOG2("Joystick y:", y);

  // Forward or Reverse ?
  if (y > -1 ) {
    mr_pwm = y - x; 
    ml_pwm = y + x;
  } else {
    mr_pwm = y + x; 
    ml_pwm = y - x;
  }
  setMotor();
}

void setup() {
  delay(10);
  Serial.begin(74880);

  // Initialize GPIO
  pinMode(PIN_AIN1, OUTPUT);
  pinMode(PIN_AIN2, OUTPUT);
  pinMode(PIN_BIN1, OUTPUT);
  pinMode(PIN_BIN2, OUTPUT);
  pinMode(PIN_EYER, OUTPUT);
  pinMode(PIN_EYEL, OUTPUT);

  // Initialize PWM
  analogWriteFreq(PWM_FREQ);
  analogWriteRange(PWM_RANGE);

  // Initialize VL53LOX
  lox.begin();

  // Connect WiFi and Blynk cloud server
  Blynk.begin(auth, ssid, pass);
  timer.setInterval(200L, keepDistance); // 200 msec
}

void loop() {
  Blynk.run();
  timer.run();
  setMotor();
}

追っかけボタンをオンにすると、新設のkeepDistance()ファンクションが有効になり、#defineで定義している4つの"Follow me mode"のモードに従って、動作を切り替えるようにしています。

コードはかなりシンプルで、全体で200行程度しかないので、興味が湧いたら実際に試してみることをお勧めします。

もっと大きなロボットで動作させる前に、プロトタイプの参考として使ってください！

ライブラリ

おっと、忘れていました。VL53L0Xを使うのにAdafruitのライブラリを使っています。ライブラリマネージャで追加しておいてください。

Blynkのライブラリも当然使っているので、Blynkを初めて使う人は一緒に追加してくださいね。

終わり