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)
この値を変更します。