Go to Qiita Advent Calendar 2024 Top
LoginSignup
24
19

Delete article

Deleted articles cannot be recovered.

Draft of this article would be also deleted.

Are you sure you want to delete this article?

More than 5 years have passed since last update.

@MENDY(MENDY)

Raspi+ROSでモータを回してみた

Last updated at Posted at 2018-06-07

はじめに

LaserROS+MATLABで物体検出してみたでは物体検出までやってみましたが，今度はmatlabはまったく関係なく，ROSでモータを回すということをやります

目的

ROSでRaspiからモータを回す

用意するもの

・Raspi(Ubuntu16.04)/type B
・電源
・Maxon Motor
・ESCON
のみ！
RaspiにはGPIOがあるので良いですね
mbedなどなどはいりません

プログラム構成

一般的に，当たり前ですがモータはモータドライバを用いて制御をします
今回はこのESCONがモータドライバになってモータをコントロールします
Raspiがやるのは，モータドライバに指示をだすところ！
モータドライバへの指示はPMWでだします
ので，RaspiからいくつのPWMでだしてね！！(duty比ですが）と指示をだせばモータは回るということです．

長々と説明しましたが，要は
Raspi(PWM)→ESCON→モータ(Maxon)ってことです

さらに，プログラム構成ですが，ROSをかじった人ならわかると思いますが，普通，cmd_velという形で（Geometry_msgs/Twist)で指示をもらうはずです！（速度，角速度）
よってそれを回転数に変換
回転数からduty比を計算
duty比をESCONに送信
ESCONがPWMを送ってくれる
というわけです
二輪想定なので、左右のモーターに送ります

イメージ図
図mtor1.png

事前準備

ESCONを使う場合は、事前にいろいろ設定する必要があります。
ESCONの設定用ソフトウェアをPCにインストールして、USBのケーブルで接続してください
プロパティを押して、Maxonの注文書や型番を見ながらポチポチします

接続

ESCONの説明書を見ながら接続します
こんな感じ
図1cccc.png

ここで、Raspiのピン番号を決めてください！
image.png
ただし！
GPIOのライブラリとしてRPi.GPIO
を使う場合、GND以外は，どのピンでも基本変わりません(ソフトウェアPWMなので)
他のライブラリを使う場合は注意してください

コード

さてコードですが，まずTwistを変換するノードを作ります
2輪を想定していますので，タイヤ幅や，車軸の長さなどは適宜お願いします

twist2int64.py
#!/usr/bin/env python
# -*- coding: utf-8 -*-

import rospy
from geometry_msgs.msg import Twist
from std_msgs.msg import Int64

class Twist2int64():
    def __init__(self):
	    self.command_left = Int64()
        self.command_right = Int64()
        self.received_twist = None
        rospy.init_node('Twist2int64')
        rospy.Subscriber('motor/twist/cmd_vel', Twist, self.callback)
        self.pub_right = rospy.Publisher('right_motor/cmd_vel', Int64, queue_size=10)#name, topic_type, size
        self.pub_left = rospy.Publisher('left_motor/cmd_vel', Int64, queue_size=10)#name, topic_type, size

    def main_twist2int64(self):
        
        rospy.spin()

    def callback(self, message):
        self.received_twist = message #input data
        self.command_right, self.command_left = self.twist2rpm(self.received_twist)
        self.pub_right.publish(self.command_right)
        self.pub_left.publish(self.command_left)

    def twist2rpm(self, received_data):#convert to speed
        #(m/s, rad/s)
        wheeles_size = 0.075#wheel size
        axle_length = 0.35#axle_size(2d)

        v = received_data.linear.x#(m/s)
        omega = received_data.angular.z#(rad/s)

        v_r = (omega*axle_length + 2*v)/2
        v_l = (omega*axle_length - 2*v)/(-2)

        v_r = v_r/(wheeles_size * 2 * 3.14) #wheel_speed(1/s)
        v_l = v_l/(wheeles_size * 2 * 3.14) #wheel_speed(1/s)
        r_rpm = 60 * v_r * 19 #gear rate
        l_rpm = 60 * v_l * 19 #gear rate

        return r_rpm, l_rpm

#Main Program
Convert = Twist2int64()
Convert.main_twist2int64()

#pub memo
	#rostopic pub motor/twist/cmd_vel geometry_msgs/Twist '[1.0, 0.0, 0.0]' '[0.0, 0.0, 0.0]'

さらにそれをduty比にしてPWMを送るプログラムです

:motor_right.py
#!/usr/bin/env python
# -*- coding: utf-8 -*-

import RPi.GPIO as GPIO#GPIOの設定
import time
import rospy
from std_msgs.msg import Int64

class motor_command():
    def __init__(self):#初期化
        #初期変数		
        self.RotationDirect = True #Forward
        self.Stop = False #Forward
        self.Enable = True
        self.pin_num_PWM = 18#pinの名前
        self.pin_num_RotationDirect = 15
        self.pin_num_Enable = 14
        self.pin_num_Stop = 23
        self.freq = 50 # Hz (PWM のパルスを一秒間に 50 個生成)
        #ノード立ち上げ（ROS）
        rospy.init_node('maxon_test')
        rospy.Subscriber('right_motor/cmd_vel', Int64, self.callback)
        rospy.loginfo('Ready to receive motor command')

        #初期設定（GPIO）
        GPIO.setmode(GPIO.BCM)
        # GPIOをデジタル出力に設定 GPIOのセットアップ
        GPIO.setup(self.pin_num_PWM, GPIO.OUT) #PWM
        GPIO.setup(self.pin_num_RotationDirect, GPIO.OUT) #rotation direct
        GPIO.setup(self.pin_num_Enable, GPIO.OUT) #enable
        GPIO.setup(self.pin_num_Stop, GPIO.OUT) #stop`
        # PWM 設定
        GPIO.output(self.pin_num_RotationDirect, GPIO.LOW)
        GPIO.output(self.pin_num_Enable, GPIO.LOW)
        GPIO.output(self.pin_num_Stop, GPIO.HIGH)
        self.pwm = GPIO.PWM(self.pin_num_PWM, self.freq)
        duty = 0.0 # デューティー比 0.0 で出力開始 (パルス内に占める HIGH 状態の時間が 0.0 %)
        self.pwm.start(duty)

    def motor_main(self):
        rospy.spin()
        #お片付け
        GPIO.cleanup()
        rospy.loginfo('回転終了')
        self.pwm.stop()#終了
		
    def callback(self, data):
        self.rpm_command = data.data#rpmで取得
        rospy.loginfo(self.rpm_command)
        duty_command = self.rpm2duty(self.rpm_command)#rpm→duty比へ（制約考慮）
        rospy.loginfo('motor command is '+ str(duty_command))
        #PINいじり
        if duty_command >= 0:#正逆回転
            self.RotationDirect = True
        else:
            self.RotationDirect = False#逆回転
            rospy.loginfo('Reverse')
            duty_command = -duty_command

        if self.RotationDirect:
            GPIO.output(self.pin_num_RotationDirect,GPIO.HIGH)
        else:
            GPIO.output(self.pin_num_RotationDirect,GPIO.LOW)

        if self.Stop:
            GPIO.output(self.pin_num_Stop, GPIO.HIGH)
        else:
            GPIO.output(self.pin_num_Stop, GPIO.LOW)

        if self.Enable:
            GPIO.output(self.pin_num_Enable, GPIO.HIGH)
        else:
            GPIO.output(self.pin_num_Enable, GPIO.LOW)
	
        rospy.loginfo('Change Command')

        self.pwm.ChangeDutyCycle(duty_command)#回転量を変える

    def rpm2duty(self, rpm):
        max_rpm = 6000 #規定値90%
        max_duty = 90
        min_rpm = 0 #規定値10%
        min_duty = 10
        rpm_rate = (max_rpm-min_rpm)/(max_duty-min_duty)#1%に対するrpm
        if rpm >= 0:
            duty = rpm / rpm_rate + min_duty
        else:
            duty = rpm / rpm_rate - min_duty  
        #速度制約
        if duty > 90:
            rospy.loginfo('Speed limit!!')
            duty = 90
        elif duty < -90:
            rospy.loginfo('Speed limit!!')
            duty = -90
        return duty

if __name__ == '__main__':#これがメイン実行file
    Motor_Command = motor_command()
    Motor_Command.motor_main()
        #pubする場合の参考
        #rostopic pub right_motor/cmd_vel std_msgs/Int64 "data: 10

今回は二輪ロボットを想定しているので左右のモーターに指示を送ります
これは左ですね！

安定化電源の電圧は決められたものにしてください
普通は12Vです

これらのプログラムを用いて，

roscore

で後は2つのプログラムを順に回してください！！

ただ，これだけでは回らないので

rostopic pub motor/twist/cmd_vel geometry_msgs/Twist '[1.0, 0.0, 0.0]' '[0.0, 0.0, 0.0]'

でpublishしてください！

最後に

Rapiを使ってモータを回しました
次はエンコーダをとってみましょう！
しかし，Maxonモータはエンコーダのパルス数が多いので．．．．
Raspiで対応できないかもしれないですね．．．
工夫案ある方募集中です

今日のは
https://github.com/Shunichi09/Qiita/tree/master/motor
に上がってます
確認お願いします

24
19
1

Register as a new user and use Qiita more conveniently

  1. You get articles that match your needs
  2. You can efficiently read back useful information
  3. You can use dark theme
What you can do with signing up
Sign upLogin
24
19

Delete article

Deleted articles cannot be recovered.

Draft of this article would be also deleted.

Are you sure you want to delete this article?