wall follower-코드 정리

제어 코드

#!/usr/bin/env pybricks-micropython
from pybricks.hubs import EV3Brick
from pybricks.ev3devices import (Motor, TouchSensor, ColorSensor,
                                 InfraredSensor, UltrasonicSensor, GyroSensor)
from pybricks.parameters import Port, Stop, Direction, Button, Color
from pybricks.tools import wait, StopWatch, DataLog
from pybricks.robotics import DriveBase
from pybricks.media.ev3dev import SoundFile, ImageFile
import math

# PORT declare
LEFT_MOTOR = Motor(Port.A)
RIGHT_MOTOR = Motor(Port.B)
#LEFT_MOTOR = Motor(Port.B)
#RIGHT_MOTOR = Motor(Port.C)
ULTRA = UltrasonicSensor(Port.S1)

# initialize the angle
RIGHT_MOTOR.reset_angle(0)
LEFT_MOTOR.reset_angle(0)

# PID parameters
Kp = 0.13
Ki = 0.001
Kd = 0.000

target = 150
error = 0.0
last_error = 0.0
acc_error = 0.0
dt = 0.004

vel = 180

count = 0


while True:
    dist = ULTRA.distance()

    # Filtering!

    if dist < 600 and dist >= 250:
        dist = math.sqrt(dist)


    # PID calculation
    error = target - dist
    diff = error - last_error
    acc_error += error
    P_control = Kp * error
    I_control = Ki * acc_error *dt
    D_control = Kd * diff / dt
    control = P_control + I_control + D_control

    # Situation: too far
    if dist > 600 :
        LEFT_MOTOR.run(vel)
        RIGHT_MOTOR.run(vel + 60)

    # Situation: normal
    else:
        LEFT_MOTOR.run(vel + control)
        RIGHT_MOTOR.run(vel - control)

    last_error = error
    # Calculate Time gap
    #end_time = StopWatch.time()
    #end_time = time.time()
    #end_time = time.time_ns() // 1000000
    #time_gap = end_time - start_time

    # Data record
    print("%d " %ULTRA.distance())
    #print("T: %d " %time_gap)

    # Quit when time over
    # ms : 1000ms == 1s
    #if time_gap > 20:
    #   print("Time Over!")
    #    break


    #wait(dt*100)
    count += 1
    #if count > 40:
    #   print("Time Over!")
    #   break

제어 코드-클래스 사용

#!/usr/bin/env pybricks-micropython
from pybricks.hubs import EV3Brick
from pybricks.ev3devices import (Motor, TouchSensor, ColorSensor,
                                 InfraredSensor, UltrasonicSensor, GyroSensor)
from pybricks.parameters import Port, Stop, Direction, Button, Color
from pybricks.tools import wait, StopWatch, DataLog
from pybricks.robotics import DriveBase
from pybricks.media.ev3dev import SoundFile, ImageFile
import time


class PID_ctrl:
    def __init__(self):
        # PID parameters
        self.Kp = 0.13
        self.Ki = 0.000
        self.Kd = 0.000

        self.target = 150
        self.error = 0.0
        self.last_error = 0.0
        self.acc_error = 0.0
        self.dt = 0.004

        self.vel = 180

        # time
        self.current_time= StopWatch.time()

        self.start_time = self.current_time

    def run(self, ULTRA, RIGHT_MOTOR, LEFT_MOTOR):
        while True:
            dist = ULTRA.distance()

            # Filtering!
            if dist > 600:
                dist = self.target

            # PID calculation
            self.error = self.target - self.dist
            self.diff = self.error - self.last_error
            self.acc_error += self.error
            P_control = self.Kp * self.error
            I_control = self.Ki * self.acc_error *self.dt
            D_control = self.Kd * self.diff / self.dt

            control = P_control + I_control + D_control

            # Situation: too far
            if dist > 600:
                LEFT_MOTOR.run(self.vel)
                RIGHT_MOTOR.run(self.vel + 30)

            # Situation: normal
            else:
                LEFT_MOTOR.run(self.vel + control)
                RIGHT_MOTOR.run(self.vel - control)

            self.last_error = self.error

            # Calculate Time gap
            end_time = StopWatch.time()
            time_gap = end_time - self.start_time

            # Display data
            print("%d " %ULTRA.distance())
            print("T: %d " %time_gap())

            # Quit when time over
            if time_gap > 20*1000:
                print("Time Over!")
                break

            #wait(dt*100)


if __name__ == "__main__":
    # PORT declare
    #LEFT_MOTOR = Motor(Port.A)
    #RIGHT_MOTOR = Motor(Port.B)
    LEFT_MOTOR = Motor(Port.B)
    RIGHT_MOTOR = Motor(Port.C)
    ULTRA = UltrasonicSensor(Port.S1)

    # initialize the angle
    RIGHT_MOTOR.reset_angle(0)
    LEFT_MOTOR.reset_angle(0)

    # Run
    PC = PID_ctrl()
    PC.run(ULTRA, RIGHT_MOTOR, LEFT_MOTOR)

시험용 코드

#!/usr/bin/env pybricks-micropython
import time

# PID parameters
Kp = 0.13
Ki = 0.000
Kd = 0.000

target = 150
error = 0.0
last_error = 0.0
acc_error = 0.0
dt = 0.004

vel = 180


# time
current_time= time.time()
start_time = current_time


while True:
    dist = int(input())

    # Filtering!
    if dist > 600:
        dist = 0

    # PID calculation
    error = target - dist
    diff = error - last_error
    acc_error += error
    P_control = Kp * error
    I_control = Ki * acc_error *dt
    D_control = Kd * diff / dt
    control = P_control + I_control + D_control

    # Data record
    print("%d " %dist)
    print("%d " %time.time())

    # too far
    if dist == 0:
        print("####left: %d" %vel)
        print("####right: %d" %(vel+30))

    # normal situation
    else:
        print("##left: %d" %(vel+control))
        print("##right: %d" %(vel-control))

    last_error = error

    end_time = time.time()
    time_gap = end_time - start_time

    if time_gap > 10:
        print("Time Over")
        break


    #wait(dt*100)

Uploaded by N2T