Jetbot for ROS Rotsonを作ろう! JetBotで4モーターを動かそう メカナムホイールを動かそう

Jetbotのモーターは、左右2個ですが、ROTSONではモーターを2個から4個に変更可能です。

JetbotのSDイメージの中の設定ファイルは以下です。
cd usr/local/lib/python3.6/dist-packages/jetbot-0.4.0-py3.6.egg/jetbot/robot.py

これは変わる事がありますので検索してみましょう。
（jetbot-0.4.0-py3.6.eggの部分が変わると思います）

なお、Githubのフォルダはこちらです。

https://github.com/NVIDIA-AI-IOT/jetbot

今回もvimを使います(nanoでも問題ないです）
消去はx
行削除はddです

基本は、追加になる部分をコピー＆ペーストします。

今回は、leftとrightを追加します。
left2_motor
right2_motor
としました。
現在は、メカナムを使う際の調整はしていません。
本来は、右周り、左回り、斜め等で設定を変更しないといけません。
さらに、speedを調整することで対応可能です。

TTモーターは、前後で逆方向になるので 正逆はspeedと-speedの組み合わせになります。
これについては、各々で調整してください。

以下は、私のプログラムです。

import time
import traitlets
from traitlets.config.configurable import SingletonConfigurable
from Adafruit_MotorHAT import Adafruit_MotorHAT
from .motor import Motor


class Robot(SingletonConfigurable):

    left_motor = traitlets.Instance(Motor)
    right_motor = traitlets.Instance(Motor)
    left2_motor = traitlets.Instance(Motor)
    right2_motor= traitlets.Instance(Motor)
    # config
    i2c_bus = traitlets.Integer(default_value=1).tag(config=True)
    left_motor_channel = traitlets.Integer(default_value=1).tag(config=True)
    left_motor_alpha = traitlets.Float(default_value=1.0).tag(config=True)
    right_motor_channel = traitlets.Integer(default_value=2).tag(config=True)
    right_motor_alpha = traitlets.Float(default_value=1.0).tag(config=True)
    left2_motor_channel = traitlets.Integer(default_value=3).tag(config=True)
    left2_motor_alpha = traitlets.Float(default_value=1.0).tag(config=True)
    right2_motor_channel = traitlets.Integer(default_value=4).tag(config=True)
    right2_motor_alpha = traitlets.Float(default_value=1.0).tag(config=True)


    def __init__(self, *args, **kwargs):
        super(Robot, self).__init__(*args, **kwargs)
        self.motor_driver = Adafruit_MotorHAT(i2c_bus=self.i2c_bus)
        self.left_motor = Motor(self.motor_driver, channel=self.left_motor_channel, alpha=self.left_motor_alpha)
        self.right_motor = Motor(self.motor_driver, channel=self.right_motor_channel, alpha=self.right_motor_alpha)
        self.left2_motor = Motor(self.motor_driver, channel=self.left2_motor_channel, alpha=self.left2_motor_alpha)
        self.right2_motor = Motor(self.motor_driver, channel=self.right2_motor_channel, alpha=self.right2_motor_alpha)

    def set_motors(self, left_speed, right_speed):
        self.left_motor.value = left_speed
        self.right_motor.value = right_speed
        self.left2_motor.value = left_speed
        self.right2_motor.value = right_speed

    def forward(self, speed=1.0, duration=None):
        self.left_motor.value = speed
        self.right_motor.value = speed
        self.left2_motor.value = -speed
        self.right2_motor.value = -speed
    def backward(self, speed=1.0):
        self.left_motor.value = -speed
        self.right_motor.value = -speed
        self.left2_motor.value = speed
        self.right2_motor.value = speed
    def left(self, speed=1.0):
        self.left_motor.value = -speed
        self.right_motor.value = speed
        self.left2_motor.value  = -speed
        self.right2_motor.value  = speed
    def right(self, speed=1.0):
        self.left_motor.value = speed
        self.right_motor.value = -speed
        self.left2_motor.value = speed
        self.right2_motor.value = -speed
    def stop(self):
        self.left_motor.value = 0
        self.right_motor.value = 0
        self.left2_motor.value = 0
        self.right2_motor.value =  0

書き換え後は、以下を入れます。

:w !sudo tee %

ここでOを入力します。
これで保存完了です。

これでモーター4個動作可能になります。

teleoperation.ipynb

Connect gamepad controller to robot motors
は以下に書き換えました。

from jetbot import Robot
import traitlets

robot = Robot()

left_link = traitlets.dlink((controller.axes[1], 'value'), (robot.left_motor, 'value'), transform=lambda x: -x)
left2_link = traitlets.dlink((controller.axes[1], 'value'), (robot.left2_motor, 'value'), transform=lambda x: x)
right_link = traitlets.dlink((controller.axes[1], 'value'), (robot.right_motor, 'value'), transform=lambda x: -x)
right2_link = traitlets.dlink((controller.axes[1], 'value'), (robot.right2_motor, 'value'), transform=lambda x: x)
slide_link = traitlets.dlink((controller.axes[2], 'value'), (robot.left_motor, 'value'), transform=lambda x: x)
slide2_link = traitlets.dlink((controller.axes[2], 'value'), (robot.left2_motor, 'value'), transform=lambda x: -x)
slide3_link = traitlets.dlink((controller.axes[2], 'value'), (robot.right_motor, 'value'), transform=lambda x: -x)
slide4_link = traitlets.dlink((controller.axes[2], 'value'), (robot.right2_motor, 'value'), transform=lambda x: -x)

ROS

ROSのプログラムは以下の通りです
motor_left2
motor_right2
を作り、ここに動作内容を入れています。
ここまでは簡単ですね。
https://github.com/dusty-nv/jetbot_ros

#!/usr/bin/env python
import rospy
import time

from Adafruit_MotorHAT import Adafruit_MotorHAT
from std_msgs.msg import String

# sets motor speed between [-1.0, 1.0]
def set_speed(motor_ID, value):
    max_pwm = 115.0
    speed = int(min(max(abs(value * max_pwm), 0), max_pwm))

    if motor_ID == 1:
        motor = motor_left
    elif motor_ID == 2:
        motor = motor_right
    elif motor_ID == 4:
        motor = motor_left2
    elif motor_ID == 3:
        motor = motor_right2        

    else:
        rospy.logerror('set_speed(%d, %f) -> invalid motor_ID=%d', motor_ID, value, motor_ID)
        return

    motor.setSpeed(speed)

    if value > 0:
        motor.run(Adafruit_MotorHAT.FORWARD)
    else:
        motor.run(Adafruit_MotorHAT.BACKWARD)


# stops all motors
def all_stop():
    motor_left.setSpeed(0)
    motor_right.setSpeed(0)
    motor_left2.setSpeed(0)
    motor_right2.setSpeed(0)

    motor_left.run(Adafruit_MotorHAT.RELEASE)
    motor_right.run(Adafruit_MotorHAT.RELEASE)
    motor_left2.run(Adafruit_MotorHAT.RELEASE)
    motor_right2.run(Adafruit_MotorHAT.RELEASE)

# directional commands (degree, speed)
def on_cmd_dir(msg):
    rospy.loginfo(rospy.get_caller_id() + ' cmd_dir=%s', msg.data)

# raw L/R motor commands (speed, speed)
def on_cmd_raw(msg):
    rospy.loginfo(rospy.get_caller_id() + ' cmd_raw=%s', msg.data)

# simple string commands (left/right/forward/backward/stop)
def on_cmd_str(msg):
    rospy.loginfo(rospy.get_caller_id() + ' cmd_str=%s', msg.data)

    if msg.data.lower() == "left":
        set_speed(motor_left_ID,  -1.0)
        set_speed(motor_right_ID,  -0.3) 
        set_speed(motor_left2_ID,  1.0)
        set_speed(motor_right2_ID,  0.3) 
    elif msg.data.lower() == "right":
        set_speed(motor_left_ID,   -0.3)
        set_speed(motor_right_ID, -1.0) 
        set_speed(motor_left2_ID,   0.3)
        set_speed(motor_right2_ID, 1.0) 
    elif msg.data.lower() == "backward":
        set_speed(motor_left_ID,   1.0)
        set_speed(motor_right_ID,  1.0)
        set_speed(motor_left2_ID,   -1.0)
        set_speed(motor_right2_ID,  -1.0)       
    elif msg.data.lower() == "forward":
        set_speed(motor_left_ID,  -1.0)
        set_speed(motor_right_ID, -1.0)  
        set_speed(motor_left2_ID,  1.0)
        set_speed(motor_right2_ID, 1.0) 
    elif msg.data.lower() == "stop":
        all_stop()
    else:
        rospy.logerror(rospy.get_caller_id() + ' invalid cmd_str=%s', msg.data)


# initialization
if __name__ == '__main__':

    # setup motor controller
    motor_driver = Adafruit_MotorHAT(i2c_bus=1)

    motor_left_ID = 1
    motor_right_ID = 2
    motor_left2_ID = 4
    motor_right2_ID = 3

    motor_left = motor_driver.getMotor(motor_left_ID)
    motor_right = motor_driver.getMotor(motor_right_ID)
    motor_left2 = motor_driver.getMotor(motor_left2_ID)
    motor_right2 = motor_driver.getMotor(motor_right2_ID)
    # stop the motors as precaution
    all_stop()

    # setup ros node
    rospy.init_node('jetbot_motors')

    rospy.Subscriber('~cmd_dir', String, on_cmd_dir)
    rospy.Subscriber('~cmd_raw', String, on_cmd_raw)
    rospy.Subscriber('~cmd_str', String, on_cmd_str)

    # start running
    rospy.spin()

    # stop motors before exiting
    all_stop()

調整

DCモーターは、ここの性能が異なるため、調整をするようにしてください。
set_speed(motor_XXXX_ID, 1.0)
この値を変更します。