Control with Python
We describe the VESC control method using python.

1. Using CAN Custom Control Message

English
Korean
โ€‹
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 ์ „์†ก ๋ฐฉ์‹์œผ๋กœ ๋ถ€๋ฅด๋„๋ก ํ•œ๋‹ค.
1
// CAN commands
2
typedef enum {
3
CAN_PACKET_SET_DUTY = 0,
4
CAN_PACKET_SET_CURRENT,
5
CAN_PACKET_SET_CURRENT_BRAKE,
6
CAN_PACKET_SET_RPM,
7
CAN_PACKET_SET_POS,
8
CAN_PACKET_FILL_RX_BUFFER,
9
CAN_PACKET_FILL_RX_BUFFER_LONG,
10
CAN_PACKET_PROCESS_RX_BUFFER,
11
CAN_PACKET_PROCESS_SHORT_BUFFER,
12
CAN_PACKET_STATUS,
13
CAN_PACKET_SET_CURRENT_REL,
14
CAN_PACKET_SET_CURRENT_BRAKE_REL,
15
CAN_PACKET_SET_CURRENT_HANDBRAKE,
16
CAN_PACKET_SET_CURRENT_HANDBRAKE_REL,
17
CAN_PACKET_STATUS_2,
18
CAN_PACKET_STATUS_3,
19
CAN_PACKET_STATUS_4,
20
CAN_PACKET_PING,
21
CAN_PACKET_PONG,
22
CAN_PACKET_DETECT_APPLY_ALL_FOC,
23
CAN_PACKET_DETECT_APPLY_ALL_FOC_RES,
24
CAN_PACKET_CONF_CURRENT_LIMITS,
25
CAN_PACKET_CONF_STORE_CURRENT_LIMITS,
26
CAN_PACKET_CONF_CURRENT_LIMITS_IN,
27
CAN_PACKET_CONF_STORE_CURRENT_LIMITS_IN,
28
CAN_PACKET_CONF_FOC_ERPMS,
29
CAN_PACKET_CONF_STORE_FOC_ERPMS,
30
CAN_PACKET_STATUS_5,
31
CAN_PACKET_POLL_TS5700N8501_STATUS,
32
CAN_PACKET_CONF_BATTERY_CUT,
33
CAN_PACKET_CONF_STORE_BATTERY_CUT,
34
CAN_PACKET_SHUTDOWN,
35
CAN_PACKET_IO_BOARD_ADC_1_TO_4,
36
CAN_PACKET_IO_BOARD_ADC_5_TO_8,
37
CAN_PACKET_IO_BOARD_ADC_9_TO_12,
38
CAN_PACKET_IO_BOARD_DIGITAL_IN,
39
CAN_PACKET_IO_BOARD_SET_OUTPUT_DIGITAL,
40
CAN_PACKET_IO_BOARD_SET_OUTPUT_PWM,
41
CAN_PACKET_BMS_V_TOT,
42
CAN_PACKET_BMS_I,
43
CAN_PACKET_BMS_AH_WH,
44
CAN_PACKET_BMS_V_CELL,
45
CAN_PACKET_BMS_BAL,
46
CAN_PACKET_BMS_TEMPS,
47
CAN_PACKET_BMS_HUM,
48
CAN_PACKET_BMS_SOC_SOH_TEMP_STAT
49
} CAN_PACKET_ID;
Copied!
CAN Custom Control Message ์ „์†ก ๋ฐฉ์‹์€ ์•„๋ž˜์™€ ๊ฐ™๋‹ค. ์•„๋ž˜๋Š” USBํ†ต์‹ ์„ ํ†ตํ•ด CAN Custom Control Message ์ „์†ก ๋ฐฉ์‹์œผ๋กœ ๋ณด๋‚ด์ง„ ๋ฐ์ดํ„ฐ์˜ ์˜ˆ๋กœ ์ด๋ฅผ ํ•ด์„ํ•˜๋ฉด ๋‹ค์Œ๊ณผ ๊ฐ™๋‹ค.
์ฒซ๋ฒˆ์งธ, ๋‘๋ฒˆ์งธ ๋ฉ”์„ธ์ง€ ํ•ด์„
์„ธ๋ฒˆ์งธ ๋ฉ”์„ธ์ง€ ํ•ด์„
์•ž์„  USB๋ฅผ ํ†ตํ•ด ์ „์†ก๋œ CAN Custom Control Message์˜ ํ•ด์„๊ฒฐ๊ณผ 101๋ฒˆ ๋ชจํ„ฐ์— DPS ์ œ์–ด๋ฅผ ๊ฐ€ํ•˜๋Š” ๋ช…๋ น์–ด์ž„์„ ์•Œ ์ˆ˜ ์žˆ๋‹ค. ์ด๋ฅผ ํ†ตํ•ด ์ผ๋ฐ˜์ ์œผ๋กœ ์‚ฌ์šฉ๊ฐ€๋Šฅํ•œ CAN Custom Control Message์˜ ํฌ๋ฉง์„ ์ƒ๊ฐํ•ด๋ณผ ์ˆ˜ ์žˆ๊ณ  ๋‹ค์Œ๊ณผ ๊ฐ™์€ ํ˜•ํƒœ๋ฅผ ์‚ฌ์šฉํ•˜๋ฉด ๋œ๋‹ค.
๋ณ€์ˆ˜
๋‚ด์šฉ
ID
๋ชฉ์ ํ•˜๋Š” VESC ID์˜ Hex ๊ฐ’
CS
์ œ์–ด๋ฐฉ์‹์„ ๋‚˜ํƒ€๋‚ด๋Š” 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)
1
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์— ์กด์žฌํ•œ๋‹ค..
1
from PCANBasic import *
2
โ€‹
3
'''
4
# crc check program from vesc firmware, dll file generated by below command
5
> gcc -fPIC -c crc.c
6
> gcc --shared -ocrc_vesc.dll crc.o
7
'''
8
import ctypes
9
crc_vesc = ctypes.cdll.LoadLibrary('./crc_vesc.dll')
10
โ€‹
11
can_packet_id = {
12
'CAN_PACKET_SET_DUTY':0,
13
'CAN_PACKET_SET_CURRENT':1,
14
'CAN_PACKET_SET_CURRENT_BRAKE':2,
15
'CAN_PACKET_SET_RPM':3,
16
'CAN_PACKET_SET_POS':4,
17
'CAN_PACKET_FILL_RX_BUFFER':5,
18
'CAN_PACKET_FILL_RX_BUFFER_LONG':6,
19
'CAN_PACKET_PROCESS_RX_BUFFER':7,
20
'CAN_PACKET_PROCESS_SHORT_BUFFER':8,
21
'CAN_PACKET_STATUS':9
22
}
23
โ€‹
24
comm_set_custom = {
25
'COMM_SET_DUTY':5,
26
'COMM_SET_CURRENT':6,
27
'COMM_SET_CURRENT_BRAKE':7,
28
'COMM_SET_RELEASE':100,
29
'COMM_SET_DPS':101,
30
'COMM_SET_DPS_VMAX':102,
31
'COMM_SET_DPS_AMAX':103,
32
'COMM_SET_SERVO':104,
33
'COMM_SET_TRAJ':105
34
}
35
โ€‹
36
global pc1
37
โ€‹
38
time_prev = 0
39
msg_type = {'0x0':'PCAN_MESSAGE_STANDARD',
40
'0x00':'PCAN_MESSAGE_STANDARD',
41
'0x1':'PCAN_MESSAGE_RTR',
42
'0x01':'PCAN_MESSAGE_RTR',
43
'0x2':'PCAN_MESSAGE_EXTENDED',
44
'0x02':'PCAN_MESSAGE_EXTENDED',
45
'0x4':'PCAN_MESSAGE_FD',
46
'0x04':'PCAN_MESSAGE_FD',
47
'0x8':'PCAN_MESSAGE_BRS',
48
'0x08':'PCAN_MESSAGE_BRS',
49
'0x10':'PCAN_MESSAGE_ESI',
50
'0x40':'PCAN_MESSAGE_ERRFRAME',
51
'0x80':'PCAN_MESSAGE_STATUS'}
52
โ€‹
53
โ€‹
54
def pcan_Open():
55
result = pc1.Initialize(PCAN_USBBUS1, PCAN_BAUD_1M)
56
if result != PCAN_ERROR_OK:
57
# An error occurred, get a text describing the error and show it
58
#
59
result = pc1.GetErrorText(result)
60
print(result[1])
61
else:
62
print("PCAN-USB (Ch-1) was initialized")
63
โ€‹
64
def pcan_Close():
65
# The USB Channel is released
66
#
67
result = pc1.Uninitialize(PCAN_USBBUS1)
68
if result != PCAN_ERROR_OK:
69
# An error occurred, get a text describing the error and show it
70
#
71
result = pc1.GetErrorText(result)
72
print(result[1])
73
else:
74
print("PCAN-USB (Ch-1) was released")
75
โ€‹
76
def ReadMessage():
77
# We execute the "Read" function of the PCANBasic
78
#
79
result = pc1.Read(PCAN_USBBUS1)
80
โ€‹
81
if result[0] == PCAN_ERROR_OK:
82
# We show the received message
83
#
84
ProcessMessage(result[1:])
85
86
return result[0]
87
โ€‹
88
def ReadMessages():
89
result = PCAN_ERROR_OK,
90
while (result[0] & PCAN_ERROR_QRCVEMPTY) != PCAN_ERROR_QRCVEMPTY:
91
result = pc1.Read(PCAN_USBBUS1)
92
if result[0] != PCAN_ERROR_QRCVEMPTY:
93
ProcessMessage(result[1:])
94
else:
95
if(result[0] != PCAN_ERROR_QRCVEMPTY): print('ERROR_CODE:{}'.format(hex(result[0])))
96
โ€‹
97
def ProcessMessage(*args):
98
global time_prev
99
โ€‹
100
theMsg = args[0][0]
101
itsTimeStamp = args[0][1]
102
โ€‹
103
newMsg = TPCANMsgFD()
104
newMsg.ID = theMsg.ID
105
newMsg.DLC = theMsg.LEN
106
for i in range(8 if (theMsg.LEN > 8) else theMsg.LEN):
107
newMsg.DATA[i] = theMsg.DATA[i]
108
newMsg.MSGTYPE = theMsg.MSGTYPE
109
newTimestamp = TPCANTimestampFD()
110
newTimestamp.value = (itsTimeStamp.micros + 1000 * itsTimeStamp.millis + 0x100000000 * 1000 * itsTimeStamp.millis_overflow)
111
โ€‹
112
time = "Timestamp:{:0.3f}sec".format(newTimestamp.value/1000000)
113
period = newTimestamp.value - time_prev
114
cycle_time = "Cycle Time:{:0.3f}msec".format(period/1000)
115
TYPE = "TYPE:{}".format(msg_type[hex(newMsg.MSGTYPE)])
116
EID = "EID:{}".format(hex(newMsg.ID))
117
DLC = "DLC:{}".format(newMsg.DLC)
118
DATA = ' '.join('{:02x}'.format(newMsg.DATA[i]) for i in range(newMsg.DLC))
119
โ€‹
120
if newMsg.MSGTYPE == 0x02: # PCAN_MESSAGE_EXTEND
121
print(time,"|",TYPE,"|",EID,"|",DLC,"|",DATA,"|",cycle_time)
122
123
time_prev = newTimestamp.value
124
โ€‹
125
def WriteFrame(id, dlc, data):
126
CANMsg = TPCANMsg()
127
โ€‹
128
CANMsg.ID = id
129
CANMsg.LEN = dlc
130
CANMsg.MSGTYPE = PCAN_MESSAGE_EXTENDED
131
โ€‹
132
for i in range(CANMsg.LEN):
133
CANMsg.DATA[i] = data[i]
134
โ€‹
135
return pc1.Write(PCAN_USBBUS1, CANMsg)
136
โ€‹
137
def GetFormatedError(error):
138
stsReturn = pc1.GetErrorText(error, 0)
139
if stsReturn[0] != PCAN_ERROR_OK:
140
return "An error occurred. Error-code's text ({0:X}h) couldn't be retrieved".format(error)
141
else:
142
return stsReturn[1]
143
โ€‹
144
def VescCustumControl(target_vesc_id, ctrl_type, ctrl_value):
145
'''
146
# Custom Control CAN Msg Frame Structure Example, id=0x53, comm_set_custom=0x65=101=COMM_SET_DPS, data=2000000
147
WriteFrame(0x00000553,8,[0x00, 0x24, 0x05, 0x01, 0xFF, 0x65, 0x00, 0x1E])
148
WriteFrame(0x00000553,3,[0x07, 0x84, 0x80])
149
WriteFrame(0x00000753,6,[0x53, 0x00, 0x00, 0x09, 0xBB, 0x4E])
150
'''
151
152
if ctrl_type == 'COMM_SET_DUTY':
153
value = int(ctrl_value * 100000.0)
154
elif ctrl_type == 'COMM_SET_POS':
155
value = int(ctrl_value * 1000000.0)
156
elif ctrl_type == 'COMM_SET_RPM' or ctrl_type == 'COMM_SET_RELEASE':
157
value = int(ctrl_value)
158
else:
159
value = int(ctrl_value * 1000.0)
160
โ€‹
161
command = comm_set_custom[ctrl_type]
162
โ€‹
163
d1 = (value >> 24) & 0xFF
164
d2 = (value >> 16) & 0xFF
165
d3 = (value >> 8) & 0xFF
166
d4 = value & 0xFF
167
data_arr = [d1, d2, d3, d4]
168
169
# Frame1, Frame2 is for 'CAN_PACKET_FILL_RX_BUFFER(0x05)'
170
eid1 = (can_packet_id['CAN_PACKET_FILL_RX_BUFFER'] << 8) | target_vesc_id
171
dlc1 = 8
172
# 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]
173
Frame1 = [0x00, 0x24, 0x06, 0x01, 0xFF, command, d1, d2]
174
โ€‹
175
eid2 = (can_packet_id['CAN_PACKET_FILL_RX_BUFFER'] << 8) | target_vesc_id
176
dlc2 = 3
177
# Frame2 = [0x07(length index), d3, d4]
178
Frame2 = [0x07, d3, d4]
179
โ€‹
180
# crc calculation only for 9byte data frame (that is except 0x00, 0x07)
181
data_arr = Frame1[1:] + Frame2[1:]
182
arr = (ctypes.c_ubyte * len(data_arr))(*data_arr)
183
โ€‹
184
crc = crc_vesc.crc16(arr,9)
185
crch = (crc >> 8) & 0xFF
186
crcl = crc & 0xFF
187
โ€‹
188
# Frame3 is for 'CAN_PACKET_PROCESS_RX_BUFFER(0x07)'
189
eid3 = (can_packet_id['CAN_PACKET_PROCESS_RX_BUFFER'] << 8) | target_vesc_id
190
dlc3 = 6
191
# Frame2 = [target_vesc_id, send, data_length high, data_length low, crch, crcl]
192
Frame3 = [target_vesc_id, 0x00, 0x00, 0x09, crch, crcl]
193
โ€‹
194
'''
195
# debug print of can msgs
196
print('Frame1 - eid:', format(hex(eid1)), '| dlc:', format(hex(dlc1)), '| data:', '[{}]'.format(', '.join(hex(x) for x in Frame1)) )
197
print('Frame2 - eid:', format(hex(eid2)), '| dlc:', format(hex(dlc2)), '| data:', '[{}]'.format(', '.join(hex(x) for x in Frame2)) )
198
print('Frame3 - eid:', format(hex(eid3)), '| dlc:', format(hex(dlc3)), '| data:', '[{}]'.format(', '.join(hex(x) for x in Frame3)) )
199
'''
200
โ€‹
201
WriteFrame(eid1, dlc1, Frame1)
202
WriteFrame(eid2, dlc2, Frame2)
203
WriteFrame(eid3, dlc3, Frame3)
204
205
print('Run VESC CAN Direct Control,', ctrl_type,':',ctrl_value)
206
โ€‹
207
โ€‹
208
if __name__ == '__main__':
209
pc1 = PCANBasic()
210
โ€‹
211
# Open PCAN Device
212
pcan_Open()
213
โ€‹
214
'''
215
# CAN MSG Read Routine
216
num = 0
217
# try~ except ํŠน์ • ์˜ˆ์™ธ
218
try:
219
# ๋ฌดํ•œ ๋ฐ˜๋ณต
220
while True:
221
ReadMessages()
222
num += 1
223
# Ctrl + C๋ฅผ ์ž…๋ ฅํ•  ๊ฒฝ์šฐ
224
except KeyboardInterrupt:
225
print('Total Rcv number is {}, Quit to receive'.format(num))
226
'''
227
โ€‹
228
# CAN MSG Write Routine
229
#VescCustumControl(83, 'COMM_SET_DUTY',0.1)
230
#VescCustumControl(83, 'COMM_SET_CURRENT',0.5)
231
#VescCustumControl(83, 'COMM_SET_CURRENT_BRAKE',5)
232
#VescCustumControl(83, 'COMM_SET_DPS',0) # position lock on current position
233
#VescCustumControl(83, 'COMM_SET_DPS',1000)
234
#VescCustumControl(83, 'COMM_SET_DPS_VMAX',2000)
235
#VescCustumControl(83, 'COMM_SET_SERVO',0)
236
VescCustumControl(83, 'COMM_SET_RELEASE',0)
237
โ€‹
238
# Close PCAN Device
239
pcan_Close()
Copied!
์œ„ ์ฝ”๋“œ์—์„œ #(์ฃผ์„)์ฒ˜๋ฆฌ๋œ VescCustumControl(~~~)๋ฅผ ํ•˜๋‚˜์”ฉ ์‹คํ–‰ํ•˜์—ฌ 83๋ฒˆ VESC์— ํ•ด๋‹น ๋ช…๋ น์–ด๋ฅผ CAN ํ†ต์‹ ์„ ํ†ตํ•ด ์ „์†ก๊ฐ€๋Šฅํ•จ์„ ํ™•์ธํ•˜์˜€๋‹ค.
โ€‹
โ€‹
โ€‹
โ€‹
โ€‹
โ€‹
Last modified 7mo ago
Copy link