Line Tracer-코드 정리

제어 코드

#!/usr/bin/env pybricks-micropython

from pybricks.hubs import EV3Brick
from pybricks.ev3devices import Motor,ColorSensor,InfraredSensor
from pybricks.robotics import DriveBase
from pybricks.parameters import Port, Stop, Direction, Button, Color
from pybricks.tools import wait, StopWatch, DataLog

# Initialize the motors.
L_motor = Motor(Port.A)
R_motor = Motor(Port.B)

# Initialize the color sensor.
line_sensor = ColorSensor(Port.S1)

# Initialize the drive base.
wheel_diameter=55.5
axle_track=104
robot = DriveBase(L_motor, R_motor, wheel_diameter, axle_track)

### Calculate the light threshold. Choose values based on your measurements.
BLACK = 6  #6
WHITE = 58  # 55~64
# threshold ~~ target
threshold = (BLACK + WHITE) / 2.0

# Set the drive speed at 100 millimeters per second.
DRIVE_SPEED = 180

# Set the gain of the proportional line controller. This means that for every
# percentage point of light deviating from the threshold, we set the turn
# rate of the drivebase to 1.2 degrees per second.

# For example, if the light value deviates from the threshold by 10, the robot
# steers at 10*1.2 = 12 degrees per second.

# PID parameters
Kp = 3 #1.7 2.2 & threshold 2.0 2.8580
Ki = 0.04
Kd = 0.04
print("Kp= %f" %Kp)
print("Ki= %f" %Ki)
print("Kd= %f" %Kd)

deviation = 0.0
last_deviation = 0.0
acc_deviation = 0.0
dt = 0.05

count = 0



# Start following the line endlessly.
while True:
    # Calculate the deviation from the threshold.
    deviation = line_sensor.reflection() - threshold

    #print("Color_reflection: ",line_sensor.reflection())
    print("%f" %deviation)
    print("T= %d" %count)

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


    # Calculate the turn rate.
    #turn_rate = Kp * deviation
    turn_rate = control

    # Set the drive base speed and turn rate.
    robot.drive(DRIVE_SPEED, turn_rate)

    count += 1
    # You can wait for a short time or do other things in this loop.
    wait(dt*10)

시험용 코드

#!/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