RaspberryPiのROSからTB6612でDCモータを制御する

ROS

カムロボットで使用したDCモータの制御用プログラムの解説です.
RaspberryPiとTB6612との配線はカムロボットでの配線を見てください.

まず,TB6612の入出力の関係は次の表の通りです.TB6612は1つのモータ当たり3つの信号線(うち1つはPWM)が必要になります.

TB6612の入出力 (l/L : Low Input/Output, h/H: HIGH Input/Output,Z: High Impedance, %DUTY: PWMのデューティー比)
IN1 IN2 OUT1 OUT2 モータの状態
l l Z Z ストップ
l h L H 逆転(%DUTY)
h l H L 正転(%DUTY)
h h H H ブレーキ

モータを制御する際に毎度この表を見るのは面倒なので,TB6612というクラスを使ってピンへの指令を行います.
実際にモータを制御するtest_run.cppではpigpioを使ってGPIOを操作します.
実際に動かす時には,モータ駆動編の記事と同様に次のコマンドを実行します.

sudo pigpiod
cd ~/catkin_ws
rosparam read src/tamiya_cam_robot/pigpio.yaml
rosrun tamiya_cam_robot test_run

では実際にtest_run.cppの中身を見ていきます.

6–11行目

使用するヘッダファイルを読み込みます.ROSとC++の基本的なものの他に,TB6612というクラスを書いたTB6612.hppとpigpiod用のpigpiod_if2.hをインクルードします.pigpioではデーモンのpigpiodを使用しない場合と,使用する場合でインクルードするヘッダファイルが異なります.pigpiodを使用しない場合,実行ファイルをsudoで実行する必要があります.しかしROSではsudoでのノードの立ち上げを推奨していないため,今回はpigpiodを使用する方法をとります.

16–17行目

main関数でROSのノードの初期設定を行います.

20–26行目

pigpiodに接続します.この時,事前にpigpiodを立ち上げていない場合,pi<0となりプログラムは終了します.

29–56行目

ピンの割り当てをROSPARAMから読みこみます.実行前に読み込んだpigpio.yamlでは,ピンの設定を文字列の配列としています.33行目でstd::vectorpinoutsに’pigpio’のROSPARAMを読み込みます.この時,’pinout.at(0)’は0番ピンの設定が格納されています.pinoutsからA_IN1B_PWMまでの6つのピンを検索し,TB6612用のピン番号を読み取ります.

59–87行目

読みとったピンの割り当てを使って,TB6612クラスを’driver’という名前で立ち上げます.
TB6612クラスは’drive’というメソッドを持っていて,次のように宣言されています.

/**
* drive a motor
* @param motor motor's id
* @param power power of a motor (-255 -- 255)
*/
void drive(int motor, int power);

motorにはモータの番号,powerにはモータのパワーを入力します.powerはプラスとマイナスで回転方向が変わります.
std::this_thread::sleep_for(std::chrono::seconds(3));は3秒待機する命令です.
前進->後退->右旋回->左旋回を3秒ずつ行い,最後に静止します.

90行目

最後にpigpiodとの通信を切って,プログラムは終了となります.

コメント