【Raspberry Pi Pico】自動運転ロボットカーの製作 ②回路・プログラム編

【Raspberry Pi Pico】自動運転ロボットカーの製作 ②回路・プログラム編

こんにちは! けい(Twitter)です。

今回は、Raspberry Pi Pico(ラズピコ)で自動運転ロボットカーを作るの第二弾ということで、前回はロボットの組み立てを行ったので、今回は回路とプログラムを紹介していきたいと思います。

前回の記事を読んでいない方はぜひ読んでみて下さい。

回路構成

全ての回路図を一つの画像にすると分かりにくいので、電源・モーター系距離測定系の2部構成にします。

電源・モーター系

全体像はこんな感じにしました。
画像では電池2個ですが、実際には3個使っています。

電源は単三3本

モーターの電源と、ラズピコの電源を全て電池の4.5Vで供給しています。

ラズピコへの外部電源での動作は以前の記事にまとめた通り、1.8V~5.5Vの電源を外部電源として使用できます。

そこで、今回は超音波センサー、サーボモーター、モータードライバの電源として全て使用できる4.5Vの電源(単三電池3本)を使用しました。

モーター系

モーターは2つのモーターを制御したいので、それができるのにぴったりのモータードライバを使用しました。Hブリッジ回路を2つ持っているなら何でも大丈夫です。
1つのモーターを制御するのに、2本の信号線を使用します。

モータードライバの使い方については以前の記事でまとめているので、参考にしてみて下さい。

タミヤのダブルギヤボックスに使われているモーターの定格が3Vなのですが、9Vで使用している方もいるみたいなので、4.5Vでも故障することはないと思われます、、

距離測定系

サーボモーターと超音波センサーで距離を測定します。
電源・モーター系と回路は共有しています。

サーボモーター

サーボモーター(sg90)の電源は3.3~6Vを使用できるので、4.5Vでも問題ないです。(データシート)

また、サーボモーターへの信号線としてpwm信号が出せるピンを使用します。今回はGP5を使用しています。

超音波センサー

超音波センサー(HC-SR504)の電源は5Vですが、4.5Vでも問題なく動作しました。

ラズピコのoutputに超音波センサーのTrigピンを、ラズピコのinputに超音波センサーのEchoピンを接続します。
ここで、Echoピンからの入力を直接ラズピコへ入力するとラズピコが壊れる可能性があります。
というのも、ラズピコは3.3V系なので5V系である超音波センサーの信号のレベルを下げてあげる必要があります。

そこで、抵抗による分圧を行い3.3Vに近くなるようにしています。
詳しくは以前の記事で解説しています。

プログラム

ソースコード

main.pyで保存します。main.pyで保存すれば、電源が入ると自動でプログラムが実行されます。

from machine import PWM, Pin
import utime

class Robot:
    def __init__(self):
        self.IN1 = Pin(1, Pin.OUT)
        self.IN2 = Pin(2, Pin.OUT)
        self.IN3 = Pin(3, Pin.OUT)
        self.IN4 = Pin(4, Pin.OUT) 
    def forward(self):
        self.IN1.value(1)
        self.IN2.value(0)
        self.IN3.value(1)
        self.IN4.value(0)
    def right(self):
        self.IN1.value(1)
        self.IN2.value(1)
        self.IN3.value(1)
        self.IN4.value(0)
    def left(self):
        self.IN1.value(1)
        self.IN2.value(0)
        self.IN3.value(1)
        self.IN4.value(1)
    def back(self):
        self.IN1.value(0)
        self.IN2.value(1)
        self.IN3.value(0)
        self.IN4.value(1)
    def stop(self):
        self.IN1.value(1)
        self.IN2.value(1)
        self.IN3.value(1)
        self.IN4.value(1)    
    def release(self):
        self.IN1.value(0)
        self.IN2.value(0)
        self.IN3.value(0)
        self.IN4.value(0)
        
class Distance:
    def __init__(self):
        self.trigger = Pin(14, Pin.OUT)
        self.echo = Pin(15, Pin.IN)
        self.servo = PWM(Pin(5))
        self.servo.freq(50)
        self.deg = {"-90":0.025, "-60":0.040833, "-45":0.04875, "-30":0.056666, "-22.5":0.060625, "0":0.0725, "22.5":0.084375, "30":0.088333, "45":0.09625, "60":0.104166, "90":0.12}
        self.max_duty = 65025
        self.d = [0]*5
        self.angle = [-60,-30,0,30,60]
        
    def change_angle(self, ang):
        self.servo.duty_u16(int(self.max_duty*self.deg[str(ang)]))
        
    def read_distance(self):
        self.trigger.low()
        utime.sleep_us(2)
        self.trigger.high()
        utime.sleep(0.00001)
        self.trigger.low()
        while self.echo.value() == 0:
            self.signaloff = utime.ticks_us()
        while self.echo.value() == 1:
            self.signalon = utime.ticks_us()
        self.timepassed = self.signalon - self.signaloff
        self.distance = (self.timepassed * 0.0343) / 2
        return self.distance
    
    def read_5angle(self):
        for i in range(5):
            self.change_angle(self.angle[i])
            if i==0:
                utime.sleep(0.5)
            utime.sleep(0.2)
            self.d[i] = self.read_distance()
        return self.d
    
robot = Robot()
distance = Distance()
utime.sleep(1)

while True:
    d = distance.read_5angle()
    print(d)
    if (d[2]>20) & (d[1]>10) &(d[3]>10):
        robot.forward()
    else:
        robot.stop()
        utime.sleep(0.5)
        d = distance.read_5angle()
        min_idx = d.index(min(d))
        if d[2] <= 8:
                robot.back()
                utime.sleep(1)
                robot.stop()
        # 前方に障害物
        elif min_idx == 2:
            if (d[0]+d[1]) >= (d[3]+d[4]):
                robot.right()
                utime.sleep(1)
                robot.stop()
            else:
                robot.left()
                utime.sleep(1)
                robot.stop()
        # 右側に障害物
        elif min_idx <= 1:
            robot.left()
            utime.sleep(0.5)
            robot.stop()
        # 左側に障害物
        elif min_idx >= 3:
            robot.right()
            utime.sleep(0.5)
            robot.stop()

プログラム解説

モーター駆動系(Robotクラス)

モーターの制御に関しては、4~39行目のRobotクラスにまとめています。

4つのgpioピンでロボットの前進・後進・回転・停止を制御しています。

一つのモーターに注目すると、IN1,IN2でOUT1,OUT2、つまり右側のモーターを制御しています。
IN1、IN2にかける電圧をLOWかHIGHかにすることで、一つのモーターの制御を行います。
これを2つのモーターで行うことで、2つ同時に正転すれば前進、2つ同時に逆転すれば後進というようにロボット全体の動作を制御しています。

IN1IN2モーターにかかる力
LOWLOW
HIGHLOW正転
LOWHIGH逆転
HIGHHIGHブレーキ

距離測定系(Distanceクラス)

対象物との距離を測定するプログラムは、41~76行目にまとめています。

52行目change_angle関数でサーボモーターの角度を決めています。
サーボモーターはpwmのデューティー比を変更することで角度を変化できるので、予めサーボの角度に対するデューティー比を47行目の辞書として設定しておきます。
ちなみにデューティー比はデータシートから決めています。

55行目read_distance関数が超音波センサーで実際に距離を測定するプログラムです。
ここは以前の記事から借用したので、そちらを参考にしてください。

69行目read_5angle関数で5つの角度で距離を取得しています。
change_angle関数とread_distance関数を利用して、パッケージにしています。
返り値として5つの要素を持ったリストを返します。

メインプログラム

5つの角度での距離情報を元にロボットを前進させるか、回転させるかなどを判断しています。

ここはお使いの環境や、意図によって変わってくるので参考程度にしてください。
試行錯誤を繰り返してこのプログラムに落ち着きましたが、まだ満足しているわけではありません。

また、今回のプログラムでは回転する際に、片方のモーターが前進し、もう片方のモーターが停止しているため、普通の車のような曲がり方をします。しかし、キャタピラを使用しているため、戦車のように超信地旋回が可能です。
それは実装していないので、ぜひご自身で挑戦してみてください。

実行結果