【Raspberry Pi Pico】ロボット制御入門④～ライントレース～【Python】

from machine import Pin, I2C
import ssd1306
import time

class lcd_display:
    def __init__(self):
        # I2C通信の初期化
        self.i2c = I2C(0, sda=Pin(20), scl=Pin(21), freq=400000)

        # 接続されているI2Cデバイスをスキャン
        addr = self.i2c.scan()
        if not addr:
            raise ValueError(f"I2Cデバイスが見つかりません。接続を確認してください。指定されたアドレス: {hex(address)}")

        # OLEDディスプレイの初期化
        try:
            self.oled = ssd1306.SSD1306_I2C(128, 64, self.i2c, addr=0x3C)
        except Exception as e:
            raise RuntimeError(f"OLEDの初期化に失敗しました: {e}")

        # 初期化時に画面をクリア
        self.clear()

    def clear(self, sleep_time=1):
        """OLEDディスプレイをクリアします。"""
        time.sleep(sleep_time)
        self.oled.fill(0)
        self.oled.show()

from machine import Pin, ADC, I2C
import ssd1306
import time
from lcd_library import lcd_display

lcd_instance = lcd_display()

led = Pin("LED", Pin.OUT)  # LEDのピン設定
photoref_left = ADC(1)  # ADC0を使う. （）内はGPIOの番号ではない
photoref_right = ADC(0)  # ADC1を使う. （）内はGPIOの番号ではない

# LEDの状態を1（オン）に初期化
led.value(1)

def read_sensor():
    # センサー下の明度をパーセント表示する
    photoref_out_left = photoref_left.read_u16() / 65535 # 0〜1の範囲に正規化
    photoref_out_right = photoref_right.read_u16() / 65535 # 0〜1の範囲に正規化

    # タプル形式で値をまとめて返す
    return (photoref_out_left, photoref_out_right)

try:
    while True:
        lcd_instance.clear(0.5)
        data = read_sensor()  # センサーの値を読み取る
        lcd_instance.oled.text('left :' + str(data[0]), 0, 16)  # センサーの値を位置(0, 16)に表示
        lcd_instance.oled.text('right:' + str(data[1]), 0, 32)  # センサーの値を位置(0, 32)に表示
        lcd_instance.oled.show() # ディスプレイを更新する関数
        time.sleep(0.05)  # 50ミリ秒待機
finally:
    # OLEDディスプレイを黒で塗りつぶす
    lcd_instance.oled.fill(0)
    lcd_instance.oled.show()

from machine import Pin, ADC
import utime

led = Pin("LED", Pin.OUT)
# LED の状態を 1 (オン) に初期化します. 
led.value(1)
trigger = Pin(5, Pin.OUT)  # GPIO 5番ピンをトリガーとして設定
echo = Pin(4, Pin.IN)  # GPIO 4番ピンをエコーとして設定

# パーセンテージの閾値
Thr_num_left = 0.55
Thr_num_right = 0.55

# ピンの入出力設定
photoref_left = ADC(1)  # ADC0を使う. （）内はGPIOの番号ではない
photoref_right = ADC(0)  # ADC1を使う. （）内はGPIOの番号ではない
# モーター用のピンの入出力設定
MotorL_plus = Pin(0, Pin.OUT)
MotorL_minus = Pin(1, Pin.OUT)
MotorR_plus = Pin(2, Pin.OUT)
MotorR_minus = Pin(3, Pin.OUT)

# センサーが取得する明るさ（＝明度）を読み取る
def read_sensor():
    # センサー下の明度をパーセント表示する
    photoref_out_left = photoref_left.read_u16() / 65535 # 0〜1の範囲に正規化
    photoref_out_right = photoref_right.read_u16() / 65535 # 0〜1の範囲に正規化

    # タプル形式で値をまとめて返す
    return (photoref_out_left, photoref_out_right)
    
# 1秒停止
# 左側のモーターを前進させる関数
def left_go():
    MotorL_plus.value(1)
    MotorL_minus.value(0)

# 右側のモーターを前進させる関数
def right_go():
    MotorR_plus.value(1)
    MotorR_minus.value(0)

# 左側のモーターを後退させる関数
def left_back():
    MotorL_plus.value(0)
    MotorL_minus.value(1)

# 右側のモーターを後退させる関数
def right_back():
    MotorR_plus.value(0)
    MotorR_minus.value(1)

# モーターを停止する関数
def stop():
    MotorL_plus.value(1)
    MotorL_minus.value(1)
    MotorR_plus.value(1)
    MotorR_minus.value(1)
    
def read_distance():
    trigger.low()  # トリガーをLowにして準備
    utime.sleep_us(2)  # 2マイクロ秒待機
    trigger.high()  # トリガーをHighにして超音波を発信
    utime.sleep(0.00001)  # 10マイクロ秒間超音波を発信
    trigger.low()  # 再度トリガーをLowにする
    
    # エコーが反応を返すまでの時間を計測
    while echo.value() == 0:  # エコー信号が来るのを待つ
        signaloff = utime.ticks_us()  # 反応なしの時間を記録
    
    while echo.value() == 1:  # エコー信号が来るのを待つ
        signalon = utime.ticks_us()  # 反応が返ってきた時間を記録

    # 反応時間を計算し、距離を求める
    timepassed = float(signalon) - float(signaloff)
    distance = (timepassed * 0.0343) / 2  # 距離を計算（音速は約0.0343 cm/μs、片道なので2で割る）
    
    # 測定した距離を表示
    
    return distance

try:
    while True:
        utime.sleep(0.1)  # 0.1秒待機
        data = read_sensor()  # 照度センサーの値を取得
        distance1=read_distance()
        print(data[0], data[1],distance1)  # センサーの値を確認

        # 左のセンサーが閾値より低い（黒を認識）場合
        if data[0] <= Thr_num_left:
            left_go()  # 左のタイヤを前進
        else:
            left_back()  # 左のタイヤを後退

        # 右のセンサーが閾値より低い（黒を認識）場合
        if data[1] <= Thr_num_right:
            right_go()  # 右のタイヤを前進
        else:
            right_back()  # 右のタイヤを後退
        
        if distance1 <= 10:
            stop()


# プログラムが終了する際にモーターを停止
finally:
    stop()