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
を流用する。
実行環境
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勉強記まとめ
この勉強記は以下のページでまとめてます。
