├── .gitignore ├── README.md ├── docs ├── ZLAC8015D RS485通信例程Version 1.00-20201019.pdf └── ZLAC8015D_eng.odt ├── examples ├── test_position_control.py ├── test_read.py ├── test_speed_control.py └── test_speed_control_sineWave.py ├── images ├── motor1.jpg ├── motor2.jpg └── wiring_diagram.png ├── setup.py └── zlac8015d ├── ZLAC8015D.py └── __init__.py /.gitignore: -------------------------------------------------------------------------------- 1 | build/ 2 | dist/ 3 | zlac8015d.egg-info/ 4 | __pycache__/ 5 | 6 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # A Python package of ZLAC8015D AC Servo Driver 2 | 3 | This is a simple python package by using pymodbus to be able to access registers of ZLAC8015D. 4 | 5 | You can find more detail of ZLAC8015D on their official site [here](http://www.zlrobotmotor.com/info/401.html). 6 | 7 | ## Hardware 8 | 9 | ZLAC8015D is only compatible with dual 8 inch motors size. 10 | 11 | ![](images/motor1.jpg) 12 | 13 | ![](images/motor2.jpg) 14 | 15 | Along with the driver, you will need to have RS485-USB converter to plug it on your PC. Please check on the following diagram. 16 | 17 | ![](images/wiring_diagram.png) 18 | 19 | 20 | ## Installation 21 | ```sh 22 | #1. Install dependecies 23 | ## For python2 24 | sudo pip install pymodbus 25 | ## or python3 26 | sudo pip3 install pymodbus 27 | 28 | #2. Install this package 29 | ## For python2 30 | sudo python setup.py install 31 | ## or python3 32 | sudo python3 setup.py install 33 | 34 | #3. add user to dialout group 35 | sudo usermod -a -G dialout $USER 36 | ``` 37 | ## Features 38 | 39 | - Velocity control, we can send command RPMs and also read feedback RPMs from the motors, please check on `test_speed_control.py` 40 | 41 | - Position control, we can send how much angle or even direct distance to travel, in case of we are using default 8 inch wheel the circumference distance would be 0.655 meters. Please check on `test_position_control.py`. 42 | 43 | Those two control modes can be switched during operation, the initialization step has to be done every times when changed to another mode. 44 | 45 | ***Remark*** 46 | 47 | `get_rpm()` can be called in velocity control mode, but couldn't get feedback if in position control mode. 48 | 49 | `get_wheels_travelled()` can be called in both modes. 50 | 51 | `modbus_fail_read_handler()` is a helper function to handle failure read because some there is error of ModbusIOException. 52 | 53 | ## Registers 54 | 55 | For more information of data registers and example packets, please check on [docs](./docs/). -------------------------------------------------------------------------------- /docs/ZLAC8015D RS485通信例程Version 1.00-20201019.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/rasheeddo/ZLAC8015D_python/58f82a4d860ce1635942a21750485f5391bf3a2d/docs/ZLAC8015D RS485通信例程Version 1.00-20201019.pdf -------------------------------------------------------------------------------- /docs/ZLAC8015D_eng.odt: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/rasheeddo/ZLAC8015D_python/58f82a4d860ce1635942a21750485f5391bf3a2d/docs/ZLAC8015D_eng.odt -------------------------------------------------------------------------------- /examples/test_position_control.py: -------------------------------------------------------------------------------- 1 | 2 | from zlac8015d import ZLAC8015D 3 | import time 4 | 5 | motors = ZLAC8015D.Controller(port="/dev/ttyUSB0") 6 | 7 | motors.disable_motor() 8 | 9 | motors.set_accel_time(200,200) 10 | motors.set_decel_time(200,200) 11 | motors.set_maxRPM_pos(100,100) 12 | 13 | motors.set_mode(1) 14 | motors.set_position_async_control() 15 | motors.enable_motor() 16 | 17 | for i in range(10): 18 | 19 | motors.set_relative_angle(90,90) 20 | motors.move_left_wheel() 21 | motors.move_right_wheel() 22 | print(i) 23 | # time.sleep(0.01) 24 | l_meter, r_meter = motors.get_wheels_travelled() 25 | print(l_meter, r_meter) 26 | # reg = motors.get_wheels_travelled() 27 | # print(reg) 28 | time.sleep(2.0) 29 | -------------------------------------------------------------------------------- /examples/test_read.py: -------------------------------------------------------------------------------- 1 | 2 | from zlac8015d import ZLAC8015D 3 | import time 4 | 5 | motors = ZLAC8015D.Controller(port="/dev/ttyUSB0") 6 | 7 | # motors.disable_motor() 8 | 9 | # motors.set_accel_time(1000,1000) 10 | # motors.set_decel_time(1000,1000) 11 | 12 | # motors.set_mode(3) 13 | # motors.enable_motor() 14 | 15 | last_time = time.time() 16 | i = 0 17 | period = 0.0 18 | while True: 19 | try: 20 | # motors.set_rpm(cmds[0],cmds[1]) 21 | l_tick, r_tick = motors.get_wheels_tick() 22 | 23 | print("period: {:.4f} l_tick: {:.1f} | r_tick: {:.1f}".format(period,l_tick,r_tick)) 24 | period = time.time() - last_time 25 | time.sleep(0.01) 26 | 27 | 28 | except KeyboardInterrupt: 29 | # motors.disable_motor() 30 | break 31 | 32 | last_time = time.time() -------------------------------------------------------------------------------- /examples/test_speed_control.py: -------------------------------------------------------------------------------- 1 | 2 | from zlac8015d import ZLAC8015D 3 | import time 4 | 5 | motors = ZLAC8015D.Controller(port="/dev/ttyUSB0") 6 | 7 | motors.disable_motor() 8 | 9 | motors.set_accel_time(1000,1000) 10 | motors.set_decel_time(1000,1000) 11 | 12 | motors.set_mode(3) 13 | motors.enable_motor() 14 | 15 | # cmds = [140, 170] 16 | #cmds = [100, 50] 17 | #cmds = [150, -100] 18 | cmds = [-50, 30] 19 | 20 | motors.set_rpm(cmds[0],cmds[1]) 21 | 22 | last_time = time.time() 23 | i = 0 24 | period = 0.0 25 | while True: 26 | try: 27 | # motors.set_rpm(cmds[0],cmds[1]) 28 | rpmL, rpmR = motors.get_rpm() 29 | 30 | print("period: {:.4f} rpmL: {:.1f} | rpmR: {:.1f}".format(period,rpmL,rpmR)) 31 | period = time.time() - last_time 32 | time.sleep(0.01) 33 | 34 | 35 | except KeyboardInterrupt: 36 | motors.disable_motor() 37 | break 38 | 39 | last_time = time.time() -------------------------------------------------------------------------------- /examples/test_speed_control_sineWave.py: -------------------------------------------------------------------------------- 1 | 2 | from zlac8015d import ZLAC8015D 3 | import time 4 | import numpy as np 5 | import threading 6 | 7 | global motors 8 | 9 | motors = ZLAC8015D.Controller(port="/dev/ttyUSB0") 10 | 11 | motors.disable_motor() 12 | 13 | motors.set_accel_time(50,50) 14 | motors.set_decel_time(50,50) 15 | 16 | motors.set_mode(3) 17 | motors.enable_motor() 18 | 19 | # cmds = [140, 170] 20 | #cmds = [100, 50] 21 | #cmds = [150, -100] 22 | cmds = [-50, 30] 23 | 24 | # motors.set_rpm(cmds[0],cmds[1]) 25 | 26 | 27 | last_time = time.time() 28 | i = 0 29 | period = 0.0 30 | wave_period = 5.0 31 | t = 0.0 32 | last_stamp = time.time() 33 | rpmL_cmd = 0.0 34 | rpmR_cmd = 0.0 35 | inc_t = 0.05 36 | while True: 37 | try: 38 | startTime = time.time() 39 | 40 | ## we change speed at every inc_t 41 | if (time.time() - last_stamp) > inc_t: 42 | rpmL_cmd = 50.0*np.sin((2*np.pi/wave_period)*t) 43 | rpmR_cmd = -50.0*np.sin((2*np.pi/wave_period)*t) 44 | 45 | t += inc_t 46 | if t > wave_period: 47 | t = 0.0 48 | 49 | motors.set_rpm(int(rpmL_cmd), int(rpmR_cmd)) 50 | 51 | last_stamp = time.time() 52 | 53 | rpmL_fb, rpmR_fb = motors.get_rpm() 54 | print("period: {:.5f} t: {:.3f} rpmL_cmd: {:.2f} rpmR_cmd: {:.2f} rpmL_fb: {:.2f} rpmR_fb: {:.2f}".format(\ 55 | period, t, rpmL_cmd, rpmR_cmd, rpmL_fb, rpmR_fb)) 56 | 57 | # print(period) 58 | period = time.time() - startTime 59 | ## this loop takes 0.015 ~ 0.03 seconds for set and get RPM 60 | 61 | except KeyboardInterrupt: 62 | motors.disable_motor() 63 | break 64 | 65 | last_time = time.time() 66 | 67 | -------------------------------------------------------------------------------- /images/motor1.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/rasheeddo/ZLAC8015D_python/58f82a4d860ce1635942a21750485f5391bf3a2d/images/motor1.jpg -------------------------------------------------------------------------------- /images/motor2.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/rasheeddo/ZLAC8015D_python/58f82a4d860ce1635942a21750485f5391bf3a2d/images/motor2.jpg -------------------------------------------------------------------------------- /images/wiring_diagram.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/rasheeddo/ZLAC8015D_python/58f82a4d860ce1635942a21750485f5391bf3a2d/images/wiring_diagram.png -------------------------------------------------------------------------------- /setup.py: -------------------------------------------------------------------------------- 1 | from setuptools import setup 2 | 3 | setup( 4 | name = "zlac8015d", 5 | version = "0.0.1", 6 | author = "Rasheed Kittinanthapanya", 7 | author_email = "rasheedo.kit@gmail.com", 8 | url='https://github.com/rasheeddo/ZLAC8015D_python', 9 | license='GPLv3', 10 | packages=['zlac8015d',], 11 | install_requires = [ 12 | 'pymodbus', 13 | 'numpy' 14 | ] 15 | ) -------------------------------------------------------------------------------- /zlac8015d/ZLAC8015D.py: -------------------------------------------------------------------------------- 1 | 2 | from pymodbus.client.sync import ModbusSerialClient as ModbusClient 3 | import numpy as np 4 | 5 | class Controller: 6 | 7 | def __init__(self, port="/dev/ttyUSB0"): 8 | 9 | self._port = port 10 | 11 | self.client = ModbusClient(method='rtu', port=self._port, baudrate=115200, timeout=1) 12 | 13 | self.client.connect() 14 | 15 | self.ID = 1 16 | 17 | ###################### 18 | ## Register Address ## 19 | ###################### 20 | ## Common 21 | self.CONTROL_REG = 0x200E 22 | self.OPR_MODE = 0x200D 23 | self.L_ACL_TIME = 0x2080 24 | self.R_ACL_TIME = 0x2081 25 | self.L_DCL_TIME = 0x2082 26 | self.R_DCL_TIME = 0x2083 27 | 28 | ## Velocity control 29 | self.L_CMD_RPM = 0x2088 30 | self.R_CMD_RPM = 0x2089 31 | self.L_FB_RPM = 0x20AB 32 | self.R_FB_RPM = 0x20AC 33 | 34 | ## Position control 35 | self.POS_CONTROL_TYPE = 0x200F 36 | 37 | self.L_MAX_RPM_POS = 0x208E 38 | self.R_MAX_RPM_POS = 0x208F 39 | 40 | self.L_CMD_REL_POS_HI = 0x208A 41 | self.L_CMD_REL_POS_LO = 0x208B 42 | self.R_CMD_REL_POS_HI = 0x208C 43 | self.R_CMD_REL_POS_LO = 0x208D 44 | 45 | self.L_FB_POS_HI = 0x20A7 46 | self.L_FB_POS_LO = 0x20A8 47 | self.R_FB_POS_HI = 0x20A9 48 | self.R_FB_POS_LO = 0x20AA 49 | 50 | ## Troubleshooting 51 | self.L_FAULT = 0x20A5 52 | self.R_FAULT = 0x20A6 53 | 54 | ######################## 55 | ## Control CMDs (REG) ## 56 | ######################## 57 | self.EMER_STOP = 0x05 58 | self.ALRM_CLR = 0x06 59 | self.DOWN_TIME = 0x07 60 | self.ENABLE = 0x08 61 | self.POS_SYNC = 0x10 62 | self.POS_L_START = 0x11 63 | self.POS_R_START = 0x12 64 | 65 | #################### 66 | ## Operation Mode ## 67 | #################### 68 | self.POS_REL_CONTROL = 1 69 | self.POS_ABS_CONTROL = 2 70 | self.VEL_CONTROL = 3 71 | 72 | self.ASYNC = 0 73 | self.SYNC = 1 74 | 75 | ################# 76 | ## Fault codes ## 77 | ################# 78 | self.NO_FAULT = 0x0000 79 | self.OVER_VOLT = 0x0001 80 | self.UNDER_VOLT = 0x0002 81 | self.OVER_CURR = 0x0004 82 | self.OVER_LOAD = 0x0008 83 | self.CURR_OUT_TOL = 0x0010 84 | self.ENCOD_OUT_TOL = 0x0020 85 | self.MOTOR_BAD = 0x0040 86 | self.REF_VOLT_ERROR = 0x0080 87 | self.EEPROM_ERROR = 0x0100 88 | self.WALL_ERROR = 0x0200 89 | self.HIGH_TEMP = 0x0400 90 | self.FAULT_LIST = [self.OVER_VOLT, self.UNDER_VOLT, self.OVER_CURR, self.OVER_LOAD, self.CURR_OUT_TOL, self.ENCOD_OUT_TOL, \ 91 | self.MOTOR_BAD, self.REF_VOLT_ERROR, self.EEPROM_ERROR, self.WALL_ERROR, self.HIGH_TEMP] 92 | 93 | ############## 94 | ## Odometry ## 95 | ############## 96 | ## 8 inches wheel 97 | self.travel_in_one_rev = 0.655 98 | self.cpr = 16385 99 | self.R_Wheel = 0.105 #meter 100 | 101 | ## Some time if read immediatly after write, it would show ModbusIOException when get data from registers 102 | def modbus_fail_read_handler(self, ADDR, WORD): 103 | 104 | read_success = False 105 | reg = [None]*WORD 106 | while not read_success: 107 | result = self.client.read_holding_registers(ADDR, WORD, unit=self.ID) 108 | try: 109 | for i in range(WORD): 110 | reg[i] = result.registers[i] 111 | read_success = True 112 | except AttributeError as e: 113 | print(e) 114 | pass 115 | 116 | return reg 117 | 118 | def rpm_to_radPerSec(self, rpm): 119 | return rpm*2*np.pi/60.0 120 | 121 | def rpm_to_linear(self, rpm): 122 | 123 | W_Wheel = self.rpm_to_radPerSec(rpm) 124 | V = W_Wheel*self.R_Wheel 125 | 126 | return V 127 | 128 | def set_mode(self, MODE): 129 | if MODE == 1: 130 | print("Set relative position control") 131 | elif MODE == 2: 132 | print("Set absolute position control") 133 | elif MODE == 3: 134 | print("Set speed rpm control") 135 | else: 136 | print("set_mode ERROR: set only 1, 2, or 3") 137 | return 0 138 | 139 | result = self.client.write_register(self.OPR_MODE, MODE, unit=self.ID) 140 | return result 141 | 142 | def get_mode(self): 143 | 144 | # result = self.client.read_holding_registers(self.OPR_MODE, 1, unit=self.ID) 145 | registers = self.modbus_fail_read_handler(self.OPR_MODE, 1) 146 | 147 | mode = registers[0] 148 | 149 | return mode 150 | 151 | def enable_motor(self): 152 | result = self.client.write_register(self.CONTROL_REG, self.ENABLE, unit=self.ID) 153 | 154 | def disable_motor(self): 155 | result = self.client.write_register(self.CONTROL_REG, self.DOWN_TIME, unit=self.ID) 156 | 157 | def get_fault_code(self): 158 | 159 | fault_codes = self.client.read_holding_registers(self.L_FAULT, 2, unit=self.ID) 160 | 161 | L_fault_code = fault_codes.registers[0] 162 | R_fault_code = fault_codes.registers[1] 163 | 164 | L_fault_flag = L_fault_code in self.FAULT_LIST 165 | R_fault_flag = R_fault_code in self.FAULT_LIST 166 | 167 | return (L_fault_flag, L_fault_code), (R_fault_flag, R_fault_code) 168 | 169 | def clear_alarm(self): 170 | result = self.client.write_register(self.CONTROL_REG, self.ALRM_CLR, unit=self.ID) 171 | 172 | def set_accel_time(self, L_ms, R_ms): 173 | 174 | if L_ms > 32767: 175 | L_ms = 32767 176 | elif L_ms < 0: 177 | L_ms = 0 178 | 179 | if R_ms > 32767: 180 | R_ms = 32767 181 | elif R_ms < 0: 182 | R_ms = 0 183 | 184 | result = self.client.write_registers(self.L_ACL_TIME, [int(L_ms),int(R_ms)], unit=self.ID) 185 | 186 | 187 | def set_decel_time(self, L_ms, R_ms): 188 | 189 | if L_ms > 32767: 190 | L_ms = 32767 191 | elif L_ms < 0: 192 | L_ms = 0 193 | 194 | if R_ms > 32767: 195 | R_ms = 32767 196 | elif R_ms < 0: 197 | R_ms = 0 198 | 199 | result = self.client.write_registers(self.L_DCL_TIME, [int(L_ms), int(R_ms)], unit=self.ID) 200 | 201 | def int16Dec_to_int16Hex(self,int16): 202 | 203 | lo_byte = (int16 & 0x00FF) 204 | hi_byte = (int16 & 0xFF00) >> 8 205 | 206 | all_bytes = (hi_byte << 8) | lo_byte 207 | 208 | return all_bytes 209 | 210 | 211 | def set_rpm(self, L_rpm, R_rpm): 212 | 213 | if L_rpm > 3000: 214 | L_rpm = 3000 215 | elif L_rpm < -3000: 216 | L_rpm = -3000 217 | 218 | if R_rpm > 3000: 219 | R_rpm = 3000 220 | elif R_rpm < -3000: 221 | R_rpm = -3000 222 | 223 | left_bytes = self.int16Dec_to_int16Hex(L_rpm) 224 | right_bytes = self.int16Dec_to_int16Hex(R_rpm) 225 | 226 | result = self.client.write_registers(self.L_CMD_RPM, [left_bytes, right_bytes], unit=self.ID) 227 | 228 | def get_rpm(self): 229 | 230 | 231 | # rpms = self.client.read_holding_registers(self.L_FB_RPM, 2, unit=self.ID) 232 | # fb_L_rpm = np.int16(rpms.registers[0])/10.0 233 | # fb_R_rpm = np.int16(rpms.registers[1])/10.0 234 | 235 | registers = self.modbus_fail_read_handler(self.L_FB_RPM, 2) 236 | fb_L_rpm = np.int16(registers[0])/10.0 237 | fb_R_rpm = np.int16(registers[1])/10.0 238 | 239 | return fb_L_rpm, fb_R_rpm 240 | 241 | def get_linear_velocities(self): 242 | 243 | rpmL, rpmR = self.get_rpm() 244 | 245 | VL = self.rpm_to_linear(rpmL) 246 | VR = self.rpm_to_linear(-rpmR) 247 | 248 | return VL, VR 249 | 250 | def map(self, val, in_min, in_max, out_min, out_max): 251 | 252 | return (val - in_min) * (out_max - out_min) / (in_max - in_min) + out_min 253 | 254 | def set_maxRPM_pos(self, max_L_rpm, max_R_rpm): 255 | 256 | if max_L_rpm > 1000: 257 | max_L_rpm = 1000 258 | elif max_L_rpm < 1: 259 | max_L_rpm = 1 260 | 261 | if max_R_rpm > 1000: 262 | max_R_rpm = 1000 263 | elif max_R_rpm < 1: 264 | max_R_rpm = 1 265 | 266 | result = self.client.write_registers(self.L_MAX_RPM_POS, [int(max_L_rpm), int(max_R_rpm)], unit=self.ID) 267 | 268 | def set_position_async_control(self): 269 | 270 | result = self.client.write_register(self.POS_CONTROL_TYPE, self.ASYNC, unit=self.ID) 271 | 272 | def move_left_wheel(self): 273 | 274 | result = self.client.write_register(self.CONTROL_REG, self.POS_L_START, unit=self.ID) 275 | 276 | def move_right_wheel(self): 277 | 278 | result = self.client.write_register(self.CONTROL_REG, self.POS_R_START, unit=self.ID) 279 | 280 | def deg_to_32bitArray(self, deg): 281 | 282 | dec = int(self.map(deg, -1440, 1440, -65536, 65536)) 283 | HI_WORD = (dec & 0xFFFF0000) >> 16 284 | LO_WORD = dec & 0x0000FFFF 285 | 286 | return [HI_WORD, LO_WORD] 287 | 288 | def set_relative_angle(self, ang_L, ang_R): 289 | 290 | L_array = self.deg_to_32bitArray(ang_L) 291 | R_array = self.deg_to_32bitArray(ang_R) 292 | all_cmds_array = L_array + R_array 293 | 294 | result = self.client.write_registers(self.L_CMD_REL_POS_HI, all_cmds_array, unit=self.ID) 295 | 296 | def get_wheels_travelled(self): 297 | 298 | # read_success = False 299 | # while not read_success: 300 | 301 | # result = self.client.read_holding_registers(self.L_FB_POS_HI, 4, unit=self.ID) 302 | # try: 303 | # l_pul_hi = result.registers[0] 304 | # l_pul_lo = result.registers[1] 305 | # r_pul_hi = result.registers[2] 306 | # r_pul_lo = result.registers[3] 307 | 308 | # l_pulse = ((l_pul_hi & 0xFFFF) << 16) | (l_pul_lo & 0xFFFF) 309 | # r_pulse = ((r_pul_hi & 0xFFFF) << 16) | (r_pul_lo & 0xFFFF) 310 | # l_travelled = (float(l_pulse)/self.cpr)*self.travel_in_one_rev # unit in meter 311 | # r_travelled = (float(r_pulse)/self.cpr)*self.travel_in_one_rev # unit in meter 312 | 313 | # read_success = True 314 | 315 | # except AttributeError: 316 | # # print("error") 317 | # pass 318 | 319 | registers = self.modbus_fail_read_handler(self.L_FB_POS_HI, 4) 320 | l_pul_hi = registers[0] 321 | l_pul_lo = registers[1] 322 | r_pul_hi = registers[2] 323 | r_pul_lo = registers[3] 324 | 325 | l_pulse = np.int32(((l_pul_hi & 0xFFFF) << 16) | (l_pul_lo & 0xFFFF)) 326 | r_pulse = np.int32(((r_pul_hi & 0xFFFF) << 16) | (r_pul_lo & 0xFFFF)) 327 | l_travelled = (float(l_pulse)/self.cpr)*self.travel_in_one_rev # unit in meter 328 | r_travelled = (float(r_pulse)/self.cpr)*self.travel_in_one_rev # unit in meter 329 | 330 | return l_travelled, r_travelled 331 | 332 | def get_wheels_tick(self): 333 | 334 | registers = self.modbus_fail_read_handler(self.L_FB_POS_HI, 4) 335 | l_pul_hi = registers[0] 336 | l_pul_lo = registers[1] 337 | r_pul_hi = registers[2] 338 | r_pul_lo = registers[3] 339 | 340 | l_tick = np.int32(((l_pul_hi & 0xFFFF) << 16) | (l_pul_lo & 0xFFFF)) 341 | r_tick = np.int32(((r_pul_hi & 0xFFFF) << 16) | (r_pul_lo & 0xFFFF)) 342 | 343 | return l_tick, r_tick 344 | -------------------------------------------------------------------------------- /zlac8015d/__init__.py: -------------------------------------------------------------------------------- 1 | from zlac8015d import ZLAC8015D --------------------------------------------------------------------------------