Spresenseとと3D TOF距離センサ (MM-S50MV)を使って走るローラースケートロボ

タミヤのローラースケートロボのような動きをするロボットを作ってみました。
インラインスケートやアイススケートでいうところの”ひょうたん”、”スイベル”といった名前のついている動きで進む動きを特徴としています。
今回は上記の動きを再現するだけではなく、不整地でもほふく前進のように進めることを目的として先端に車輪がついたアーム型ロボットとしました。

私はもともとインラインスケートなどのスケート関係のスポーツが好きです。
その中で特に面白いと思うのが、モーターなどの動力源を持たないただのタイヤが、人間の足の動かし方次第で強力な推進力を生み出すと言うところです。
ぜひこの面白くて、不思議な現象を簡単に味わえるようなロボットを作りたいと思いました。

今回はSPRESENSEのマイコンボードと3D TOFセンサー(MM-S50MV)が利用できましたので、それらを使って構築を目指します。

SPRESENSEを基本として演算部を用意します。
SPRESENSEにはTOFセンサーを接続して、センサーによって、周囲の地形の検出と進むべき方向や動作モード(後述）の判定を行うこととします。
あとは自分の姿勢と床平面との関係を把握した上でアーム動作へのフィードバックすることを狙います。

2本のアームはそれぞれ2個のサーボモーターを動力源として動かします。
今回はほふく前進モードというのを設定しますので、回転角度が通常の180度ではなく270度と大きめのサーボを選定しました。
また、サーボモーター駆動用のマイコンピン数削減のため、別途でI2C通信によるサーボモータードライバボードを使用し、サーボモーターはそちらに接続するものとします。

アームの肩の位置に相当する部分には差動ギアを使ったメカを用います。
ギア、ホルダー部などは全てモデリング＆3Dプリントとしています。
ベアリングなどがいるかと思いましたがそこは意外となんとかなりました。
（記事下段動作中の動画を載せました）

アーム部の先端にはキャスター車輪がついています。
こちらの車輪は進行方向に対して左右に45度程度しか向きを変えないことから、アームを進行方向横方向に動かすことで、前方方向への推進力を発生させる構想です。

このロボットは2つの動作モードを持ちます。
1つはスイスイモード。
このモードでは、横方向に動かしたタイヤによって生み出される前方向の推進力を使って進みます。

もう一つはほふく前進モード。
このモードではタイヤは使わず、アームを使ってほふく前進のように進みます。

路面が平らなところでは、スイスイモードで省エネルギーに移動します。
路面に凹凸が検知されたときに、ほふく前進モードに切り替えて、タイヤを使わず、ジリジリと前に進むことになります。

懺悔
〜〜ここから〜〜

SPRESENSEとTOFセンサーを用いたロボットとする予定だったのですが、TOFセンサーの動作確認も終え、せっせとプログラム作成中、ある書き込みの際に非常に時間がかかり待ちきれずUSBを抜いてしまいました。
するとその後の書き込みも全て失敗するようになり・・・。
絶望に包まれました。

だが待てよ、まだ慌てる時間じゃない。大体こういう時は工場出荷に戻すための手立てがあるはずだ。

脳内に確かに聞こえた声に従い調べてみると以下のリンクを発見。
リカバリーツールなるもので工場出荷状態に戻せるとのこと。
https://developer.sony.com/ja/spresense/downloads/recovery-tool

そちらに従い復帰を試みるも・・・
こちらもPC画面上ではステータスの進行は確認できるものの、最終の状態には辿り着けず。。。

せっかくモニター提供いただいたにも関わらず壊してしまったようです。
大変申し訳ありませんでした。

そのためSPRESENSEでのトライはここまでとなってしまいました。。。
今回制作物の供養の意味も込めて、以降は別マイコンボード板での説明とさせていただきたいと思います。

〜〜ここまで〜〜

メカ部分、サーボ動作などは順調に作成（失敗）を進めていき、工具箱1箱分ほどのボツ部品を作ったところで大体機能は実現できました。

動作中の様子はこちら

M5AtomLITES3を使った例
#include <M5Unified.h>
#include <FastLED.h>

//サーボドライバ
#include <Wire.h>
#include <Adafruit_PWMServoDriver.h>

// called this way, it uses the default address 0x40
Adafruit_PWMServoDriver pwm = Adafruit_PWMServoDriver();
// you can also call it with a different address you want
//Adafruit_PWMServoDriver pwm = Adafruit_PWMServoDriver(0x41);
// you can also call it with a different address and I2C interface
//Adafruit_PWMServoDriver pwm = Adafruit_PWMServoDriver(0x40, Wire);

// Depending on your servo make, the pulse width min and max may vary, you
// want these to be as small/large as possible without hitting the hard stop
// for max range. You'll have to tweak them as necessary to match the servos you
// have!
#define SERVOMIN 150   // This is the 'minimum' pulse length count (out of 4096)
#define SERVOMAX 600   // This is the 'maximum' pulse length count (out of 4096)
#define USMIN 350      // This is the rounded 'minimum' microsecond length based on the minimum pulse of 150
#define USMAX 2450     // This is the rounded 'maximum' microsecond length based on the maximum pulse of 600
#define SERVO_FREQ 50  // Analog servos run at ~50 Hz updates

// our servo # counter
uint8_t servonum = 0;


//Wii ヌンチャク

#include <WiiChuck.h>
Accessory nunchuck1;


#define NUM_LEDS 1
#define LED_DATA_PIN 35

CRGB leds[NUM_LEDS];



int arm1 = 0;
int arm2 = 0;
unsigned long prevTime = 0;    //直前で実施したループで取得した経過時間
unsigned long Loop_time = 50;  // [ms]
int recvCmd;
int key = 1;
bool datasent = false;  // データ送信可否フラグ true: 送信する   false : 送信しない




void setup() {

  auto cfg = M5.config();
  cfg.serial_baudrate = 9600;
  M5.begin(cfg);
  FastLED.addLeds<WS2811, LED_DATA_PIN, GRB>(leds, NUM_LEDS);
  FastLED.setBrightness(20);


  //サーボドライバ
  pwm.begin();
  pwm.setOscillatorFrequency(27000000);
  pwm.setPWMFreq(SERVO_FREQ);  // Analog servos run at ~50 Hz updates

  delay(10);


  //アナログ入力インプット設定
  pinMode(5, INPUT);
  pinMode(6, INPUT);
  pinMode(7, OUTPUT);
  pinMode(8, OUTPUT);

  Serial.print("arm1");  //データAの凡例を送信
  Serial.print(",");
  Serial.println("arm2");  //データBの凡例を送信


  //Wii ヌンチャク設定

  digitalWrite(7, HIGH);
  delay(1);
  digitalWrite(8, LOW);
  delay(5);

  nunchuck1.begin();
  if (nunchuck1.type == Unknown) {
    /** If the device isn't auto-detected, set the type explicatly
		 * 	NUNCHUCK,
		 WIICLASSIC,
		 GuitarHeroController,
		 GuitarHeroWorldTourDrums,
		 DrumController,
		 DrawsomeTablet,
		 Turntable
		 */
    nunchuck1.type = NUNCHUCK;
  }


  digitalWrite(7, LOW);
  delay(1);
  digitalWrite(8, HIGH);
  delay(5);

  nunchuck1.begin();
  if (nunchuck1.type == Unknown) {
uck1.type = NUNCHUCK;
  }
}

int joy_R_x = 0;
int joy_R_y = 0;

int joy_L_x = 0;
int joy_L_y = 0;

void loop() {

  //ヌンチャクデータ読み込み


  digitalWrite(7, HIGH);
  digitalWrite(8, LOW);

  nunchuck1.readData();  // Read inputs and update maps

  // //読み込み結果のシリアル出力
  // Serial.print("Joy_X = ");
  // Serial.print(nunchuck1.values[0]);
  // Serial.print(" , ");

  // Serial.print("Joy_Y = ");
  // Serial.print(nunchuck1.values[1]);
  // Serial.print(" , ");

  // Serial.print("Joy_C_button = ");
  // Serial.print(nunchuck1.values[11]);
  // Serial.print(" , ");

  // Serial.print("Joy_Z_button = ");
  // Serial.println(nunchuck1.values[10]);


  joy_R_y = -map(nunchuck1.values[0], 33, 229, -100, 100);
  joy_R_x = -map(nunchuck1.values[1], 36, 224, -100, 100);



  digitalWrite(7, LOW);
  digitalWrite(8, HIGH);


  nunchuck1.readData();  // Read inputs and update maps


  joy_L_y = -map(nunchuck1.values[0], 33, 229, -100, 100);
  joy_L_x = map(nunchuck1.values[1], 36, 224, -100, 100);




  Serial.print("joy_R_x = ");
  Serial.print(joy_R_x);
  Serial.print(" , ");
  Serial.print("joy_R_y = ");
  Serial.print(joy_R_y);

  Serial.print(" /// ");

  Serial.print("joy_L_x = ");
  Serial.print(joy_L_x);
  Serial.print(" , ");
  Serial.print("joy_L_y = ");
  Serial.println(joy_L_y);



  pwm.writeMicroseconds(0, map(-joy_R_x + joy_R_y, -150, 150, USMIN, USMAX));
  pwm.writeMicroseconds(1, map(joy_R_x + joy_R_y, -150, 150, USMIN, USMAX));

 pwm.writeMicroseconds(2, map(-joy_L_x + joy_L_y, -150, 150, USMIN, USMAX));
  pwm.writeMicroseconds(3, map(joy_L_x + joy_L_y, -150, 150, USMIN, USMAX));



  servonum = 0;


  if ((millis() - prevTime) > Loop_time) {



    if (Serial.available()) {  // 受信データがあるか？
      key = Serial.read();     // 1文字だけ読み込む
      // Serial.print("millis = ");
      // Serial.print(millis());
      // Serial.print(" , ");
      // Serial.print("key = ");
      // Serial.println(key);  // 1文字送信。受信データをそのまま送り返す。
    }

    switch (key) {
      default:
        datasent = false;
        break;

      case '0':

        if (datasent == false) {
          Serial.println("start");
        }
        datasent = true;
        break;

      case '1':
        if (datasent == true) {
          Serial.println("stop");
        }
        datasent = false;
        break;
    }


    if (datasent == true) {
      arm1 = analogRead(5);
      arm2 = analogRead(6);

      //Serial.print("arm1 = ");
      Serial.print(arm1);
      Serial.print(",");

      //Serial.print("arm2 = ");
      Serial.println(arm2);
    }

    prevTime = millis();
  }
}