数日前に記事を書いた、移動するだけのTANKから、ギア比を大きくして速度を落とし、パワーを上げてみました。結果として、カーペットの上でも旋回するようになったのですが、ネコを追いかけるには全くスピードが足りなくなりました。ま、それは良いとして、早速自動ブレーキの実装をしていきたいと思います。
基本的な考え方
- 前方40cm以内に障害物を検知したら、モーター正回転のpwmを落とす（0.3倍にする）
- 前方20cm以内に障害物を検知したら、モーター正回転を禁止する。
この二つの動作を実装してみます。
また、モニターを見なくても挙動が分かるように、LEDを1つ繋ぎ、その明るさで40cm以内に障害物があるときは、暗く点灯し、20cm以内に障害物があるときはフル点灯させます。
このLED制御は、当然ながらPCA9685（4ch）を使う事にします。ラズパイでLチカをやる際には、ブレッドボードにLEDと抵抗を直列に繋ぐのですが、PCA9685を使うと、内部に既にLED用の抵抗が入っているので、pwmピンとGNDピンにLEDをダイレクトに繋いでOKなので楽ちんです。
改造したプログラム
前回の移動するだけのプログラムは、ゲームパッドの入力にだけ従っていれば良かったので、ゲームパッドの入力があった場合のみpwm信号を送れば良かったのですが、自動ブレーキ実装となると、障害物検知でもpwm信号を送らなければいけないので、pwm信号を送る命令を一段外に置かなければいけません。
前回のプログラムがこちら。
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))自動ブレーキを実装したプログラムがこちら。
import pygame
import Adafruit_PCA9685
from gpiozero import DistanceSensor
# 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に設定
# 距離センサーの初期設定
sensor = DistanceSensor(23, 24) # 23:Echo 24:Trig
motor = [0,0,0,0] # pwm各チャンネルに送るデータの格納場所を作成
Acc = 0 # Accを定義しておく
while True:
# 距離センサーの値を取得
dist = round(sensor.distance*100,1)
# ゲームパッドの入力があった場合
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.2 ) # 右側正回転
motor[1] = (hat == 2) + (hat == 1) + (hat == 6) # 右側逆回転
motor[2] = ((hat == 8) + (hat == 9) + (hat == 6) + (hat==7) * 0.2 ) # 左側正回転
motor[3] = (hat == 2) + (hat == 3) + (hat == 4) # 左側逆回転
# PWM送信
for n in range(2): # 前進用PWMの設定
# 40cm以上あればsafe=1, 20cm〜40cmならsafe=0.3, 20cm以下ならsafe=0
safe = (dist >= 40) + (dist<40 and dist>20)*0.3
# tale lampの輝度調整 20cm未満ならFull点灯、20cm以上40cm未満ならハーフ点灯
tale = (dist < 20) * 3500 + (dist < 40) * 595
ch=n*2
pwm.set_pwm(ch, 0, int(motor[ch]*Acc*safe))
for n in range(2): # 後進用PWMの設定
ch=n*2+1
pwm.set_pwm(ch, 0, int(motor[ch]*Acc))
pwm.set_pwm(4, 0, tale) # テールランプの輝度設定
print("前方距離",dist,"cm")ちょっと解説すると、pwm送信を while True: 直下に置いているので、Accを定義しておかないとエラーになっちゃいます。従って、ループに入る前にAccを定義しました。
ループに入ると、まず変数 dist に距離センサーの値を小数第二位を四捨五入して代入します。
次にゲームパッドの入力チェックをしますが、何も入力がなければ、そのまま次のpwm送信になります。
pwm送信の前に、論理式を使って前方クリアなら1を、直前に障害物なら0を、近くに障害物なら、0.3を safe という変数に代入します。
taleは、テールランプLEDの輝度です。
そして、正回転と逆回転を分けて、pwm送信します。
最後に、テールランプのpwmを送信します。
テールランプが正しく機能しているかを確認した動画がこちら。
動画でLEDが点滅しているように見えるのは、フリッカー現象です。実際は点滅していません。
問題発生・・・。
さて、正常に動作しているように見えますが、これじゃダメでした。
実際に走らせてみたら、壁に激突しまくりでした。そうなんです。距離センサーの挙動が遅いのです。
これ、どうにかならないものかと、gpiozeroの距離センサーのマニュアルを見に行きました。
そしたら色々書いてあるじゃないですか！トリガーとエコー以外にもパラメーターを色々と設定出来るようでした。
| echo | エコーを入れるGPIOピン |
| trigger | トリガーを出すGPIOピン |
| queue_len | キューの長さ。これが大きいと値が正確になる代わりに、反応が遅くなり、これが小さいと反応が早くなる代わりに、値がばらつく。初期値は9 |
| max_distance | 検知する最大距離。初期値は1m |
| threshold_distance | スレッショルド距離。初期値は0.3m。この距離より近づくと、in_rangeイベントを発生させ、この距離より離れると、out_rangeイベントを発生させる。 |
| partial | 初期値はFalse。これをTrueにすると、内部キューが一杯にならなくても値を取り出せる。すぐ値が欲しい時のみTrueにする。 |
| pin_factory | 値は、FactoryかNoneのみ。Api-pinsを参照のこと。 |
ということで、とりあえず上のプログラムの15行目を次のように変えてみたところ、反応速度がだいぶ改善しました。
sensor = DistanceSensor(echo=23, trigger=24, quueue_len=5)5より小さくすると不安定になるし、5より大きくすると反応が若干遅く感じるので、5が良いみたいです。
実際に走らせてみたのが次の動画です。
これで、壁に激突しなくなりましたが、音を反射しにくい柔らかい素材だとまぁ激突するんですけどね。
コメント