移動するだけのTANK

ゲームパッドの入力を読んで、ただ移動するだけのプログラムがこちらです。

操縦方法は、ハットスイッチで移動方向を指示し、LT(アナログ出力)をアクセルとして、モーターを回します。詳しい挙動は以下の通り。

ハットスイッチ挙動
前進
右上右旋回
左上左旋回
右回転
左回転
右下右後方旋回(小半径)
左下左後方旋回(小半径)
後退
import pygame
import Adafruit_PCA9685

# pygame初期設定
pygame.init()
joy = pygame.joystick.Joystick(0) # joystickのインスタンスを作成
joy.init() # joyの初期化

# PCA9685初期設定
pwm = Adafruit_PCA9685.PCA9685() # PCA9685のインスタンスを作成
pwm.set_pwm_freq(60) # PWMの周波数を60Hzに設定

motor = [0,0,0,0] # pwm各チャンネルに送るデータの格納場所を作成

while True:
    # ゲームパッドの入力があった場合
    if pygame.event.get():
        
        Acc = int((joy.get_axis(5) + 1)*2048) # RTの入力(-1 ~ 1)を0~4095に増幅
        hat = 2 + joy.get_hat(0)[0]+(joy.get_hat(0)[1] + 1) * 3 # hatを10キーの値に変換
        motor[0] = (hat == 8) + (hat == 7) + (hat == 4) + (hat == 9) * 0.3 # 右側正回転
        motor[1] = (hat == 2) + (hat == 1) + (hat == 6) # 右側逆回転
        motor[2] = (hat == 8) + (hat == 9) + (hat == 6) + (hat == 7) * 0.3 # 左側正回転
        motor[3] = (hat == 2) + (hat == 3) + (hat == 4) # 左側逆回転
        
        print(Acc,hat,motor); # 値の確認
        
        for n in range(4): # PWMの設定
            pwm.set_pwm(n, 0, int(motor[n]*Acc))

PCA9685は今の所、以下の通りL9110S(モータードライバー)にしか繋げていません。

0chA1:右側モーター正回転
1chA2:右側モーター逆回転
2chB1:左側モーター正回転
3chB2:左側モーター逆回転
PCA9685の出力

このプログラムで、ちょっと説明が必要なのが、アナログ入力軸は-1〜1(正確には0.999969482421875)の値を取るのですが、初期化すると値が0にリセットされます。従って、プログラムを走らせるとAccが2048になります。しかしトリガーの物理的な位置が-1なので、プログラムを走らせたら、ハットスイッチを触る前にまずLTを一度触って、Accを0にします。

それから、プログラム中にある (hat == 8) は論理式です。hatが8なら (hat ==8) は1になり、hatが8以外なら0になります。論理式を使うと、4つのpwm値を決めるのを4行で書けてしまうのです。素晴らしいですね。これをif文でやったら大変な事になります。

この移動だけするTANKで、気になって近寄ってきた猫を追いかけてみました。

コメント

タイトルとURLをコピーしました