メカナムホイールカーをJetson Nanoから制御してみた

スポンサーリンク

前回の続き。
キットの組み立て、動作確認が終わったので、次はJetson Nanoと接続して制御。
ゲームパッドで操作したかったので、USBゲームパッドF710も接続。

完成後はこんな感じ

写真が前回と代わり映えしないので、中身

写真には写ってないけど、モバイルバッテリも必要。

ハードウェア環境

構成

コメント
メカナムホイールカーキット OSOYOO メカナムホイールカー キット
Jetson Nano 古いA02モデル。メモリ4G
Jetson Nano用電源 INIU モバイルバッテリ 容量10000mAh。最大出力3A
Wi-Fiドングル TP-Link TL-WN725N
ゲームパッド ロジクール ワイヤレスゲームパッド F710
PCA9685 VKLSVAN PCA9685 16チャンネル 12ビット PWM ドライバー

Jetson Nanoの電源はとりあえず車体とは別にしました。

接続

接続の概略図 f:id:sato_susumu:20201203083513p:plain

Jetson NanoとPCA9685間はI2C接続(5V)。

Jetson Nano PCA9685
5V VCC
SDA(Pin3) SDA
SCL(Pin5) SCL
GND GND

あとは、Jetson Nano側回路のGNDとL298N側回路のGNDを接続する。

ソフトウェア環境

ベースとなるMicroSDイメージ

ベースとなるSDイメージは、こちらにあるJetbotのSDイメージ jetbot-042_nano-4gb-jp441.zip を使用。

このイメージの中身は次のとおり。

項目 バージョン
OS Ubuntu 18.04
JetPack 4.4.1
JetBot 0.4.2

Wi-Fi設定

設定方法はこちらの ソフトウェアセットアップ > Wi-Fi設定 参照 www.sato-susumu.com

Wi-Fi経由でsshアクセス

コンピュータ名は nano-4gb-jp441 だったので、それを使ってsshアクセス。

$ ssh jetbot@nano-4gb-jp441

もしくは ssh jetbot@<IPアドレス>

Jetson Nanoのパワーモデルを5Wモードに変更

モバイルバッテリを使うので、パワーモデルを5Wモードに変更

$ sudo nvpmodel -m 1
$ sudo nvpmodel -q

ゲームパッドのテスト

こちらを参照 www.sato-susumu.com

設定はMode OFF。上部のスライドスイッチはX側。
一定時間で省電力モードに移行するので、操作するときにはSTARTボタン!。よく忘れる。

ツールやライブラリのインストール

pipとpythonライブラリをインストール

$ sudo apt install -y python3-pip
$ pip3 install Adafruit_PCA9685

プログラム

$ cd ~
$ vi mecanum-control.py

mecanum-control.pyの内容は次のとおり。
注意:左右でモーターの回転方向が変わるはずなのにそれに該当する処理がない。。。 どこかに問題を抱えているはず。

プログラムの内容 (クリックで展開)

#!/usr/bin/python3
# -*- coding: utf-8 -*-

import Adafruit_PCA9685
import time
import struct
from typing import List, Union


class PWMMotor:
    __pwmChannels = [
        {'en': 0, 'in1': 1, 'in2': 2},
        {'en': 5, 'in1': 3, 'in2': 4},
        {'en': 6, 'in1': 7, 'in2': 8},
        {'en': 11, 'in1': 9, 'in2': 10},
    ]
    __PWM_MAX = 0xfff

    def __init__(self, motorID: int):
        self.motorID = motorID

    def __setPwm(self, ch: int, speed: Union[int, float]):
        value = self.__translate(speed, 0, 1, 0, self.__PWM_MAX)
        if value < 0:
            value = 0
        if value > self.__PWM_MAX:
            value = self.__PWM_MAX
        pwm.set_pwm(ch, 0, int(value))
        # print("setPWM", ch, 0, int(value))

    def __translate(self, sensor_val: Union[int, float], in_from: Union[int, float], in_to: Union[int, float], out_from: Union[int, float], out_to: Union[int, float]) -> float:
        # numpy.interpの代わり
        out_range = out_to - out_from
        in_range = in_to - in_from
        in_val = sensor_val - in_from
        val = (float(in_val) / in_range) * out_range
        out_val = out_from + val
        return out_val

    def setSpeed(self, speed: Union[int, float]):
        ch_en = self.__pwmChannels[self.motorID]['en']
        ch_in1 = self.__pwmChannels[self.motorID]['in1']
        ch_in2 = self.__pwmChannels[self.motorID]['in2']
        if speed == 0:
            self.__setPwm(ch_in1, 0)
            self.__setPwm(ch_in2, 0)
            self.__setPwm(ch_en, 1)
        elif speed > 0:
            self.__setPwm(ch_in1, 0)
            self.__setPwm(ch_in2, 1)
            self.__setPwm(ch_en, speed)
        else:
            self.__setPwm(ch_in1, 1)
            self.__setPwm(ch_in2, 0)
            self.__setPwm(ch_en, -speed)


class AllMotors:
    def __init__(self):
        self.frontLeftMotor = PWMMotor(0)
        self.frontRightMotor = PWMMotor(1)
        self.rearLeftMotor = PWMMotor(2)
        self.rearRightMotor = PWMMotor(3)

    def setSpeed(self, speeds: List[int]):
        self.frontLeftMotor.setSpeed(speeds[0])
        self.frontRightMotor.setSpeed(speeds[1])
        self.rearLeftMotor.setSpeed(speeds[2])
        self.rearRightMotor.setSpeed(speeds[3])

    def brake(self):
        self.frontLeftMotor.setSpeed(0)
        self.frontRightMotor.setSpeed(0)
        self.rearLeftMotor.setSpeed(0)
        self.rearRightMotor.setSpeed(0)


pwm = Adafruit_PCA9685.PCA9685(address=0x40, busnum=1)
pwm.set_pwm_freq(60)
allMotors = AllMotors()
allMotors.brake()

with open("/dev/input/js0", "rb") as f:
    while True:
        a = f.read(8)
        t, value, code, index = struct.unpack(
            "<Ihbb", a)  # 4 bytes, 2 bytes, 1 byte, 1 byte
        # print("time(ms): {:10d}, value: {:6d}, code: {:1d}, index: {:1d}".format(
        #    t, value, code, index))
        if code == 2 and index == 7 and value > 10000:
            allMotors.setSpeed(
                [1, 1,
                 1, 1])
        elif code == 2 and index == 7 and value < -10000:
            allMotors.setSpeed(
                [-1, -1,
                 -1, -1])
        elif code == 2 and index == 6 and value < -10000:
            allMotors.setSpeed(
                [-1, 1,
                 -1, 1])
        elif code == 2 and index == 6 and value > 10000:
            allMotors.setSpeed(
                [1, -1,
                 1, -1])
        elif code == 1 and index == 4 and value == 1:
            allMotors.setSpeed(
                [1, -1,
                 -1, 1])
        elif code == 1 and index == 5 and value == 1:
            allMotors.setSpeed(
                [-1, 1,
                 1, -1])
        else:
            allMotors.brake()

プログラム実行

$ python3 mecanum-control.py

起動時にプログラムを自動実行(オプション)

不要になったときには、自動実行の止め忘れに注意。

$ sudo mv mecanum-control.py /usr/sbin/
$ sudo chmod 755 /usr/sbin/mecanum-control.py
$ sudo vi /etc/systemd/system/mecanum-control.service

mecanum-control.serviceの内容は次のとおり。

[Unit]
Description = Mecanum wheel car control
After = NetworkManager.service

[Service]
Type = simple
User = jetbot
ExecStart = /usr/sbin/mecanum-control.py
Restart = always

[Install]
WantedBy = multi-user.target

サービスに登録して、実行。

$ sudo systemctl daemon-reload
$ sudo systemctl start mecanum-control
$ systemctl status mecanum-control

動作に問題なければ、自動起動するように設定。

$ sudo systemctl enable mecanum-control
$ systemctl status mecanum-control

再起動して、ゲームパッドで操作できれば完成。

$ sudo reboot

補足

I2Cの接続状況を確認

I2Cの接続状況を確認。コマンドを実行してスムーズに表示されない場合は、配線ミス。

$ i2cdetect -r -y 1
     0  1  2  3  4  5  6  7  8  9  a  b  c  d  e  f
00:          -- -- -- -- -- -- -- -- -- -- -- -- -- 
10: -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- 
20: -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- 
30: -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- 
40: 40 -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- 
50: -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- 
60: -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- 
70: 70 -- -- -- -- -- -- --    

PCA9685の動作状況確認

PCA9685の動作状況はI2Cの読み込みで確認できる。

$ i2cdump -y 1 0x40
No size specified (using byte-data access)
     0  1  2  3  4  5  6  7  8  9  a  b  c  d  e  f    0123456789abcdef
00: 01 04 e2 e4 e8 e0 00 00 ff 0f 00 00 00 00 00 00    ??????...?......
10: ff 0f 00 00 ff 0f 00 00 00 00 00 00 ff 0f 00 00    .?...?.......?..
20: ff 0f 00 00 00 00 00 00 ff 0f 00 00 ff 0f 00 00    .?.......?...?..
30: 00 00 00 00 ff 0f 00 00 00 00 00 00 00 00 00 00    .....?..........
40: 00 00 00 00 00 00 XX XX XX XX XX XX XX XX XX XX    ......XXXXXXXXXX
省略

GPIOを使うための設定

JetbotのSDイメージを使う場合は設定済みのため確認不要。
GPIOを使うためには、グループ作成とルール設定、グループへのユーザー追加が必要らしい。

グループに登録されているユーザーの確認方法。

$ getent group gpio
gpio:x:999:jetbot