ラズパイタンク製作記 〜ソフトウェア編〜

ハードウェア編はこちら

まずは動作させているところの動画です。

pic.twitter.com/UWw8SkSaUo April 24, 2020

Nintendo Switchのコントローラ(Joy-Con)で操縦できるようにしました。
Joy-Conとのペアリング方法はこちらの記事を参考にさせてもらいました。
Nintendo Switch の Joy-Con で家電を操作する

以下にソースコードを載せます。
いかにも素人臭いコードになっているかと思いますが、ご容赦ください。

import time
import pygame
import RPi.GPIO as GPIO
import pigpio

#モーターの順転/逆転 制御用GPIOの設定
GPIO.setwarnings(False) 
GPIO.setmode(GPIO.BCM)
GPIO.setup(17, GPIO.OUT) #右モーターの回転方向制御
GPIO.setup(27, GPIO.OUT) #左モーターの回転方向制御

#モーターの回転速度制御用GPIO(PWM)の設定
gpio_pin0 = 12
gpio_pin1 = 13

pi = pigpio.pi()
pi.set_mode(gpio_pin0, pigpio.OUTPUT)

#モーター回転速度設定パラメータ
duty = 5
freq = 500
sleep_time = 0.1

#コントローラの初期化
pygame.init()
joys = pygame.joystick.Joystick(0)
joys.init()

while 1:
    hat_LR,hat_FB=0,0
    hat_back,hat_front=0,0
    pygame.event.get()
    hat_LR,hat_FB=joys.get_hat(0)
    if hat_FB < -0.5:
        hat_back = 1
    elif hat_FB > 0.5:
        hat_front = 1

    button_front=joys.get_button(3)
    button_back=joys.get_button(0)
    button_right=joys.get_button(1)
    button_left=joys.get_button(2)

    #print('左前：'+str(hat_front)+', ' +'左後：'+str(hat_back)+', ' +'右前：'+str(button_front)+', ' +'右後：'+str(button_back)+', ' +'speed:' + str(duty-2))

    #前進
    if hat_front == 1:
        GPIO.output(17, GPIO.HIGH)
        GPIO.output(27, GPIO.HIGH)
        pi.hardware_PWM(gpio_pin0, freq, duty*100000)
        pi.hardware_PWM(gpio_pin1, freq, duty*100000)
        time.sleep(sleep_time)
        pi.hardware_PWM(gpio_pin0, freq, 000000)
        pi.hardware_PWM(gpio_pin1, freq, 000000)

    #後退
    elif hat_back == 1:
        GPIO.output(17, GPIO.LOW)
        GPIO.output(27, GPIO.LOW)
        pi.hardware_PWM(gpio_pin0, freq, duty*100000)
        pi.hardware_PWM(gpio_pin1, freq, duty*100000)
        time.sleep(sleep_time)
        pi.hardware_PWM(gpio_pin0, freq, 000000)
        pi.hardware_PWM(gpio_pin1, freq, 000000)

    #左旋回
    elif button_left == 1:
        GPIO.output(17, GPIO.LOW)
        GPIO.output(27, GPIO.HIGH)
        pi.hardware_PWM(gpio_pin0, freq, duty*100000)
        pi.hardware_PWM(gpio_pin1, freq, duty*100000)
        time.sleep(sleep_time)
        pi.hardware_PWM(gpio_pin0, freq, 000000)
        pi.hardware_PWM(gpio_pin1, freq, 000000)

    #右旋回
    elif button_right == 1:
        GPIO.output(17, GPIO.HIGH)
        GPIO.output(27, GPIO.LOW)
        pi.hardware_PWM(gpio_pin0, freq, duty*100000)
        pi.hardware_PWM(gpio_pin1, freq, duty*100000)
        time.sleep(sleep_time)
        pi.hardware_PWM(gpio_pin0, freq, 000000)
        pi.hardware_PWM(gpio_pin1, freq, 000000)

    time.sleep(0.03)

解説

下のJoy-Conの写真で、左側のスティックが「hatスイッチ」、右側のボタンが「button」です。

青色のコントローラ(Joy-Con(L))だと、ボタンを押した時に入力される値は以下のような対応でした。

ボタン	値
SR	5
SL	4
↑	3
↓	0
←	2
→	1
○	13
ー	8
hat↑	0,1
hat↓	0,-1
hat←	-1,0
hat→	1,0
hat.def	0,0

hat.defは、hatスイッチのデフォルト状態の入力です。

最終的にですが、上のコントローラの写真のように、hatスイッチの前で「前進」、後ろで「後退」、十字キー(？）の左で「左旋回」、右で「右旋回」という操作仕様にしました。
最初に載せた動画の時には、もう少しアナログな操作感にしていたのですが、それでは子供が操作するには難しかったようで、このようなシンプル操作にしました。
上記のソースコードはこの「シンプル操作」版です。

    hat_LR,hat_FB = 0,0
    hat_back,hat_front = 0,0
    pygame.event.get()
    hat_LR,hat_FB = joys.get_hat(0)
    if hat_FB < -0.5:
        hat_back = 1
    elif hat_FB > 0.5:
        hat_front = 1

ここでやっているのは、
hat_LR,hat_FB = joys.get_hat(0) で、hatスイッチの入力を取得してhat_LR,hat_FBに格納。
hat_FBがマイナスだったら後退、プラスだったら前進と判定しています。

    button_front=joys.get_button(3)
    button_back=joys.get_button(0)
    button_right=joys.get_button(1)
    button_left=joys.get_button(2)

ここでは十字キーの入力を取得しています。

ここまでで取得した値に基づき、そのあとのif~elifでモーターに出力しています。

地味に苦労したのが、動作の「ヌルヌル感」です。
#モーター回転速度設定パラメータ
のところをいじれば調節できます。
ただ、一番効いたのは while の最後の time.sleep(0.03) だった気がします。

以上です。
おまけで、最初の動画の、アナログ操作感の強いコードも以下に載せておきます。
hatスイッチの前後で左モーター、十字キーの前後で右モーターを制御します。
また、十字キーの左でスピードダウン、右でスピードアップです。

import time
import pygame
import RPi.GPIO as GPIO
import pigpio

GPIO.setwarnings(False) 
GPIO.setmode(GPIO.BCM)
GPIO.setup(17, GPIO.OUT)
GPIO.setup(27, GPIO.OUT)

gpio_pin0 = 12
gpio_pin1 = 13

pi = pigpio.pi()
pi.set_mode(gpio_pin0, pigpio.OUTPUT)

duty = 5
freq = 500
sleep_time = 0.1
push = 0

pygame.init()
joys = pygame.joystick.Joystick(0)
joys.init()

while 1:
    hat_LR,hat_FB=0,0
    hat_back,hat_front=0,0
    pygame.event.get()
    hat_LR,hat_FB=joys.get_hat(0)
    if hat_FB < -0.5:
        hat_back = 1
    elif hat_FB > 0.5:
        hat_front = 1

    button_front=joys.get_button(3)
    button_back=joys.get_button(0)
    button_speedUP=joys.get_button(1)
    button_speedDOWN=joys.get_button(2)

    print('左前：'+str(hat_front)+', ' +'左後：'+str(hat_back)+', ' +'右前：'+str(button_front)+', ' +'右後：'+str(button_back)+', ' +'speed:' + str(duty-2))

    if button_speedUP == 1:
        push = push + 1
        if push > 5:
            duty = duty + 1
            if duty > 7:
                duty = 7
            push = 0

    elif button_speedDOWN == 1:
        push = push + 1
        if push > 5:
            duty = duty-1
            if duty < 3:
                duty = 3
            push = 0

    elif hat_front == 1 and button_front == 1:
        GPIO.output(17, GPIO.HIGH)
        GPIO.output(27, GPIO.HIGH)
        pi.hardware_PWM(gpio_pin0, freq, duty*100000)
        pi.hardware_PWM(gpio_pin1, freq, duty*100000)
        time.sleep(sleep_time)
        pi.hardware_PWM(gpio_pin0, freq, 000000)
        pi.hardware_PWM(gpio_pin1, freq, 000000)

    elif hat_back == 1 and button_back == 1:
        GPIO.output(17, GPIO.LOW)
        GPIO.output(27, GPIO.LOW)
        pi.hardware_PWM(gpio_pin0, freq, duty*100000)
        pi.hardware_PWM(gpio_pin1, freq, duty*100000)
        time.sleep(sleep_time)
        pi.hardware_PWM(gpio_pin0, freq, 000000)
        pi.hardware_PWM(gpio_pin1, freq, 000000)

    elif hat_front == 1 and button_back == 1:
        GPIO.output(17, GPIO.LOW)
        GPIO.output(27, GPIO.HIGH)
        pi.hardware_PWM(gpio_pin0, freq, duty*100000)
        pi.hardware_PWM(gpio_pin1, freq, duty*100000)
        time.sleep(sleep_time)
        pi.hardware_PWM(gpio_pin0, freq, 000000)
        pi.hardware_PWM(gpio_pin1, freq, 000000)

    elif hat_back == 1 and button_front == 1:
        GPIO.output(17, GPIO.HIGH)
        GPIO.output(27, GPIO.LOW)
        pi.hardware_PWM(gpio_pin0, freq, duty*100000)
        pi.hardware_PWM(gpio_pin1, freq, duty*100000)
        time.sleep(sleep_time)
        pi.hardware_PWM(gpio_pin0, freq, 000000)
        pi.hardware_PWM(gpio_pin1, freq, 000000)

    elif hat_front == 1:
        #GPIO.output(17, GPIO.HIGH)
        GPIO.output(27, GPIO.HIGH)
        #pi.hardware_PWM(gpio_pin0, freq, duty*100000)
        pi.hardware_PWM(gpio_pin1, freq, duty*100000)
        time.sleep(sleep_time)
        #pi.hardware_PWM(gpio_pin0, freq, 000000)
        pi.hardware_PWM(gpio_pin1, freq, 000000)

    elif button_front == 1:
        GPIO.output(17, GPIO.HIGH)
        #GPIO.output(17, GPIO.HIGH)
        pi.hardware_PWM(gpio_pin0, freq, duty*100000)
        #pi.hardware_PWM(gpio_pin1, freq, duty*100000)
        time.sleep(sleep_time)
        pi.hardware_PWM(gpio_pin0, freq, 000000)
        #pi.hardware_PWM(gpio_pin1, freq, 000000)

    elif hat_back == 1:
        #GPIO.output(17, GPIO.HIGH)
        GPIO.output(27, GPIO.LOW)
        #pi.hardware_PWM(gpio_pin0, freq, duty*100000)
        pi.hardware_PWM(gpio_pin1, freq, duty*100000)
        time.sleep(sleep_time)
        #pi.hardware_PWM(gpio_pin0, freq, 000000)
        pi.hardware_PWM(gpio_pin1, freq, 000000)

    elif button_back == 1:
        GPIO.output(17, GPIO.LOW)
        #GPIO.output(17, GPIO.HIGH)
        pi.hardware_PWM(gpio_pin0, freq, duty*100000)
        #pi.hardware_PWM(gpio_pin1, freq, duty*100000)
        time.sleep(sleep_time)
        pi.hardware_PWM(gpio_pin0, freq, 000000)
        #pi.hardware_PWM(gpio_pin1, freq, 000000)

    time.sleep(0.03)