[라즈베리파이 + DC모터 + PYTHON] 3. 라즈베리파이로 엔코더DC모터 PID 위치제어하기

알도 2017-11-15 (수) 17:23 7년전 8677

목록 글쓰기

라즈베리파이 DC모터 드라이버 PYTHON 엔코더 홀센서 PID제어

안녕하세요.

[라즈베리파이 + DC모터 + PYTHON] 1. 라즈베리파이와 MD10C로 DC모터 제어하기와

[라즈베리파이 + DC모터 + PYTHON] 1. 라즈베리파이로 엔코더 읽기에 이어서 하는 강좌입니다.

지난번에 아두이노, DC모터 드라이버, 엔코더를 이용해서 위치제어하는 방법을 알아봤는데요.

[아두이노 + DC모터]1. L298로 DC모터 제어하기

[아두이노 + DC모터]2. DC모터 엔코더 사용하기
[아두이노 + DC모터]3. 엔코더를 이용한 정밀한 모터 위치 제어

이번에는 아두이노를 라즈베리파이로 바꿔서 해보겠습니다.

이론적인 부분은 위에 아두이노 강좌를 참고하시길 바랍니다.

지난 두 강좌에서 라즈베리파이로 DC모터를 제어하고 엔코더값을 읽어오는 방법을 알아봤습니다.

오늘은 이 두가지를 합쳐서 위치제어를 해보도록 하겠습니다.

회로구성은 아래와 같습니다.

계속해서 말씀드리는 내용이지만, 라즈베리파이는 별도의 PWM 출력과 인터럽트 입력을 받는 하드웨어 없이 소프트웨어로만 구현하기 때문에 핀은 코드와 일치하기만 하면 원하시는대로 변경하셔도 상관없습니다.

코드는 다음와 같습니다.

길이가 좀 있으니 2부분으로 나누겠습니다. 먼저 아래는 여러 기본세팅입니다. 이전 강좌 내용을 합친 것이니 패스하겠습니다.

import RPi.GPIO as IO
import time

pwmPin = 19
dirPin = 13
encPinA = 23
encPinB = 24

IO.setmode(IO.BCM)
IO.setwarnings(False)
IO.setup(encPinA, IO.IN, pull_up_down=IO.PUD_UP)
IO.setup(encPinB, IO.IN, pull_up_down=IO.PUD_UP)
IO.setup(pwmPin,IO.OUT)
IO.setup(dirPin,IO.OUT)

p = IO.PWM(19,100)
p.start(0)

encoderPos = 0

def encoderA(channel):
    global encoderPos
    if IO.input(encPinA) == IO.input(encPinB):
        encoderPos += 1
    else:
        encoderPos -= 1

def encoderB(channel):
    global encoderPos
    if IO.input(encPinA) == IO.input(encPinB):
        encoderPos -= 1
    else:
        encoderPos += 1

IO.add_event_detect(encPinA, IO.BOTH, callback=encoderA)
IO.add_event_detect(encPinB, IO.BOTH, callback=encoderB)

여기서부터는 PID가 들어간 핵심코드입니다.

PID에 대해서는 코드 밑에서 설명하겠습니다.

targetDeg= 360.
ratio = 360./270./64.
Kp = 1000.
Kd = 0.
Ki = 0.
dt = 0.
dt_sleep = 0.01
tolerance = 0.01

start_time = time.time()
error_prev = 0.
time_prev = 0.

while True:
motorDeg = encoderPos * ratio

    error = targetDeg - motorDeg
    de = error-error_prev
    dt = time.time() - time_prev
    control = Kp*error + Kd*de/dt + Ki*error*dt;

    error_prev = error
    time_prev = time.time()

    IO.output(dirPin, control >= 0)
    p.ChangeDutyCycle(min(abs(control), 100))

    print('P-term = %7.1f, D-term = %7.1f, I-term = %7.1f' %(Kp*error, Kd*de/dt, Ki*de*dt))
    print('time = %6.3f, enc = %d, deg = %5.1f, err = %5.1f, ctrl = %7.1f' %(time.time()-start_time, encoderPos, motorDeg, error, control))
    print('%f, %f' %(de, dt))

    if abs(error) <= tolerance:
        IO.output(dirPin, control >= 0)
        p.ChangeDutyCycle(0)
        break

    time.sleep(dt_sleep)