MD ROBOT BLDC 모터 파이썬으로 제어하기

네트워크

MD ROBOT BLDC 모터 파이썬으로 제어하기

sstonne 2024. 9. 20. 15:16

지난 글에서는 MODBUS 통신, RS232, RS485, COM통신에 대해서 간단히 알아보았는데요, 이번엔 실습으로 돌아왔습니다.

MD로봇에서 제공하는 제어기 통신 사양서를 참고하여 MD Robot사의 MD100 제어기를 작동시키기 위해 Tkinter을 사용한 간단한 파이썬 통신 프로그램을 만들었습니다.

MD 모터 드라이버 통신 사양에 나와있는 모든 기능을 구현하지는 못했지만 핵심 기능을 몇가지 구현하여 참고하여 사용할 수 있도록 구성했습니다.

MD ROBOT사에서 기존 통신 코드는 C언어로 제공되기 때문에 파이썬 사용자들을 위한 간단한 코드 예시라고 봐주시면 될 것 같습니다.

제어기 사양

제어기	전압(Volt)	전류(A)	RS485	TTL232	CAN	ENC	PULSE_IN	CLUTCH	RJ45	POW_SW
MD100	DC12~36	7	○		○	○	○

통신 패킷의 구조

Header		ID	Parameter ID	Data nubmer	Data	Check sum
RMID	TMID	ID	PID	DataNumber	DATA	CHK
1 byte	1 byte	1 byte	1 byte	1 byte	1~n bytes	1 byte

RMID(Receiving Machine ID) : 패킷의 첫번째 인식바이트(183, 모터제어기)

TMID(Transmitting Machine ID) : 패킷의 두번째 인식바이트(TMID, 사용자 제어기)

ID : 각제어기의 ID(0~253, Broadcasting ID: 254)

PID : Parameter IDentification number

CHK : Check Sum

우선 COM 포트로 설정된 USB 장치를 PC에 연결해 주세요.

1. 가장 먼저 필요한 라이브러리를 설치해줍니다.

pip install pyserial numpy matplotlib

2. PC에 USB 연결 후, COM포트로 인식이 되는지 확인해줍니다.

#Available serial ports list
ports = serial.tools.list_ports.comports()

연결이 잘 되었다면 코드 실행시 연결된 포트 정보가 출력됩니다. 또한 GUI에서 Port 드롭다운 박스에서 해당 포트를 선택합니다.

3. 시리얼 통신 (Serial Communication)

PySerial 라이브러리를 사용해 시리얼 포트에 연결하고, 장치와 통신할 수 있도록 합니다.

serial.Serial()
- 시리얼 통신을 위해 포트와 속도를 설정해 시리얼 연결을 열고 통신합니다.

prm.trans = serial.Serial(prm.sPort, prm.sVelo, timeout=1)

4. 데이터 송수신 및 프로토콜 처리

데이터 송신
- processStateData(prm.state,'put') 는 데이터를 전송하기 위해 Ring Buffer에 데이터를 쌓는 역할을 합니다.
  
  Ring buffer의 크기는 10(=limitBF)으로 설정합니다.
- send_message() 에서 시리얼 포트를 통해 데이터를 전송합니다. 이때, 데이터를 바이트(byte) 형태로 전송하는데, 각 숫자를 1바이트씩 변환하여 보냅니다.

def processStateData(state, mode):
    # mode에는 'sum' 또는 'put', 'display' 전달
    value = prmmap.get(state, lambda mode: None)(mode)

    if mode == "put": # 비트 필드 업데이트
        for i in range (len(value)):
            prm.RingBF.append(value[i])
            prm.cPut += 1
            prm.cRing += 1
        if prm.cPut >= prm.limitBF:
            prm.cPut = 0
            
def send_message(message): #send data
    byCHK = processStateData(prm.state, 'sum') 
    for number in prm.RingBF:
        prm.trans.write(number.to_bytes(1,'big')) #send byte type date
    prm.trans.write(byCHK.to_bytes(1,'big')) #send byte type cheksum

데이터 수신
- 수신할 때는 23바이트를 읽어 장치에서 전송된 데이터를 처리합니다.

response = prm.trans.read(23) # 수신

5. 체크섬 (Checksum) 계산

체크섬은 데이터의 무결성을 확인하는 데 사용됩니다.
processStateData 함수에서 데이터를 송신하기 전 체크섬을 계산합니다. 1바이트 단위로 데이터를 AND 연산하여 체크섬을 계산하고, 이를 2의 보수로 처리하여 데이터 전송 중 오류가 발생했는지 확인합니다.
Data bytes - RMID, TMID, ID, PID, Data number, data, ....., CHK

Check Sum 방법

보낼 때	BYTE byChkSend, byCHK; byChkSend = RMID+TMID+ID+PID+Data number + Data..; byCHK = (~byChkSend) +1
받을 때	BYTE byChkRecv; byChkRecv = RMID+TMID+ID+PID+Data number + Data..+CHK; byChkRecv가 0이면 정상

예시) 메인제어기에서 모터드라이버로 데이터 요청하는 경우

183, 184, ID, PID_REQ_PID_DATA(4), 1, Requested PID, CHK

    def processStateData(state, mode):
        elif mode == 'sum':
            byChkSend = value & 0xFF  # 'sum'일 때 체크섬 계산
            byCHK = (~byChkSend + 1) & 0xFF  # 체크섬 값 계산
            return byCHK

6. 멀티 스레딩 (Multithreading)

데이터의 송수신이나 상태 확인 작업을 메인 스레드와 별도로 처리하기 위해 스레드를 사용하여 데이터를 지속적으로 확인하거나 처리합니다.
Ring Buffer 에 넣은 값은 getBF 함수를 스레드로 항시 받습니다.

def getBF(): #Fetching from the ring buffer, continuously running in a thread 
    while True:
        if prm.cRing > 0:
            send_message(prm.RingBF[0])
            prm.RingBF.pop(0)
            prm.cGet += 1
            prm.cRing -= 1

            if prm.cGet >= prm.limitBF:
                prm.cGet = 0

getBFThread = threading.Thread(target = getBF)
getBFThread.start()
win.mainloop()
getBFThread.join()

7. Tkinter 기반 GUI로 통신 제어

Tkinter기반 GUI 사용자가 시리얼 포트, ID, 속도(baud rate) 등을 선택하고, 데이터를 송수신하는 작업을 제어할 수 있도록 합니다.
Combobox를 사용해 사용 가능한 시리얼 포트와 속도를 선택할 수 있게 하고, 버튼을 통해 장치와 연결하거나 데이터를 송수신합니다. 통신 상태를 GUI 상에서 실시간으로 확인할 수 있도록 구성되었습니다.

COM 3에 connected 후, velocity 1000으로 Run(+) -> Stop -> Main Data Req

8. run

PID	타입	PID 이름/설명	내용/범위	기본값/기타
130 0x82	C	PID_VEL_CMD 속도명령	모터 속도제어를 위한 지령(Unit, rpm) RPM이 음수(-)인 경우 모터는 CW방향 회전 양수인 경우 모터는 CCW방향 회전 (모터 축방향에서의 회전방향) 183, TMID, ID, 130, 2, D0, D1, CHK	INT

모터를 실행시키는 명령입니다.
통신 사양서에 기재된 대로 해당 변수 값들을 지정해준 후, 연산을 진행합니다.
sum 은 체크섬 계산을 위한 연산, put 은 데이터 전송, display는 ui에 데이터를 표시하기 위함입니다.
run 동작에서는 d1,d2에 대한 값을 savePositiveValue(), saveNegativeValue() 에서 계산해줍니다.
ui에서 속도에 양수값을 준다면 모터는 시계방향으로 회전하고, 음수값을 준다면 반시계방향으로 회전합니다.

    def run(self, mode):
        if mode == 'sum':
            result = self.targetMID + self.myMID + self.sID + self.PARAMETER_ID + self.dataNumber + self.d1 + self.d2
        elif mode == 'put' or mode == 'display':
            result = [self.targetMID, self.myMID, self.sID, self.PARAMETER_ID, self.dataNumber, self.d1, self.d2]
        return result

# Saves a positive integer value from a text entry, 
# converts it to signed binary, calculates related values, 
# and updates display.
def savePositiveValue():
    prm.state = 'run'
    entry_value = text_entry.get() # Get the entry value from text entry widget
    positive = int(entry_value) # Convert the entry value to an integer
    binary = decimal_to_signed_binary(positive) # Convert the positive integer to signed binary representation
    msb,lsb = split_msb_lsb(binary)  # Split the binary representation into Most Significant Bit (MSB) and Least Significant Bit (LSB)
    # Convert LSB and MSB from binary to decimal
    prm.d1 = binary_to_decimal(lsb)
    prm.d2 = binary_to_decimal(msb)
    prm.byCHK = processStateData(prm.state,'sum') # Calculate the checksum (byCHK)
    processStateData(prm.state,'put') # Update the bitfield (BF)
    processStateData(prm.state,'display') # Send the display

9. stop

stop은 모터를 멈추는 동작을 구현합니다.

    def stop(self, mode):
        if mode == 'sum':
            result = self.targetMID + self.myMID + self.sID  + self.BY_STOP + self.DATA2 + self.stopD1 + self.stopD2
        elif mode == 'put' or mode == 'display':
            result = [self.targetMID,self.myMID, self.sID, self.BY_STOP,self.DATA2, self.stopD1, self.stopD2]
        return result

10. pid4

메인 제어기에서 모터 드라이버로 데이터를 요청합니다.

PID	타입	PID 이름/설명	내용/범위	기본값/기타
4 0x04	C	PID_REQ_PID_DATA 데이터 요청	PID : 0~253, 값을 읽기 원하는 PID번호 183, TMID, 1, 4, 1, PID, CHK	BYTE

    def pid4(self, mode):
        if mode == 'sum':
            result = self.targetMID + self.myMID + self.sID  + self.PID4 + self.DATA1 + self.PID_MAIN_DATA
        elif mode == 'put' or mode == 'display':
            result = [self.targetMID,self.myMID,self.sID,self.PID4,self.DATA1,self.PID_MAIN_DATA]
        return result

11.MainBcOn / MainBCOff

CMD_MAIN_BC_ON: PID_MAIN_DATA를 시스템이 10Hz 주기로 주기적으로 데이터를 송출합니다.
- 외부 시스템이나 다른 모듈이 이 데이터를 수신할 수 있게 됩니다.
CMD_MAIN_BC_OFF: PID_MAIN_DATA의 주기적 방송을 중지하는 명령입니다

PID	타입	PID 이름/설명	내용/범위	기본값/기타
10 0x0a	C	PID_COMMAND	CMD의 값에 따른 내용 *COMMAND의 내용은 아래표에서 참조 183, TMID, ID, 10, 1, CMD, CHK	1BYTE 명령모음

COMMAND(PID 10번)에 따른 내용

C(command), W(write data to memory)

번호	COMMAND(CMD)	Type	내용
5	CMD_MAIN.. BC_ON	C	PID_MAIN_DATA broadcasting ON(10hz)
6	CMD_MAIN_BC_OFF	C	PID_MAIN_DATA broadcasting OFF

    def MainBCOn(self, mode):
        if mode == 'sum':
            result = self.targetMID + self.myMID + self.sID  + self.PID_COMMAND + self.DATA1 + self.CMD_MAIN_BC_ON
        elif mode == 'put' or mode == 'display':
            result =  [self.targetMID,self.myMID,self.sID,self.PID_COMMAND,self.DATA1,self.CMD_MAIN_BC_ON]
        return result
        
    def MainBCOff(self, mode):
        if mode == 'sum':
            result = self.targetMID + self.myMID + self.sID  + self.PID_COMMAND + self.DATA1 + self.CMD_MAIN_BC_OFF
        elif mode == 'put' or mode == 'display':
            result =  [self.targetMID, self.myMID,self.sID,self.PID_COMMAND,self.DATA1,self.CMD_MAIN_BC_OFF]
        return result

12.controllerType

PID_TYPE, 제어기 타입을 송출합니다.

PID	타입	PID 이름/설명	내용/범위	기본값/기타
205 0xcd	R	PID_TYPE, 제어기 타입	DATA : 20bytes 이내(Character 값으로 전송됨)	BYTE

    def controllerType(self, mode):
        if mode == 'sum':
            result = self.targetMID + self.myMID + self.sID  + self.PID4 + self.DATA1 + self.PID_TYPE
        elif mode == 'put' or mode == 'display':
            result = [self.targetMID ,self.myMID , self.sID , self.PID4 , self.DATA1 , self.PID_TYPE]
        return result

13. 실시간 데이터 그래프로 표현

사용자가 입력한 속도값을 기반으로 실시간 그래프를 표시합니다. 사인파(sine wave) 를 기반으로 프레임 단위로 업데이터 되며 동작하는 애니매이션을 구현했습니다.

속도 값 0 ~ 1000 까지 그래프와 동일한 값으로 모터를 제어합니다.

def savegraphValue():
    prm.state = 'graph'
    velo = int(text_entry.get())
    midVelo = int(velo/2)
    # set x range (0~ 2π)
    x = np.linspace(0, 2 * np.pi, velo)
    # set y value
    y = np.sin(x) * velo / 2 + midVelo

    #graph initial settings
    fig, ax = plt.subplots()
    line, = ax.plot(x, y)
    ax.set_ylim(0, velo)  # setting y range

    #x-axis setting
    plt.xticks([0, np.pi / 2, np.pi, 3 * np.pi / 2, 2 * np.pi],
               ['0', r'$\frac{\pi}{2}$', r'$\pi$', r'$\frac{3\pi}{2}$', r'$2\pi$'])

    #Graph Update Function
    def update(frame):
        #Calculate new y value
        new_y = np.abs(np.sin(x + frame * 0.1) * velo / 2 + midVelo)
        #Converting each element to an integer
        new_y_int = [int(value) for value in new_y]

        #update new graph value
        line.set_ydata(new_y_int)
        #calculate the binary encoded string
        binary = decimal_to_signed_binary(new_y_int[0])
        msb, lsb = split_msb_lsb(binary)
        prm.d1 = binary_to_decimal(lsb) 
        prm.d2 = binary_to_decimal(msb)
        prm.byCHK = processStateData(prm.state,'sum') # Calculate the checksum (byCHK)
        processStateData(prm.state,'put') # Update the bitfield (BF)

        #indicates the graphical object to be updated for each frame
        return line, 

    #create graph animation
    ani = FuncAnimation(fig, update, interval=100)

    plt.show()