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
![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/285344/6cdf9f32-71a9-d36a-3918-f654278d6083.png)
これは変わる事がありますので検索してみましょう。
（jetbot-0.4.0-py3.6.eggの部分が変わると思います）

なお、Githubのフォルダはこちらです。
![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/285344/9cd6a21e-be3f-8d84-d8d5-b22f8d9d3fcb.png)
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
```
![Motor-1-1.jpg](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/285344/66ba68c0-7a9d-aea8-cffa-3391023d7322.jpeg)

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

```
:w !sudo tee %
```
![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/285344/da1e9981-d883-7d7f-6185-2cf12eff8464.png)
ここでOを入力します。
これで保存完了です。
![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/285344/82baa53b-3de9-82e0-cdd9-ff9f61afb704.png)
これでモーター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)
```
![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/285344/59473a4a-c81d-e338-eb86-0bdade3a750d.png)


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