ROS勉強記その4:Raspberry Pi 4+ROS(Noetic) を使ってPS4コントローラーの操作でDCモータを制御する

投稿日:2023年3月14日
最終更新日:2023年3月20日

Raspberry Pi 4+ROSを使ってPS4コントローラーでDCモータをPWM制御する

前回はラズパイのGPIOピンにLEDを接続して、PS4コントローラーからGPIOを制御することでLチカをやってみた。今回はもう少しロボットっぽいことを…ということで、DCモーターをPWM制御して動かしてみる。

具体的には、PS4コントローラのスティックを倒した量に応じてモータの回転速度が変わるように、モータを接続したGPIOピンをPWM制御するROSパッケージを作ってみる。

なお、DCモータとモータドライバは、以前FPGA日記で使った

  • DCモータ:Digilent DC Motor/Gearbox 290-008
  • モータドライバ:Digilent Pmod DHB1: Dual H-bridge 410-259

を流用する。

【FPGA】ZyboでDCモータを回そう日記

 

実行環境

Raspberry Pi 4 Model B / 4GB

Ubuntu Server 20.04.5 LTS(64-bit)

ROS Noetic Ninjemys

ワイヤレスコントローラーDUALSHOCK 4(Playstation4コントローラー)

 

DCモータ&モータモータドライバとラズパイを接続する

DCモータとモータドライバは同じDigilent社製なので、普通にコネクタをつなげればOKだが、ラズパイからモータドライバを操作するために、モータドライバのJ1コネクタのピンとラズパイのGPIOピンをジャンパー線で接続する。

モータドライドライバのピンアサインは以下にある。

https://digilent.com/reference/pmod/pmoddhb1/start?redirect=1

今回は以下のように接続した。

モータードライバ Raspberry Pi 4 ピン
Pin 1 EN1 GPIO 13
Pin 2 DIR1 GPIO 22
Pin 5 GND GND
Pin 6 VCC 3V3 power
Pin 7 EN2 GPIO 12
Pin 8 DIR2 GPIO 23
Pin 11 GND GND
Pin 12 VCC 3V3 power

ラズパイの電源でモーターを回すのはかなり厳しいので、モーターを駆動する電源として今回は単3電池を4つ接続できる電池BOXを使った。私が使ったのは以下のもの。ON/OFFスイッチがついていると何かと便利なのでこれを使っている。

実際にモータードライバとDCモータ、ラズパイ、電池を接続すると以下のような状態になる。

 

PS4コントローラーの入力を受け取りGPIOをPWM制御するSubscriberを実装

前回はjoyノードからmessageを受けとるsubscriberノードを実装することでLチカを実現したが、今回もGPIOを制御してDCモータを回すので、ほぼ同じ内容になる。ただ、今回使用するDCモータはPWM制御に対応しているので、PS4コントローラーの左右のスティックの入力に応じて、GPIOをPWM制御して回転速度を調整できるようにしてみる。

まずはROSのノードをいつものステップで作成。今回はdc2_joyという名前でパッケージをつくり、pwm_2motors.pyという名前でPythonでコードを実装する。

cd ~/catkin_ws/src
mkdir dc2_joy
cd dc2_joy
catkin_create_pkg dc2_joy roscpp rospy std_msgs sensor_msgs
cd dc2_joy/src
code pwm_2motors.py

以下がコードの内容。global変数使ってるとことか関数の分け方とかちょっとイマイチな感じだが、そこはおいおい検討していこう(といいつつ多分しない)

#!/usr/bin/env python3
# -*- coding: utf-8 -*-

import rospy
import pigpio
from sensor_msgs.msg import Joy

MOTOR1_EN_PIN = 13
MOTOR2_EN_PIN = 12
MOTOR1_DIR_PIN = 22
MOTOR2_DIR_PIN = 23
FREQ = 1000     # Hz
MAX_DUTY = 30   # %

pi = pigpio.pi()
motor1_dir_prev = 0
motor2_dir_prev = 0

def det_dir(axes):
    if axes < 0:
        dir = 1
    else:
        dir = 0
    return dir

def calc_duty(axes, dir, dir_prev):
    if dir == dir_prev:
        duty = int(MAX_DUTY * abs(axes) * 10000)
    else:
        duty = 0  #disable before changing direction 
    return duty

def callback(msg):
    global motor1_dir_prev
    global motor2_dir_prev
    motor1_dir = det_dir(msg.axes[1])
    motor1_duty = calc_duty(msg.axes[1], motor1_dir, motor1_dir_prev)
    motor2_dir = det_dir(msg.axes[5])
    motor2_duty = calc_duty(msg.axes[5], motor2_dir, motor2_dir_prev)

    pi.hardware_PWM(MOTOR1_EN_PIN, FREQ, motor1_duty)
    pi.hardware_PWM(MOTOR2_EN_PIN, FREQ, motor2_duty)
    rospy.sleep(0.001)
    
    pi.set_mode(MOTOR1_DIR_PIN, pigpio.OUTPUT)
    pi.write(MOTOR1_DIR_PIN, motor1_dir)
    pi.set_mode(MOTOR2_DIR_PIN, pigpio.OUTPUT)
    pi.write(MOTOR2_DIR_PIN, motor2_dir)

    motor1_dir_prev = motor1_dir
    motor2_dir_prev = motor2_dir

def listener():
    rospy.init_node('listener', anonymous=True)
    rospy.Subscriber('/joy', Joy, callback)
    rospy.spin()

if __name__ == '__main__':
    listener()

概要だけ説明すると、msg.axes[1]がPS4コントローラーの左スティック、msg.axes[5]がPS4コントローラーの右スティックの、上下に倒したときのFloat値の位置情報になっており、-1.0~1.0の値を取る。

また、det_dirはPS4コントローラのスティックが倒された方向からモーターの回転方向を返す関数で、calc_dutyがスティックの操作量に応じたDuty比を返す関数になっている。DIRを変えるときはENをLOWにするようにモータードライバのデータシートに記載があるので、calc_dutyのなかでDIRを変えるまえにDuty比を0にして、ENがLOWにしている。(これで所望の動作になっているか微妙に自信がないが)

コーディングが出来たら、前回と同様に以下で権限をつけておく。

chmod +x pwm_2motors.py

これでパッケージの準備は完了。

 

動かしてみる

あとは前回と同様に以下の順番で実行するだけ。まずはds4drvを実行して、PS4コントローラーPSボタン + ShareButtonを長押しして接続する。

sudo ds4drv

次に、pigpiodを起動させておく。

sudo pigpiod

あとはROSのノードを実行していくだけ。もちろんlaunchファイルにまとめてもOK。

roscore
rosrun joy joy_node
rosrun dc2_joy pwm_2motors.py

以下が実行してみた動画(以前3Dプリンタでつくったフレームにモータを固定している)。コントローラの左右のスティックの倒し方によって、モータの回転速度が変わっていることがわかる。

これでDCモータをPS4コントローラから動かすことができた。次回はいよいよ3Dプリンタでつくったフレームにバッテリーやラズパイを固定して、自走できるロボットをつくってみる。

 

ROS勉強記まとめ

この勉強記は以下のページでまとめてます。

ROS(Robot Operating System)勉強記 まとめ


投稿者: wakky

映画と旅行が大好きなエンジニア。お酒、ゲーム、読書も好き。

コメントを残す

メールアドレスが公開されることはありません。 が付いている欄は必須項目です

このサイトはスパムを低減するために Akismet を使っています。コメントデータの処理方法の詳細はこちらをご覧ください