ルンバをラジコンのプロポから操作する

はじめに

ルンバは Roomba Open Interface(以下ROI )というインターフェースがあり、それを使うとルンバを自由に制御できることは割と有名だと思います。

制御できるなら、ラジコンのプロポから操作してみたい！

同じ考えの人絶対いるよな！
と思ってググったものの、サンプルプログラムが引っかかりませんorz
車輪の再発明のような気もしますが、ググり疲れたので自分で作りました。というお話です。

前提条件

• Arduino IDEを使ったことがある
• ボードマネージャやライブラリマネージャで足りないものをインストールできる
• 簡単な電子工作ができる
• ROIのあるルンバと、ラジコン用プロポセットがある

ハードウェア

• ラジコン受信機からの出力であるPWM信号をそこそこの精度で読み出せること
• ROIに接続できるシリアルインターフェイスを持っていること
• 小さい
• 安い

といったあたりから Arduino互換機の Pro Micro 5V/16MHz を選択しました。

Pro Microは自作キーボード界隈ではポピュラーなマイコンなので、自作キーボード屋さんから入手しやすいです。私はTALP KEYBOARDさんから購入しました。

配線

この記事はソフトウェアメインなので、配線は文字だけで簡単に・・・。

電源はモバイルバッテリーとかでもいいですし、ルンバからバッテリー電源を引っ張ってきて、78L05(三端子レギュレータ)などで5Vを作ってもいいと思います。

ルンバとの信号線は Pro Microの TxD と ルンバの RxD をつなぐだけです。
センサの読み取りなどしたい場合は逆方向も配線しとくといいでしょう。（今回のプログラムでは使っていません。）

受信機との信号線は、ソフトウェア上で定義したピンと、受信機のそれぞれのChの信号線をつなぎましょう。

ソフトウェア

ポイントだけ。

• PWMの読み取りはPinChangeInterruptで拾ってソフトウェアでパルス幅を計っている
• ch3に、掃除モーターのON/OFFを割り当てている
• プロポからのステアリング値とルンバの旋回半径を線形にマッピングすると操作感が悪いので、謎の計算をさせている
• 停止状態からステアリングをひねると超信地旋回する
• LEDで遊んでいる

詳しくは、ソースのコメントを読んでください・・・ m(_ _)m

roomba_rc.ino

# include <util/atomic.h> // this library includes the ATOMIC_BLOCK macro.
# include <PinChangeInterrupt.h>
// ↑ここでエラーが出たらライブラリをライブラリマネージャから取り込んでください

// 受信機入力で使用するピンの定義（添え字がchで、中身がピン）
const byte pwm_input_pin_ary[] = {8,9,10};
// ボード毎に使えるピンが違います。詳細は↓参照
// https://github.com/NicoHood/PinChangeInterrupt/#pinchangeinterrupt-table

// メイン処理の間隔[ms]
# define DELAY_MS 50

// PWMパルス幅想定値の定義。
// フタバ受信機で一般的な値です(1520±450)
// 1500±500ぐらいでもいいかも？
# define CH1_MIN_PWM  1120
# define CH1_NEU_PWM  1520
# define CH1_MAX_PWM  1920

# define CH2_MIN_PWM  1120
# define CH2_NEU_PWM  1520
# define CH2_MAX_PWM  1920

// ch3 は3ポジションのスイッチなので、その閾値を定義
# define CH3_LOW_THRESHOLD  1250
# define CH3_HIGH_THRESHOLD 1750

# define DEAD_BAND 30  // ニュートラルだと認識する幅。

# define VELOCITY_MAX 500  // ルンバの走行速度最大値[mm/s] （ねぇ、秒速50センチなんだって）

# define RADIUS_MIN 100  // ルンバの旋回半径最小値
# define RADIUS_MAX 2000 // ルンバの旋回半径最大値
//MAXはいじらない方がいい。MINはお好みに応じてドウゾ

# define FIXED_POINT_COEFFICIENT 10000  // 固定小数点演算用の倍率

// 旋回計算用の値。setup()で初期化します
int st_min_rad; //90度弱
int st_max_rad; //45度

// PWMパルス幅計測用の、立ち上がりエッジ時刻
volatile unsigned long rising_time[] = {0, 0, 0, 0};

// PWMパルス幅計測結果をいれるところ。単位[μs]
volatile long pwm_width_us[] = {0, 0, 0, 0};

// 超信地旋回するしないの判定用フラグ
int prev_state=0; // 0:止まってた 1:動いてた

// ウィリー防止
int prev_th_val=0;

// ch3状態
int ch3_state=-1; // -1:none 0:サイドブラシ動作 1:加えて、メインブラシ・バキューム動作
// ↑の前回の値（意味のない同一コマンド送信を抑制するため）
int prev_ch3_state=-1;

// Lチカ更新間隔計算用
int led_refresh_count=0;

// 週表示LEDの何番目を表示するか
int week_led=0;

void setup() {
  Serial.begin(115200);   // デバッグプリント用
  Serial1.begin(115200);  // Roomba OI用

  pinMode(pwm_input_pin_ary[0], INPUT);  // steering
  pinMode(pwm_input_pin_ary[1], INPUT);  // throttle
  pinMode(pwm_input_pin_ary[2], INPUT);  // 3ch:サイドブラシ・メインブラシ・バキューム モーター制御用
  //pinMode(pwm_input_pin_ary[3], INPUT);  

  attachPinChangeInterrupt(digitalPinToPinChangeInterrupt(pwm_input_pin_ary[0]), onPinChange0, CHANGE);
  attachPinChangeInterrupt(digitalPinToPinChangeInterrupt(pwm_input_pin_ary[1]), onPinChange1, CHANGE);
  attachPinChangeInterrupt(digitalPinToPinChangeInterrupt(pwm_input_pin_ary[2]), onPinChange2, CHANGE);
  //attachPinChangeInterrupt(digitalPinToPinChangeInterrupt(pwm_input_pin_ary[3]), onPinChange3, CHANGE);

  // 旋回計算用の値を初期化
  st_min_rad = atan(RADIUS_MAX/RADIUS_MIN)*FIXED_POINT_COEFFICIENT;
  st_max_rad = PI/4*FIXED_POINT_COEFFICIENT;

  // コネクタを差すときに電源よりシリアルが後の場合があるので、ディレイ
  delay(500);
  
  Serial1.write(128);  // start
  Serial1.write(131);  // safe mode
  delay(100);  // 待たないとたまに無反応になる
  Serial1.write(164);  // Digit LEDs ASCII
  Serial1.print("4649");  // ヨロシク！
  delay(100);  // 待たないとたまに無反応になる
}

// ピンチェンジ割り込み実装
void onPinChangeImpl(byte ch)
{
  unsigned long now = micros(); // できるだけ早めに現在時刻を拾っておく
  
  uint8_t trigger = getPinChangeInterruptTrigger(digitalPinToPCINT(pwm_input_pin_ary[ch]));
  if(trigger == RISING) {
    // 立ち上がりエッジ→立ち上がり時刻記録
    rising_time[ch] = now;
  } else if(trigger == FALLING) {
    // 立ち下がりエッジ→パルス幅記録
    int us = now - rising_time[ch];
    if ( us<500 || 2500<us ) {
      // 異常値だったら未受信ということにする
      us = 0;
      Serial.println("                    異常値"); 
    }
    pwm_width_us[ch] = us;
  }
}

// ピンチェンジ割り込み入口
void onPinChange0(void) {onPinChangeImpl(0);}
void onPinChange1(void) {onPinChangeImpl(1);}
void onPinChange2(void) {onPinChangeImpl(2);}
void onPinChange3(void) {onPinChangeImpl(3);}

void loop()
{
  chk_no_signal();

  //デバッグプリント
  Serial.print(pwm_width_us[0]); 
  Serial.print(","); 
  Serial.print(pwm_width_us[1]); 
  Serial.print(","); 
  Serial.print(pwm_width_us[2]); 

  int th_val = roomba_drive();
  roomba_motors();
  roomba_led(th_val);

  Serial.println(); 
  delay(DELAY_MS);
}

// ノーコン対策
void chk_no_signal()
{
  unsigned long now_micros = micros();
  for(int i=0; i<sizeof(pwm_input_pin_ary); i++) {
    unsigned long rt;
    ATOMIC_BLOCK(ATOMIC_RESTORESTATE) {
      rt = rising_time[i];
    }
    long diff = now_micros - rt;
    // ↑signedへの暗黙キャストは、micros()呼出し後に立ち上がり割り込みが発生した場合の対策
    // （マイナスになってくれて都合がいい）
    if ( 50*1000L < diff ) {
      // パルスが一定時間来ない場合は未受信ということにする
      ATOMIC_BLOCK(ATOMIC_RESTORESTATE) {
        pwm_width_us[i] = 0;
      }
    }
  }
}

// ステアリング・スロットル処理
int roomba_drive()
{
  // ルンバに送る値
  int st_val; // -2000～2000(旋回) or 0x8000(直進)
  int th_val; // -500～500[mm/s]

  //// スロットルの処理
  int th_us;
  ATOMIC_BLOCK(ATOMIC_RESTORESTATE) {
    th_us = pwm_width_us[1];
  }
  if ( th_us == 0 ) {
    // 未受信
    th_val = 0; // 停止
  }
  else if ( th_us < CH2_NEU_PWM-DEAD_BAND ) {
    // 後退
    th_val = map(th_us, CH1_NEU_PWM-DEAD_BAND, CH2_MIN_PWM, -1, -VELOCITY_MAX);
    th_val = max(th_val, -VELOCITY_MAX);
  }
  else if ( CH2_NEU_PWM+DEAD_BAND < th_us ) {
    // 前進
    th_val = map(th_us, CH1_NEU_PWM+DEAD_BAND, CH2_MAX_PWM, 1, VELOCITY_MAX);
    th_val = min(th_val, VELOCITY_MAX);
  }
  else {
    // ニュートラル
    th_val = 0; // 停止
  }

  // 急加速はさせない
  if ( DELAY_MS*2 < th_val-prev_th_val ) {
    th_val = prev_th_val+DELAY_MS*2;
  }
  else if ( th_val-prev_th_val < -DELAY_MS*2 ) {
    th_val = prev_th_val-DELAY_MS*2;
  }
  prev_th_val = th_val;
  
  //// ステアリングの処理
  int st_us;
  ATOMIC_BLOCK(ATOMIC_RESTORESTATE) {
    st_us = pwm_width_us[0];
  }
  if ( th_val!=0 ) {
    // スロットルON時
    prev_state=1;
    if ( st_us == 0 ) {
      // ここには来ないはずだけど念のため
      st_val = 0x8000; // 直進
    }
    else if ( st_us < CH1_NEU_PWM-DEAD_BAND ) {
      // 左旋回
      // いい感じのステアリング操作感にするための謎の計算
      float rad = map(st_us, CH1_NEU_PWM-DEAD_BAND, CH1_MIN_PWM, st_min_rad, st_max_rad);
      st_val = tan(rad/FIXED_POINT_COEFFICIENT) * RADIUS_MIN;
      st_val = max(st_val,RADIUS_MIN);
    }
    else if ( CH1_NEU_PWM+DEAD_BAND < st_us ) {
      // 右旋回
      // いい感じのステアリング操作感にするための謎の計算
      float rad = map(st_us, CH1_NEU_PWM+DEAD_BAND, CH1_MAX_PWM, st_min_rad, st_max_rad);
      st_val = tan(rad/FIXED_POINT_COEFFICIENT) * -RADIUS_MIN;
      st_val = min(st_val,-RADIUS_MIN);
    }
    else {
      // ニュートラル
      st_val = 0x8000; // 直進
    }
  }
  else {
    // スロットルOFF時は超信地旋回（その場で回転）
    //  st_valを1か-1 にして、回転速度を th_val に入れます。
    if ( st_us == 0 ) {
      // 未受信
      st_val = 0x8000; // 直進
      prev_state=0;
    }
    else if ( st_us < CH1_NEU_PWM-DEAD_BAND ) {
      // 左旋回
      if ( prev_state==0 ) {
        st_val = 1;
        th_val = map(st_us, CH1_NEU_PWM-DEAD_BAND, CH1_MIN_PWM, 1, VELOCITY_MAX);
        th_val = min(th_val, VELOCITY_MAX);
      }
    }
    else if ( CH1_NEU_PWM+DEAD_BAND < st_us ) {
      // 右旋回
      if ( prev_state==0 ) {
        st_val = -1;
        th_val = map(st_us, CH1_NEU_PWM+DEAD_BAND, CH1_MAX_PWM, 1, VELOCITY_MAX);
        th_val = min(th_val, VELOCITY_MAX);
      }
    }
    else {
      // ニュートラル
      st_val = 0x8000; // 直進
      prev_state=0;
    }
  }
  
  //ルンバ動作指示
  Serial1.write(137);  // drive
  Serial1.write((th_val>>8)&0xff);
  Serial1.write(th_val&0xff);
  Serial1.write((st_val>>8)&0xff);
  Serial1.write(st_val&0xff);

  //デバッグプリント
  Serial.print(", "); 
  Serial.print(st_val); 
  Serial.print(","); 
  Serial.print(th_val);

  return th_val;
}

// CH3:掃除モーター動作指示
void roomba_motors()
{
  int motors_val;
  int ch3_raw;
  ATOMIC_BLOCK(ATOMIC_RESTORESTATE) {
    ch3_raw = pwm_width_us[2];
  }
  if ( ch3_raw == 0 ) {
    // 未受信→OFF
    ch3_state=-1;
    motors_val = 0x00;
  }
  else if ( ch3_raw < CH3_LOW_THRESHOLD ) {
    // low側→全ON
    ch3_state=1;
    motors_val = 0x07;
  }
  else if ( CH3_HIGH_THRESHOLD < ch3_raw ) {
    // high側→OFF
    ch3_state=-1;
    motors_val = 0x00;
  }
  else {
    // ニュートラル→一部ON
    ch3_state=0;
    motors_val = 0x01;
  }
  
  if ( ch3_state!=prev_ch3_state ) {
    // 状態が変わったときだけモーター作動コマンド送信
    prev_ch3_state = ch3_state;

    Serial1.write(138);  // Motors
    Serial1.write(motors_val);

    //デバッグプリント
    Serial.print(", "); 
    Serial.print(motors_val); 
  }
}

//Lチカのようなもの（速度に応じた更新間隔にしてます）
void roomba_led(int th_val)
{
  bool b_refresh = false; // 今回の更新をする/しないフラグ
  led_refresh_count+=th_val;
  if ( VELOCITY_MAX<=led_refresh_count ) {
    led_refresh_count-=VELOCITY_MAX;
    week_led++; //点灯する週LEDを右に移動
    if ( 7<=week_led ) week_led=0;
    b_refresh=true;
  }
  else if ( led_refresh_count<0 ) {
    // 後退時はLEDの流れも逆です
    led_refresh_count+=VELOCITY_MAX;
    week_led--; //点灯する週LEDを左に移動
    if ( week_led<0 ) week_led=6;
    b_refresh=true;
  }
 
  if ( b_refresh ) {
    // weekの表示(流れるLED)
    int weed_bit = 0x1<<week_led;
    Serial1.write(162);  // Scheduling LEDS
    Serial1.write(weed_bit);
    Serial1.write(0);

    // 7セグの表示(ランダム16進数)
    Serial1.write(164);  // Digit LEDs ASCII
    for(int i=0;i<4;i++){
      int c = random(0, 15);
      if ( c<10 ) {
        c += '0';
      }
      else {
        c += 'A'-10;
      }
      Serial1.write(c);
    }
  }
}

発展

前にラジコン受信機の信号読み取りでPWMを使わずS.BUSプロトコルを読む話を書きましたが、なぜ今回はPWMを使うのか。

それは、Donkey Car に応用できるからです！

今回作った回路があれば、Donkey Car を無改造で乗せることができるんじゃないかと思います。（Donkey Carの回路を載せる台を自作する必要があるとは思いますが、電子回路・ソフトウェアは無改造で済むでしょう）

Donkey Carを楽しむにあたっての障壁は、AIがどうのこうのよりも、「速すぎて教師走行が激ムズ」「速すぎて日本の住宅では場所が取れない」といったところで悩まされがちです。
ルンバなら遅いし、そもそもが住宅内で走行させるものなので、利用しやすいのではないでしょうか。
お掃除目的でルンバを所有していれば、Donkey Car向けラジコン本体が実質無料ということになります！ｗ
Donkey Carでの利用であれば、ラジコンのプロポセットは不要です。