raspi、そしてArduinoを使ってラジコンを制御する方法について説明します。

デバイス・環境一覧

Raspberry Pi4 Model B
- Ubuntu Server 18.04
- ROS melodic
Arduino Uno
ラジコン
- 「1/10RC ワイルドウイリー 2」(本体)
- 「ファインスペック2.4G 電動RCドライブセット」(サーボモータ、プロポ等)

構成

TEU-105KBK、TRU-08の回路の構成は「タミヤESCをpwm制御」にまとめてあり、とても参考になりました。

今回はモーターだけではなく、電源供給のためTEU-105KB（スピードコントローラ）とTRU-08（受信機）も使います。そのためTRU-08からTEU-105KBとステアサーボの端子を引き抜き、電源の線だけをジャンパワイヤでTRU-08に接続します。
端子には3つの線があり、赤と黒が電源になります。白の線が制御用の線で、これはArduinoのPWMが出力可能なピンに接続します。自分はステアの制御を9、スピードの制御を10としました。

赤 : +
黒 : GND
白 : PWM (モータ制御)

なお、Arduino、ステアサーボ、TEU-105BKのGNDは接続します。自分は余ったジャンパワイヤをハンダ付けしてつなげました。

PWM周波数、パルス幅

モータはPWMで制御します。PWM制御には周期とパルス幅が必要です。そのため、ステアサーボとTEU-105BKの周期とパルス幅をオシロスコープを使って調べました。

種類	周期	パルスデフォルト値	最小パルス	最大パルス
ステアサーボ	17.25 [ms]	1.5 [ms] 直進	1.1 [ms] 左	1.9 [ms] 右
ESC	17.25 [ms]	1.5 [ms] 停止	1.1 [ms] 前進	1.9 [ms] 後退

他にも同じように値を調べた方がいましたが、周期についてはどちらとも厳密には一致しませんでした。ただ、調べてみたところ、パルス幅は重要ですが周期は多少異なっていても問題ないようです。よって、今回は周期は20msとしました。
https://qiita.com/yas-nyan/items/8e358d919c663848b4e6
https://qiita.com/shigeru-yokochi/items/1e1519401c909c705117

実装

ソフトウェア構成

コントローラ入力の受信パッケージ

joyパッケージを使います。

$ sudo apt install ros-melodic-joy
$ sudo apt install ros-melodic-joystick-drivers

今回コントローラにはNintendo Switchのプロコンを使用しました。プロコンを使用する方法こちらにまとめました。

モータ指示値出力パッケージ

'joy'パッケージの出力を直接Arduino上で動作するモータ制御パッケージに送ってもいいのですが、複数のコントローラに対応したり、使用するボタンを変えた場合などに変更が容易なよう変換用のパッケージを実装します。

メッセージの実装

モータ指示値を送信するためのメッセージを実装します。以下のメッセージをこちらで実装したcontrol_msgsに追加しました。

RcCarControl.msg

uint32 count
float32 steer
float32 speed

パッケージの実装

以下の実装ではプロコンだけではなく、PlayStation3のコントローラにも対応しています。
左のジョイスティックの左右の操作でステアを、右のジョイスティックの上下の操作で前進後退をコントロールするように実装しました。
ソースコードはここで公開しています。

#include "ros/ros.h"
#include "sensor_msgs/Joy.h"
#include "std_msgs/Header.h"
#include "car_control_msgs/RcCarControl.h"
#include "controller_type.h"

ros::Publisher publisher;
ControllerType type = ControllerType::ProCon;
std::string controller_type;

void publishCarControl(const car_control_msgs::RcCarControl &msg) {
    publisher.publish(msg);
}

void joyCallback(const sensor_msgs::Joy& msg) {
  car_control_msgs::RcCarControl control;
  switch (type) {
  case ControllerType::ProCon:
    control.steer = msg.axes[0];
    control.speed = msg.axes[3];
    break;
  case ControllerType::PS3:
    control.steer = msg.axes[0];
    control.speed = msg.axes[4];
    break;
  default:
    control.steer = msg.axes[0];
    control.speed = msg.axes[3];
    break;
  }
  ROS_DEBUG("axis 0 = [%.3f], 4 = [%.3f]", control.steer, control.speed);
  publishCarControl(control);
}

int main(int argc, char **argv) {
  ros::init(argc, argv, "car_controller");
  ros::NodeHandle n;
  ros::NodeHandle pn("~");
  std::string type_str;
  pn.getParam("controller_type", type_str);
  if (type_str == "ProCon") {
    type = ControllerType::ProCon;
  } else if (type_str == "PS3") {
    type = ControllerType::PS3;
  } else {
    type = ControllerType::ProCon;
  }

  ROS_DEBUG("Controller Type: %s", type_str.c_str());
  publisher = n.advertise<car_control_msgs::RcCarControl>("car_control", 100);
  ros::Subscriber subscriber = n.subscribe("joy", 50, joyCallback);
  ros::spin();
  return 0;
}

モータ制御パッケージ

ステアとスピードの制御について

ステアとスピードの制御には、Arduinoの***Servo**ライブラリを使用しました。このライブラリはwriteMicroseconds()*という関数があり、パルス幅をマイクロ秒単位で指定できます。パルス幅はすでにわかっているので、こちらを使い方が便利です。

ただし、このライブラリで出力されるPWMの周期は20[ms]固定です。ステアサーボとTEU-105BKはこの周期で動作したため問題ありませんでしたが、周期を変える必要がある場合はServoライブラリを使用せずに直接PWMを出力する必要があります。

PWMの出力方法はネットで検索すればすぐにわかりますが、*analogWrite()*という関数を使います。指定するのはパルス幅[μs]ではなくduty比なので、パルス幅と周期から計算する必要があります。
また、Arduinoが出力するPWMの周期は、デフォルトだと約490Hzです。周期の変え方は、以下が参考になります。

https://qiita.com/hotchpotch/items/382cb6a66fafbb819767
http://garretlab.web.fc2.com/arduino/inside/hardware/arduino/avr/cores/arduino/wiring_analog.c/analogWrite.html
http://blog.kts.jp.net/arduino-pwm-change-freq/
https://playground.arduino.cc/Main/TimerPWMCheatsheet/

モータ制御パッケージの実装

ここで実装したパッケージに、モータ制御用の処理を実装しました。
ソースコードはこちらです。

#include <ros.h>
#include <Arduino.h>
#include <Servo.h>
#include "car_control_msgs/CameraControl.h"
#include "car_control_msgs/RcCarControl.h"


const int CAMERA_PIN = 11;
const int STEER_PIN = 9;
const int SPEED_PIN = 10;

const int CAMERA_OFFSET = 90; // [degree]
const int STEER_LEFT = 1100; // [micro second]
const int STEER_CENTER = 1500; // [micro second]
const int STEER_RIGHT = 1900; // [micro second]
const int STEER_BACK = 1100; // [micro second]
const int STEER_STOP = 1500; // [micro second]
const int STEER_FRONT = 1900; // [micro second]

ros::NodeHandle nh;
Servo servo_camera;
Servo servo_steer;
Servo servo_speed;


void cameraRpyCallback(const car_control_msgs::CameraControl &msg) {
  servo_camera.write(msg.roll + CAMERA_OFFSET);
}

int calcSteerPulse(const float steer) {
  int pulse;
  if (steer <= -0.1) {
    pulse = (int)((4.0 / 9.0 * steer + 139.0 / 90.0)*1000.0);
    pulse = max(pulse, STEER_LEFT);
  } else if (steer >= 0.1) {
    pulse = (int)((4.0 / 9.0 * steer + 131.0 / 90.0)*1000.0);
    pulse = min(pulse, STEER_RIGHT);
  } else {
    pulse = STEER_CENTER;
  }
  return pulse;
}

int calcSpeedPulse(const float speed) {
  return calcSteerPulse(-1.0 * speed);
}

void carControlCallback(const car_control_msgs::RcCarControl &msg) {
  int steer_pulse = calcSteerPulse(msg.steer);
  int speed_pulse = calcSpeedPulse(msg.speed);
  servo_steer.writeMicroseconds(steer_pulse);
  servo_speed.writeMicroseconds(speed_pulse);
}

ros::Subscriber<car_control_msgs::CameraControl> sub_camera("/camera_rpy", &cameraRpyCallback);
ros::Subscriber<car_control_msgs::RcCarControl> sub_car_control("/car_control", &carControlCallback);

void setup() {
  servo_camera.attach(CAMERA_PIN);
  servo_camera.write(CAMERA_OFFSET);
  servo_steer.attach(STEER_PIN);
  servo_steer.writeMicroseconds(STEER_CENTER);
  servo_speed.attach(SPEED_PIN);
  servo_speed.writeMicroseconds(STEER_STOP);

  nh.initNode();
  nh.subscribe(sub_camera);
  nh.subscribe(sub_camera);
  nh.subscribe(sub_car_control);
}

void loop() {
  nh.spinOnce();
  delay(1);
}

動作確認

最初にBluetoothデバイスが使用できるよう権限を変更し、その後ROSを実行します。

$ sudo chmod a+rw /dev/input/js0 
$ rolaunch test.launch

test.launch

<launch>
  <node name="joy_node" pkg="joy" type="joy_node" />
  <node name="car_controller" pkg="car_controller" type="car_controller_node" />
  <node name="arduino-controller" pkg="rosserial_python" type="serial_node.py">
    <param name="port" value="/dev/ttyACM0" />
  </node>
</launch>

コントローラを操作し、ラジコンが動作すれば成功です。

参考

ARラジコンを作る (概要)
Oculus GoにRaspiのカメラ画像を表示する&姿勢を取得する
Oculus Goの姿勢を使ってサーボモーターをコントロールする
Raspberry Pi, Arduinoでタミヤラジコンを制御する (本記事)