Control with Python

We describe the VESC control method using python.

1. Using CAN Custom Control Message

VESC에서 사용하는 CAN 통신 메시지중, CAN FORWARD 기능에 이용되는 CAN PACKET ID가 있다. 이는 아래 'CAN_PACKET_ID' 중, 5에 해당하는 'CAN_PACKET_FILL_RX_BUFFER' 와 7에 해당하는 'CAN_PACKET_PROCESS_RX_BUFFER' 를 이용하면 목 VESC에 임의의 명령어를 전송하는 것이 가능하다. 이는 기존에 전류, Duty, 위치, 속도 등의 위치명령을 서로 다른 CAN Message로 처리하던 방식에서, 특정 VESC ID에 임의의 명령을 하나의 통일된 CAN Message 형태로 보낼 수 있다는 장점이 있다. 단, 기존 방법에 비해 Message의 길이가 다소 늘어나는 단점은 있다. 여기서 우리는 이러한 방식을 CAN Custom Control Message 전송 방식으로 부르도록 한다.

// CAN commands
typedef enum {
	CAN_PACKET_SET_DUTY = 0,
	CAN_PACKET_SET_CURRENT,
	CAN_PACKET_SET_CURRENT_BRAKE,
	CAN_PACKET_SET_RPM,
	CAN_PACKET_SET_POS,
	CAN_PACKET_FILL_RX_BUFFER,
	CAN_PACKET_FILL_RX_BUFFER_LONG,
	CAN_PACKET_PROCESS_RX_BUFFER,
	CAN_PACKET_PROCESS_SHORT_BUFFER,
	CAN_PACKET_STATUS,
	CAN_PACKET_SET_CURRENT_REL,
	CAN_PACKET_SET_CURRENT_BRAKE_REL,
	CAN_PACKET_SET_CURRENT_HANDBRAKE,
	CAN_PACKET_SET_CURRENT_HANDBRAKE_REL,
	CAN_PACKET_STATUS_2,
	CAN_PACKET_STATUS_3,
	CAN_PACKET_STATUS_4,
	CAN_PACKET_PING,
	CAN_PACKET_PONG,
	CAN_PACKET_DETECT_APPLY_ALL_FOC,
	CAN_PACKET_DETECT_APPLY_ALL_FOC_RES,
	CAN_PACKET_CONF_CURRENT_LIMITS,
	CAN_PACKET_CONF_STORE_CURRENT_LIMITS,
	CAN_PACKET_CONF_CURRENT_LIMITS_IN,
	CAN_PACKET_CONF_STORE_CURRENT_LIMITS_IN,
	CAN_PACKET_CONF_FOC_ERPMS,
	CAN_PACKET_CONF_STORE_FOC_ERPMS,
	CAN_PACKET_STATUS_5,
	CAN_PACKET_POLL_TS5700N8501_STATUS,
	CAN_PACKET_CONF_BATTERY_CUT,
	CAN_PACKET_CONF_STORE_BATTERY_CUT,
	CAN_PACKET_SHUTDOWN,
	CAN_PACKET_IO_BOARD_ADC_1_TO_4,
	CAN_PACKET_IO_BOARD_ADC_5_TO_8,
	CAN_PACKET_IO_BOARD_ADC_9_TO_12,
	CAN_PACKET_IO_BOARD_DIGITAL_IN,
	CAN_PACKET_IO_BOARD_SET_OUTPUT_DIGITAL,
	CAN_PACKET_IO_BOARD_SET_OUTPUT_PWM,
	CAN_PACKET_BMS_V_TOT,
	CAN_PACKET_BMS_I,
	CAN_PACKET_BMS_AH_WH,
	CAN_PACKET_BMS_V_CELL,
	CAN_PACKET_BMS_BAL,
	CAN_PACKET_BMS_TEMPS,
	CAN_PACKET_BMS_HUM,
	CAN_PACKET_BMS_SOC_SOH_TEMP_STAT
} CAN_PACKET_ID;

CAN Custom Control Message 전송 방식은 아래와 같다. 아래는 USB통신을 통해 CAN Custom Control Message 전송 방식으로 보내진 데이터의 예로 이를 해석하면 다음과 같다.

앞선 USB를 통해 전송된 CAN Custom Control Message의 해석결과 101번 모터에 DPS 제어를 가하는 명령어임을 알 수 있다. 이를 통해 일반적으로 사용가능한 CAN Custom Control Message의 포멧을 생각해볼 수 있고 다음과 같은 형태를 사용하면 된다.

변수

내용

목적하는 VESC ID의 Hex 값

제어방식을 나타내는 COMM_PACKET_ID 의 Hex 값

D1 D2 D3 D4

제어 목표값의 Hex 값.

4Byte로 전송되고 제어방식에 따른 유효숫자 변경을 위해

제어방식에 10^n 배를 하여 보내고, 받는 쪽에서 10^n 배로 나눠 사용.

CRCH CRCL

CRC Check Sum의 Hex값. 상위 1Byte(CRCH)와 하위 1Byte(CRCL)로 구성됨

제어방식(COMM_PACKET_ID)에 아래 나열된 제어 명령어들이 사용가능하다.

COMM_PACKET_ID

값 (Hex)

유효숫자 10^n 배

COMM_SET_DUTY

5 (0x05)

100000.0

COMM_SET_CURRENT

6 (0x06)

1000.0

COMM_SET_CURRENT_BRAKE

7 (0x07)

1000.0

COMM_SET_RELEASE

100 (0x64)

COMM_SET_DPS

101 (0x65)

1000.0

COMM_SET_DPS_VMAX

102 (0x66)

1000.0

COMM_SET_DPS_AMAX

103 (0x67)

1000.0

COMM_SET_SERVO

104 (0x68)

1000.0

COMM_SET_TRAJ

105 (0x69)

1000.0

D1 D2 D3 D4 에 실려 보내지는 값은 유효숫자 10^n 배를 곱한 값을 보내기 때문에 받는 쪽에서 동일한 값으로 나눠줘야 물리적으로 의미있는 숫자로 변환됨을 주의해야한다.

모터의 가장 기본적인 제어인 Duty제어를 위해서는 COMM_SET_DUTY를 사용하고 이때 Duty값은 1이 100% 듀티를, 0이 0%를 의미한다.

전류제어를 위해서 COMM_SET_CURRENT를 사용하고 이때 제어값의 단위는 A(암페어)값을 전송한다. 전류제어를 이용하여 모터에 Brake를 인가할 수 있는데 이때 사용하는 것이 COMM_SET_CURRENT_BRAKE 이다. A 단위로 값을 전송한다.

모터의 속도제어를 위해서는 COMM_SET_DPS를 사용한다. Degree 단위의 값을 전송해야하며 100 DPS를 전송하면 초당 100degree의 속도로 회전한다. 이때 회전 가속도는 COMM_SET_DPS_AMAX (deg/sec^2)에서 설정한 값을 따르며 회전 최고 속도는 COMM_SET_DPS_VMAX (deg/sec)으로 제한된다. 본 속도제어는 강력한 Gain의 위치제어를 기반으로 속도 Reference를 사다리꼴 속도 Profile에 의해 생성하여 제어하는 것으로 저속에서도 고토크, 고정밀도 제어가 가능하다.

모터의 절대 위치제어는 COMM_SET_SERVO를 이용하고 이때 전송하는 값은 엔코더의 절대 위치 각도값을 degree 단위로 전송한다. COMM_SET_SERVO를 이용하면 인가한 절대위치를 기존에 설정한 속도 최대값 (COMM_SET_DPS_VMAX )에 따라 목표위치로 이동한다. 펌웨어상에 기본적으로 COMM_SET_DPS_VMAX 값은 최대로 설정해놓았으므로 Application 상황에 따라 이를 미리 조절 후, COMM_SET_SERVO을 인하해야 안전하다.

단순히, Position Lock 기능을 원한다면 COMM_SET_DPS 에 목표값을 0으로 설정해놓으면 현재 엔코더 위치에서 Position Lock이 가능하다. 이후 모터를 Release 하고 싶을 때는 COMM_SET_RELEASE 명령어를 이용하면 되고 목표값은 기본 0으로 설정하면 된다.

COMM_SET_TRAJ 는 사다리꼴 속도 Profile에 따라 위치명령값을 생성하여 위치제어를 하는데 Manipulator의 위치제어시 목표지점까지 부드러게 움직이고 싶을 때 주로 사용할 수 있다.

지금까지 설명한 CAN Custom Control Message 전송방식을 Python Code로 아래 작성하여 나타내었다. CAN 메세지 전송을 위해 Peak System사의 PCAN-USB를 이용하였으며 PCANBasic.py 라이브러리를 이용하였다. CRC Check Sum 계산을 위해서는 VESC 펌웨어에서 제공하는 c언어로 작성된 crc.c, crc.h 을 dll 라이브러리로 컴파일 하여 파이썬에서 import하여 사용하였다. VESC에서는 crc.c 파일에 crc table이 저장이 되어있어 일반적인 crc 라이브러리가 아닌 VESC 펌웨어상에 있는 crc.c 파일을 그대로 이용해야 한다. 파이썬 코드는 아래 주소에서 확인 가능하며 관련 라이브러리 또한 해당 Repository에 존재한다..

from PCANBasic import *

'''
# crc check program from vesc firmware, dll file generated by below command
> gcc -fPIC -c crc.c
> gcc --shared -ocrc_vesc.dll crc.o
'''
import ctypes
crc_vesc = ctypes.cdll.LoadLibrary('./crc_vesc.dll')

can_packet_id = {
'CAN_PACKET_SET_DUTY':0,
'CAN_PACKET_SET_CURRENT':1,
'CAN_PACKET_SET_CURRENT_BRAKE':2,
'CAN_PACKET_SET_RPM':3,
'CAN_PACKET_SET_POS':4,
'CAN_PACKET_FILL_RX_BUFFER':5,
'CAN_PACKET_FILL_RX_BUFFER_LONG':6,
'CAN_PACKET_PROCESS_RX_BUFFER':7,
'CAN_PACKET_PROCESS_SHORT_BUFFER':8,
'CAN_PACKET_STATUS':9
}

comm_set_custom = { 
'COMM_SET_DUTY':5,
'COMM_SET_CURRENT':6,
'COMM_SET_CURRENT_BRAKE':7,
'COMM_SET_RELEASE':100,
'COMM_SET_DPS':101,
'COMM_SET_DPS_VMAX':102,
'COMM_SET_DPS_AMAX':103,
'COMM_SET_SERVO':104,
'COMM_SET_TRAJ':105
}

global pc1

time_prev = 0
msg_type = {'0x0':'PCAN_MESSAGE_STANDARD',
            '0x00':'PCAN_MESSAGE_STANDARD', 
            '0x1':'PCAN_MESSAGE_RTR',
            '0x01':'PCAN_MESSAGE_RTR', 
            '0x2':'PCAN_MESSAGE_EXTENDED',
            '0x02':'PCAN_MESSAGE_EXTENDED', 
            '0x4':'PCAN_MESSAGE_FD',
            '0x04':'PCAN_MESSAGE_FD',
            '0x8':'PCAN_MESSAGE_BRS',
            '0x08':'PCAN_MESSAGE_BRS',
            '0x10':'PCAN_MESSAGE_ESI',
            '0x40':'PCAN_MESSAGE_ERRFRAME',
            '0x80':'PCAN_MESSAGE_STATUS'}


def pcan_Open():
    result = pc1.Initialize(PCAN_USBBUS1, PCAN_BAUD_1M)
    if result != PCAN_ERROR_OK:
        # An error occurred, get a text describing the error and show it
        #
        result = pc1.GetErrorText(result)
        print(result[1])
    else:
        print("PCAN-USB (Ch-1) was initialized")

def pcan_Close():
    # The USB Channel is released
    #
    result = pc1.Uninitialize(PCAN_USBBUS1)
    if result != PCAN_ERROR_OK:
        # An error occurred, get a text describing the error and show it
        #
        result = pc1.GetErrorText(result)
        print(result[1])
    else:
        print("PCAN-USB (Ch-1) was released")

def ReadMessage():
        # We execute the "Read" function of the PCANBasic
        #
        result = pc1.Read(PCAN_USBBUS1)

        if result[0] == PCAN_ERROR_OK:
            # We show the received message
            #
            ProcessMessage(result[1:])
            
        return result[0]

def ReadMessages():
    result = PCAN_ERROR_OK,
    while (result[0] & PCAN_ERROR_QRCVEMPTY) != PCAN_ERROR_QRCVEMPTY:
        result = pc1.Read(PCAN_USBBUS1)
        if result[0] != PCAN_ERROR_QRCVEMPTY:
            ProcessMessage(result[1:])
        else:
            if(result[0] != PCAN_ERROR_QRCVEMPTY):  print('ERROR_CODE:{}'.format(hex(result[0])))

def ProcessMessage(*args):
    global time_prev

    theMsg = args[0][0]
    itsTimeStamp = args[0][1]    

    newMsg = TPCANMsgFD()
    newMsg.ID = theMsg.ID
    newMsg.DLC = theMsg.LEN
    for i in range(8 if (theMsg.LEN > 8) else theMsg.LEN):
        newMsg.DATA[i] = theMsg.DATA[i]
    newMsg.MSGTYPE = theMsg.MSGTYPE
    newTimestamp = TPCANTimestampFD()
    newTimestamp.value = (itsTimeStamp.micros + 1000 * itsTimeStamp.millis + 0x100000000 * 1000 * itsTimeStamp.millis_overflow)

    time = "Timestamp:{:0.3f}sec".format(newTimestamp.value/1000000)
    period = newTimestamp.value - time_prev
    cycle_time = "Cycle Time:{:0.3f}msec".format(period/1000)
    TYPE = "TYPE:{}".format(msg_type[hex(newMsg.MSGTYPE)])
    EID = "EID:{}".format(hex(newMsg.ID))
    DLC = "DLC:{}".format(newMsg.DLC)
    DATA = ' '.join('{:02x}'.format(newMsg.DATA[i]) for i in range(newMsg.DLC))

    if newMsg.MSGTYPE == 0x02:  # PCAN_MESSAGE_EXTEND 
        print(time,"|",TYPE,"|",EID,"|",DLC,"|",DATA,"|",cycle_time)
    
    time_prev = newTimestamp.value

def WriteFrame(id, dlc, data):   
    CANMsg = TPCANMsg()

    CANMsg.ID = id
    CANMsg.LEN = dlc
    CANMsg.MSGTYPE = PCAN_MESSAGE_EXTENDED

    for i in range(CANMsg.LEN):
        CANMsg.DATA[i] = data[i]

    return pc1.Write(PCAN_USBBUS1, CANMsg)

def GetFormatedError(error):
    stsReturn = pc1.GetErrorText(error, 0)
    if stsReturn[0] != PCAN_ERROR_OK:
        return "An error occurred. Error-code's text ({0:X}h) couldn't be retrieved".format(error)
    else:
        return stsReturn[1]

def VescCustumControl(target_vesc_id, ctrl_type, ctrl_value):
    '''
    # Custom Control CAN Msg Frame Structure Example, id=0x53, comm_set_custom=0x65=101=COMM_SET_DPS, data=2000000
    WriteFrame(0x00000553,8,[0x00, 0x24, 0x05, 0x01, 0xFF, 0x65, 0x00, 0x1E])
    WriteFrame(0x00000553,3,[0x07, 0x84, 0x80])
    WriteFrame(0x00000753,6,[0x53, 0x00, 0x00, 0x09, 0xBB, 0x4E])    
    '''
    
    if ctrl_type == 'COMM_SET_DUTY':
        value = int(ctrl_value * 100000.0)
    elif ctrl_type == 'COMM_SET_POS':
        value = int(ctrl_value * 1000000.0)
    elif ctrl_type == 'COMM_SET_RPM' or ctrl_type == 'COMM_SET_RELEASE':
        value = int(ctrl_value)
    else:
        value = int(ctrl_value * 1000.0)

    command = comm_set_custom[ctrl_type]

    d1 = (value >> 24) & 0xFF
    d2 = (value >> 16) & 0xFF
    d3 = (value >> 8) & 0xFF
    d4 = value & 0xFF
    data_arr = [d1, d2, d3, d4]
    
    # Frame1, Frame2 is for 'CAN_PACKET_FILL_RX_BUFFER(0x05)'
    eid1 = (can_packet_id['CAN_PACKET_FILL_RX_BUFFER'] << 8) | target_vesc_id
    dlc1 = 8
    # Frame1 = [0x00(length index), 0x24(COMM_CUSTOM_APP_DATA=36), 0x06(host_model=6=CAN), 0x01(number of vesc), 0xFF(Target_vesc_id=default local=255), command, d1, d2]
    Frame1 = [0x00, 0x24, 0x06, 0x01, 0xFF, command, d1, d2]

    eid2 = (can_packet_id['CAN_PACKET_FILL_RX_BUFFER'] << 8) | target_vesc_id
    dlc2 = 3
    # Frame2 = [0x07(length index), d3, d4]
    Frame2 = [0x07, d3, d4]

    # crc calculation only for 9byte data frame (that is except 0x00, 0x07)
    data_arr = Frame1[1:] + Frame2[1:]
    arr = (ctypes.c_ubyte * len(data_arr))(*data_arr)

    crc = crc_vesc.crc16(arr,9)
    crch = (crc >> 8) & 0xFF
    crcl = crc & 0xFF

    # Frame3 is for 'CAN_PACKET_PROCESS_RX_BUFFER(0x07)'
    eid3 = (can_packet_id['CAN_PACKET_PROCESS_RX_BUFFER'] << 8) | target_vesc_id
    dlc3 = 6
    # Frame2 = [target_vesc_id, send, data_length high, data_length low, crch, crcl]
    Frame3 = [target_vesc_id, 0x00, 0x00, 0x09, crch, crcl]

    '''
    # debug print of can msgs
    print('Frame1 - eid:', format(hex(eid1)), '| dlc:',  format(hex(dlc1)), '| data:', '[{}]'.format(', '.join(hex(x) for x in Frame1)) )
    print('Frame2 - eid:', format(hex(eid2)), '| dlc:',  format(hex(dlc2)), '| data:', '[{}]'.format(', '.join(hex(x) for x in Frame2)) )
    print('Frame3 - eid:', format(hex(eid3)), '| dlc:',  format(hex(dlc3)), '| data:', '[{}]'.format(', '.join(hex(x) for x in Frame3)) )
    '''

    WriteFrame(eid1, dlc1, Frame1)
    WriteFrame(eid2, dlc2, Frame2)
    WriteFrame(eid3, dlc3, Frame3)
    
    print('Run VESC CAN Direct Control,', ctrl_type,':',ctrl_value)


if __name__ == '__main__':
    pc1 = PCANBasic()

    # Open PCAN Device
    pcan_Open()

    '''
    # CAN MSG Read Routine
    num = 0
    # try~ except 특정 예외
    try:
        # 무한 반복
        while True:
            ReadMessages()
            num += 1
    # Ctrl + C를 입력할 경우
    except KeyboardInterrupt:
        print('Total Rcv number is {}, Quit to receive'.format(num))
'''

    # CAN MSG Write Routine
    #VescCustumControl(83, 'COMM_SET_DUTY',0.1)
    #VescCustumControl(83, 'COMM_SET_CURRENT',0.5)
    #VescCustumControl(83, 'COMM_SET_CURRENT_BRAKE',5)
    #VescCustumControl(83, 'COMM_SET_DPS',0) # position lock on current position
    #VescCustumControl(83, 'COMM_SET_DPS',1000)
    #VescCustumControl(83, 'COMM_SET_DPS_VMAX',2000)
    #VescCustumControl(83, 'COMM_SET_SERVO',0)
    VescCustumControl(83, 'COMM_SET_RELEASE',0)

    # Close PCAN Device
    pcan_Close()

위 코드에서 #(주석)처리된 VescCustumControl(~~~)를 하나씩 실행하여 83번 VESC에 해당 명령어를 CAN 통신을 통해 전송가능함을 확인하였다.

PreviousControl with VESC driver NextControl with CAN

Last updated 3 years ago

Was this helpful?

1. Using CAN Custom Control Message

// CAN commands
typedef enum {
	CAN_PACKET_SET_DUTY = 0,
	CAN_PACKET_SET_CURRENT,
	CAN_PACKET_SET_CURRENT_BRAKE,
	CAN_PACKET_SET_RPM,
	CAN_PACKET_SET_POS,
	CAN_PACKET_FILL_RX_BUFFER,
	CAN_PACKET_FILL_RX_BUFFER_LONG,
	CAN_PACKET_PROCESS_RX_BUFFER,
	CAN_PACKET_PROCESS_SHORT_BUFFER,
	CAN_PACKET_STATUS,
	CAN_PACKET_SET_CURRENT_REL,
	CAN_PACKET_SET_CURRENT_BRAKE_REL,
	CAN_PACKET_SET_CURRENT_HANDBRAKE,
	CAN_PACKET_SET_CURRENT_HANDBRAKE_REL,
	CAN_PACKET_STATUS_2,
	CAN_PACKET_STATUS_3,
	CAN_PACKET_STATUS_4,
	CAN_PACKET_PING,
	CAN_PACKET_PONG,
	CAN_PACKET_DETECT_APPLY_ALL_FOC,
	CAN_PACKET_DETECT_APPLY_ALL_FOC_RES,
	CAN_PACKET_CONF_CURRENT_LIMITS,
	CAN_PACKET_CONF_STORE_CURRENT_LIMITS,
	CAN_PACKET_CONF_CURRENT_LIMITS_IN,
	CAN_PACKET_CONF_STORE_CURRENT_LIMITS_IN,
	CAN_PACKET_CONF_FOC_ERPMS,
	CAN_PACKET_CONF_STORE_FOC_ERPMS,
	CAN_PACKET_STATUS_5,
	CAN_PACKET_POLL_TS5700N8501_STATUS,
	CAN_PACKET_CONF_BATTERY_CUT,
	CAN_PACKET_CONF_STORE_BATTERY_CUT,
	CAN_PACKET_SHUTDOWN,
	CAN_PACKET_IO_BOARD_ADC_1_TO_4,
	CAN_PACKET_IO_BOARD_ADC_5_TO_8,
	CAN_PACKET_IO_BOARD_ADC_9_TO_12,
	CAN_PACKET_IO_BOARD_DIGITAL_IN,
	CAN_PACKET_IO_BOARD_SET_OUTPUT_DIGITAL,
	CAN_PACKET_IO_BOARD_SET_OUTPUT_PWM,
	CAN_PACKET_BMS_V_TOT,
	CAN_PACKET_BMS_I,
	CAN_PACKET_BMS_AH_WH,
	CAN_PACKET_BMS_V_CELL,
	CAN_PACKET_BMS_BAL,
	CAN_PACKET_BMS_TEMPS,
	CAN_PACKET_BMS_HUM,
	CAN_PACKET_BMS_SOC_SOH_TEMP_STAT
} CAN_PACKET_ID;

변수

내용

목적하는 VESC ID의 Hex 값

제어방식을 나타내는 COMM_PACKET_ID 의 Hex 값

D1 D2 D3 D4

제어 목표값의 Hex 값.

4Byte로 전송되고 제어방식에 따른 유효숫자 변경을 위해

제어방식에 10^n 배를 하여 보내고, 받는 쪽에서 10^n 배로 나눠 사용.

CRCH CRCL

CRC Check Sum의 Hex값. 상위 1Byte(CRCH)와 하위 1Byte(CRCL)로 구성됨

제어방식(COMM_PACKET_ID)에 아래 나열된 제어 명령어들이 사용가능하다.

COMM_PACKET_ID

값 (Hex)

유효숫자 10^n 배

COMM_SET_DUTY

5 (0x05)

100000.0

COMM_SET_CURRENT

6 (0x06)

1000.0

COMM_SET_CURRENT_BRAKE

7 (0x07)

1000.0

COMM_SET_RELEASE

100 (0x64)

COMM_SET_DPS

101 (0x65)

1000.0

COMM_SET_DPS_VMAX

102 (0x66)

1000.0

COMM_SET_DPS_AMAX

103 (0x67)

1000.0

COMM_SET_SERVO

104 (0x68)

1000.0

COMM_SET_TRAJ

105 (0x69)

1000.0

모터의 가장 기본적인 제어인 Duty제어를 위해서는 COMM_SET_DUTY를 사용하고 이때 Duty값은 1이 100% 듀티를, 0이 0%를 의미한다.

from PCANBasic import *

'''
# crc check program from vesc firmware, dll file generated by below command
> gcc -fPIC -c crc.c
> gcc --shared -ocrc_vesc.dll crc.o
'''
import ctypes
crc_vesc = ctypes.cdll.LoadLibrary('./crc_vesc.dll')

can_packet_id = {
'CAN_PACKET_SET_DUTY':0,
'CAN_PACKET_SET_CURRENT':1,
'CAN_PACKET_SET_CURRENT_BRAKE':2,
'CAN_PACKET_SET_RPM':3,
'CAN_PACKET_SET_POS':4,
'CAN_PACKET_FILL_RX_BUFFER':5,
'CAN_PACKET_FILL_RX_BUFFER_LONG':6,
'CAN_PACKET_PROCESS_RX_BUFFER':7,
'CAN_PACKET_PROCESS_SHORT_BUFFER':8,
'CAN_PACKET_STATUS':9
}

comm_set_custom = { 
'COMM_SET_DUTY':5,
'COMM_SET_CURRENT':6,
'COMM_SET_CURRENT_BRAKE':7,
'COMM_SET_RELEASE':100,
'COMM_SET_DPS':101,
'COMM_SET_DPS_VMAX':102,
'COMM_SET_DPS_AMAX':103,
'COMM_SET_SERVO':104,
'COMM_SET_TRAJ':105
}

global pc1

time_prev = 0
msg_type = {'0x0':'PCAN_MESSAGE_STANDARD',
            '0x00':'PCAN_MESSAGE_STANDARD', 
            '0x1':'PCAN_MESSAGE_RTR',
            '0x01':'PCAN_MESSAGE_RTR', 
            '0x2':'PCAN_MESSAGE_EXTENDED',
            '0x02':'PCAN_MESSAGE_EXTENDED', 
            '0x4':'PCAN_MESSAGE_FD',
            '0x04':'PCAN_MESSAGE_FD',
            '0x8':'PCAN_MESSAGE_BRS',
            '0x08':'PCAN_MESSAGE_BRS',
            '0x10':'PCAN_MESSAGE_ESI',
            '0x40':'PCAN_MESSAGE_ERRFRAME',
            '0x80':'PCAN_MESSAGE_STATUS'}


def pcan_Open():
    result = pc1.Initialize(PCAN_USBBUS1, PCAN_BAUD_1M)
    if result != PCAN_ERROR_OK:
        # An error occurred, get a text describing the error and show it
        #
        result = pc1.GetErrorText(result)
        print(result[1])
    else:
        print("PCAN-USB (Ch-1) was initialized")

def pcan_Close():
    # The USB Channel is released
    #
    result = pc1.Uninitialize(PCAN_USBBUS1)
    if result != PCAN_ERROR_OK:
        # An error occurred, get a text describing the error and show it
        #
        result = pc1.GetErrorText(result)
        print(result[1])
    else:
        print("PCAN-USB (Ch-1) was released")

def ReadMessage():
        # We execute the "Read" function of the PCANBasic
        #
        result = pc1.Read(PCAN_USBBUS1)

        if result[0] == PCAN_ERROR_OK:
            # We show the received message
            #
            ProcessMessage(result[1:])
            
        return result[0]

def ReadMessages():
    result = PCAN_ERROR_OK,
    while (result[0] & PCAN_ERROR_QRCVEMPTY) != PCAN_ERROR_QRCVEMPTY:
        result = pc1.Read(PCAN_USBBUS1)
        if result[0] != PCAN_ERROR_QRCVEMPTY:
            ProcessMessage(result[1:])
        else:
            if(result[0] != PCAN_ERROR_QRCVEMPTY):  print('ERROR_CODE:{}'.format(hex(result[0])))

def ProcessMessage(*args):
    global time_prev

    theMsg = args[0][0]
    itsTimeStamp = args[0][1]    

    newMsg = TPCANMsgFD()
    newMsg.ID = theMsg.ID
    newMsg.DLC = theMsg.LEN
    for i in range(8 if (theMsg.LEN > 8) else theMsg.LEN):
        newMsg.DATA[i] = theMsg.DATA[i]
    newMsg.MSGTYPE = theMsg.MSGTYPE
    newTimestamp = TPCANTimestampFD()
    newTimestamp.value = (itsTimeStamp.micros + 1000 * itsTimeStamp.millis + 0x100000000 * 1000 * itsTimeStamp.millis_overflow)

    time = "Timestamp:{:0.3f}sec".format(newTimestamp.value/1000000)
    period = newTimestamp.value - time_prev
    cycle_time = "Cycle Time:{:0.3f}msec".format(period/1000)
    TYPE = "TYPE:{}".format(msg_type[hex(newMsg.MSGTYPE)])
    EID = "EID:{}".format(hex(newMsg.ID))
    DLC = "DLC:{}".format(newMsg.DLC)
    DATA = ' '.join('{:02x}'.format(newMsg.DATA[i]) for i in range(newMsg.DLC))

    if newMsg.MSGTYPE == 0x02:  # PCAN_MESSAGE_EXTEND 
        print(time,"|",TYPE,"|",EID,"|",DLC,"|",DATA,"|",cycle_time)
    
    time_prev = newTimestamp.value

def WriteFrame(id, dlc, data):   
    CANMsg = TPCANMsg()

    CANMsg.ID = id
    CANMsg.LEN = dlc
    CANMsg.MSGTYPE = PCAN_MESSAGE_EXTENDED

    for i in range(CANMsg.LEN):
        CANMsg.DATA[i] = data[i]

    return pc1.Write(PCAN_USBBUS1, CANMsg)

def GetFormatedError(error):
    stsReturn = pc1.GetErrorText(error, 0)
    if stsReturn[0] != PCAN_ERROR_OK:
        return "An error occurred. Error-code's text ({0:X}h) couldn't be retrieved".format(error)
    else:
        return stsReturn[1]

def VescCustumControl(target_vesc_id, ctrl_type, ctrl_value):
    '''
    # Custom Control CAN Msg Frame Structure Example, id=0x53, comm_set_custom=0x65=101=COMM_SET_DPS, data=2000000
    WriteFrame(0x00000553,8,[0x00, 0x24, 0x05, 0x01, 0xFF, 0x65, 0x00, 0x1E])
    WriteFrame(0x00000553,3,[0x07, 0x84, 0x80])
    WriteFrame(0x00000753,6,[0x53, 0x00, 0x00, 0x09, 0xBB, 0x4E])    
    '''
    
    if ctrl_type == 'COMM_SET_DUTY':
        value = int(ctrl_value * 100000.0)
    elif ctrl_type == 'COMM_SET_POS':
        value = int(ctrl_value * 1000000.0)
    elif ctrl_type == 'COMM_SET_RPM' or ctrl_type == 'COMM_SET_RELEASE':
        value = int(ctrl_value)
    else:
        value = int(ctrl_value * 1000.0)

    command = comm_set_custom[ctrl_type]

    d1 = (value >> 24) & 0xFF
    d2 = (value >> 16) & 0xFF
    d3 = (value >> 8) & 0xFF
    d4 = value & 0xFF
    data_arr = [d1, d2, d3, d4]
    
    # Frame1, Frame2 is for 'CAN_PACKET_FILL_RX_BUFFER(0x05)'
    eid1 = (can_packet_id['CAN_PACKET_FILL_RX_BUFFER'] << 8) | target_vesc_id
    dlc1 = 8
    # Frame1 = [0x00(length index), 0x24(COMM_CUSTOM_APP_DATA=36), 0x06(host_model=6=CAN), 0x01(number of vesc), 0xFF(Target_vesc_id=default local=255), command, d1, d2]
    Frame1 = [0x00, 0x24, 0x06, 0x01, 0xFF, command, d1, d2]

    eid2 = (can_packet_id['CAN_PACKET_FILL_RX_BUFFER'] << 8) | target_vesc_id
    dlc2 = 3
    # Frame2 = [0x07(length index), d3, d4]
    Frame2 = [0x07, d3, d4]

    # crc calculation only for 9byte data frame (that is except 0x00, 0x07)
    data_arr = Frame1[1:] + Frame2[1:]
    arr = (ctypes.c_ubyte * len(data_arr))(*data_arr)

    crc = crc_vesc.crc16(arr,9)
    crch = (crc >> 8) & 0xFF
    crcl = crc & 0xFF

    # Frame3 is for 'CAN_PACKET_PROCESS_RX_BUFFER(0x07)'
    eid3 = (can_packet_id['CAN_PACKET_PROCESS_RX_BUFFER'] << 8) | target_vesc_id
    dlc3 = 6
    # Frame2 = [target_vesc_id, send, data_length high, data_length low, crch, crcl]
    Frame3 = [target_vesc_id, 0x00, 0x00, 0x09, crch, crcl]

    '''
    # debug print of can msgs
    print('Frame1 - eid:', format(hex(eid1)), '| dlc:',  format(hex(dlc1)), '| data:', '[{}]'.format(', '.join(hex(x) for x in Frame1)) )
    print('Frame2 - eid:', format(hex(eid2)), '| dlc:',  format(hex(dlc2)), '| data:', '[{}]'.format(', '.join(hex(x) for x in Frame2)) )
    print('Frame3 - eid:', format(hex(eid3)), '| dlc:',  format(hex(dlc3)), '| data:', '[{}]'.format(', '.join(hex(x) for x in Frame3)) )
    '''

    WriteFrame(eid1, dlc1, Frame1)
    WriteFrame(eid2, dlc2, Frame2)
    WriteFrame(eid3, dlc3, Frame3)
    
    print('Run VESC CAN Direct Control,', ctrl_type,':',ctrl_value)


if __name__ == '__main__':
    pc1 = PCANBasic()

    # Open PCAN Device
    pcan_Open()

    '''
    # CAN MSG Read Routine
    num = 0
    # try~ except 특정 예외
    try:
        # 무한 반복
        while True:
            ReadMessages()
            num += 1
    # Ctrl + C를 입력할 경우
    except KeyboardInterrupt:
        print('Total Rcv number is {}, Quit to receive'.format(num))
'''

    # CAN MSG Write Routine
    #VescCustumControl(83, 'COMM_SET_DUTY',0.1)
    #VescCustumControl(83, 'COMM_SET_CURRENT',0.5)
    #VescCustumControl(83, 'COMM_SET_CURRENT_BRAKE',5)
    #VescCustumControl(83, 'COMM_SET_DPS',0) # position lock on current position
    #VescCustumControl(83, 'COMM_SET_DPS',1000)
    #VescCustumControl(83, 'COMM_SET_DPS_VMAX',2000)
    #VescCustumControl(83, 'COMM_SET_SERVO',0)
    VescCustumControl(83, 'COMM_SET_RELEASE',0)

    # Close PCAN Device
    pcan_Close()

위 코드에서 #(주석)처리된 VescCustumControl(~~~)를 하나씩 실행하여 83번 VESC에 해당 명령어를 CAN 통신을 통해 전송가능함을 확인하였다.