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

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()