ArduinoのRCサーボ ライブラリを使用する

ROSから目標位置のトピック・メッセージを送信し、Arduinoに接続したRCサーボ(ラジコンのサーボモータ)を動かしてみます。

以前に、rosserialとrosserial_arduinoパッケージを使用してArduinoをROSのノードとして動作させる方法を書きました。

これに、RCサーボへ目標位置を送信する機能を追加すれば実現できることがわかります。

ところで、RCサーボの目標位置は、パルス幅によって指定します。 例えば、5msecのパルスを送ると0度、25msecのパルスでは180度の目標位置となるようにサーボが動く、という仕組みになっています。

このようにRCサーボに目標位置を送信するための、Servo library というArduinoのライブラリがあり、これを使えば簡単に機能追加できます。

rosserial_arduinoのサンプルで簡単に作る

rosserial_arduinoパッケージからArduino-IDEにインストールしたサンプルには、ROSからArduinoに接続したRCサーボを動かすためのサンプル・スケッチが含まれています。 このサンプル・スケッチを使えば、

ROS---(rosserial)--->[Arduino+Servo lib]--->RCサーボ
という、やろうとしていること、そのままズバリのことができます。

2個のRCサーボを動かす

Arduinoサンプルスケッチのros_lib->ServoControlを開くと、1個のサーボモータを動かすスケッチが開きます。これを以下のように修正して2個のサーボモータを動かすように修正します。



#if (ARDUINO >= 100)
 #include <Arduino.h>
#else
 #include <WProgram.h>
#endif

#include <Servo.h> 
#include <ros.h>
#include <std_msgs/UInt16.h>

#define SERVO_PULSE_MIN_US    500
#define SERVO_PULSE_MAX_US    2500

ros::NodeHandle  nh;

Servo servoP, servoT;

void servoP_cb( const std_msgs::UInt16& cmd_msg){
  servoP.write(cmd_msg.data); //set servo angle, should be from 0-180  
  digitalWrite(13, HIGH-digitalRead(13));  //toggle led  
}

void servoT_cb( const std_msgs::UInt16& cmd_msg){
  servoT.write(cmd_msg.data); //set servo angle, should be from 0-180  
  digitalWrite(13, HIGH-digitalRead(13));  //toggle led  
}

ros::Subscriber<std_msgs::UInt16> subP("servo_p", servoP_cb);
ros::Subscriber<std_msgs::UInt16> subT("servo_t", servoT_cb);

void setup(){
  pinMode(13, OUTPUT);

  nh.initNode();
  nh.subscribe(subP);
  nh.subscribe(subT);
  
  servoP.attach(3, SERVO_PULSE_MIN_US, SERVO_PULSE_MAX_US);
  servoT.attach(4, SERVO_PULSE_MIN_US, SERVO_PULSE_MAX_US);
}

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

このスケッチをArduinoのマイコンに書き込みます。

servo_p, servo_t トピックをsubscribeし、それぞれ3番、4番ピンに接続されたRCサーボに位置指令を送ります。

"p", "t"は、それぞれパン、チルトの意味で銘々しています。

RCサーボには5Vを供給、信号線はArduinoの3番、4番ピンに接続します。

上の写真で使用しているのは、秋月電子のATmega328用マイコンボードです。マイコンATmega328PにArduinoのブートローダを書き込み、Arduino互換にして使っています。

動作を確認

ArduinoをROSのPCに接続します。ROSからトピック・メッセージを送り、RCサーボを動かしてみます。

$ rosrun rosserial_python serial_node.py /dev/ttyUSB0   <-- rosserialを起動
$ rostopic list  <-- 他の端末で
・・・
/servo_p   <-- Arduino上に作った(実際はrosserialだが)2つのノードが見える
/servo_t
・・・
$ rostopic pub -1 /servo_p std_msgs/UInt16 0   <-- servo_p トピックに"0"を送る pin3のRCサーボが0度に動く
$ rostopic pub -1 /servo_p std_msgs/UInt16 180   <-- servo_p トピックに"180"を送る pin3のRCサーボが180度に動く
$ rostopic pub -1 /servo_t std_msgs/UInt16 0   <-- servo_t トピックに"0"を送る pin4のRCサーボが0度に動く
$ rostopic pub -1 /servo_t std_msgs/UInt16 180   <-- servo_t トピックに"180"を送る pin4のRCサーボが180度に動く
		

これだけで、とりあえず動かせるが・・・

ほとんどコーディングせずに、たったこれだけでROSからRCサーボを動かせる、というのは素晴らしいのですが、これでは全く物足りないですね。

「指定した角度までRCサーボが全速で動いて止まる」、ただそれだけです。

説明文で「RCサーボを動かす」という言葉を使っているのは、そのためなのです。

こんなことなら、わざわざROSなんて使う必要もないでしょう。

もっと「制御らいしいこと」をさせたいですね。 ・・・それについては、また今度、書いてみたいと思います。

メモ: RCサーボはArduinoのPWMピンに接続しなければならないのか

ArduinoのServo libraryでは、assign()関数でRCサーボを接続するピンを指定します。ネット上では、PWM出力が可能なピン(3,5,6,9,10,11など)に割り当てている例が多くみつかります。

RCサーボは、PWM出力(タイマモジュールのOutput compare出力)対応ピンに接続しなければならないのでしょうか?

結論から言うと、「どのピンに割り当てても良い」ということがわかりました。(少なくともArduino 1.6.7 + AVRでは)

Servo libraryの説明にも、ピン割り当てに制限があるようなことは書かれていませんでした。

では、どのようにパルスを発生させているのか? ライブラリのソースを見ると、割り込みハンドラで次のようにdigitalWrite()でピンにパルスを出力していることがわかります。

static inline void handle_interrupts(timer16_Sequence_t timer, volatile uint16_t *TCNTn, volatile uint16_t* OCRnA)
{
  if( Channel[timer] < 0 )
    *TCNTn = 0; // channel set to -1 indicated that refresh interval completed so reset the timer
  else{
    if( SERVO_INDEX(timer,Channel[timer]) < ServoCount && SERVO(timer,Channel[timer]).Pin.isActive == true )
      digitalWrite( SERVO(timer,Channel[timer]).Pin.nbr,LOW); // pulse this channel low if activated
  }

  Channel[timer]++;    // increment to the next channel
  if( SERVO_INDEX(timer,Channel[timer]) < ServoCount && Channel[timer] < SERVOS_PER_TIMER) {
    *OCRnA = *TCNTn + SERVO(timer,Channel[timer]).ticks;
    if(SERVO(timer,Channel[timer]).Pin.isActive == true)     // check if activated
      digitalWrite( SERVO(timer,Channel[timer]).Pin.nbr,HIGH); // its an active channel so pulse it high
  }
・・・ 以下省略 ・・・
				

タイマモジュールのOutput compareでパルスを生成しているわけではなく、タイマ割り込みでパルスを生成しています。

そのため、パルス生成の度に割り込みが発生します。