diff --git a/Dockerfile b/Dockerfile
index 21193d5..8337363 100644
--- a/Dockerfile
+++ b/Dockerfile
@@ -34,6 +34,7 @@
 RUN apt-get update && \
     apt-get upgrade -y && \
     apt-get install --no-install-recommends -y \
+    python3-pip \
     gedit \
     ros-noetic-rosserial \
     ros-noetic-rosserial-arduino \
@@ -73,4 +74,9 @@
 COPY src /home/ubuntu/catkin_ws/src
 
 
+RUN pip3 install pymodbus && \
+    cd /home/ubuntu/catkin_ws/src/ZLAC8015D_python && \
+    usermod -a -G dialout $USER
+
+
 COPY ./startup.sh /startup.sh
diff --git a/config/70-sensors.rules b/config/70-sensors.rules
new file mode 100644
index 0000000..750f68f
--- /dev/null
+++ b/config/70-sensors.rules
@@ -0,0 +1 @@
+SUBSYSTEMS=="usb", KERNEL=="ttyUSB[0-9]*", ACTION=="add", ATTRS{idVendor}=="10c4", ATTRS{idProduct}=="ea60", MODE="666", SYMLINK+="ZLAC8015D", GROUP="dialout"
diff --git a/copy2host.sh b/copy2host.sh
new file mode 100755
index 0000000..7de6d38
--- /dev/null
+++ b/copy2host.sh
@@ -0,0 +1 @@
+docker cp $(docker ps -q):/home/ubuntu/catkin_ws/src .
diff --git a/runMotor.sh b/runMotor.sh
new file mode 100755
index 0000000..74131de
--- /dev/null
+++ b/runMotor.sh
@@ -0,0 +1,18 @@
+if [ -n "$1" ]; then
+	TAG=$1
+else
+	read REPOSITORY TAG ID CREATED SIZE < <(docker images | grep bvbnatsu)
+fi
+
+echo "Starting bvbnatsu/tsukuba2022:$TAG"
+
+docker run --rm \
+	-p 6080:80 \
+	-p 2222:22 \
+        -p 10940:10940 \
+        -p 2368:2368/udp \
+        -p 8308:8308/udp \
+	-e RESOLUTION=1920x1080 --shm-size=512m \
+	-e HOME=/home/ubuntu -e SHELL=/bin/bash \
+	--device /dev/ZLAC8015D:/dev/ZLAC8015D:mwr \
+	--entrypoint '/startup.sh' bvbnatsu/tsukuba2022:$TAG 
diff --git a/src/ZLAC8015D_python/.gitignore b/src/ZLAC8015D_python/.gitignore
new file mode 100644
index 0000000..b08ba21
--- /dev/null
+++ b/src/ZLAC8015D_python/.gitignore
@@ -0,0 +1,5 @@
+build/
+dist/
+zlac8015d.egg-info/
+__pycache__/
+
diff --git a/src/ZLAC8015D_python/README.md b/src/ZLAC8015D_python/README.md
new file mode 100644
index 0000000..b7f4f41
--- /dev/null
+++ b/src/ZLAC8015D_python/README.md
@@ -0,0 +1,55 @@
+# A Python package of ZLAC8015D AC Servo Driver
+
+This is a simple python package by using pymodbus to be able to access registers of ZLAC8015D.
+
+You can find more detail of ZLAC8015D on their official site [here](http://www.zlrobotmotor.com/info/401.html).
+
+## Hardware
+
+ZLAC8015D is only compatible with dual 8 inch motors size.
+
+![](images/motor1.jpg)
+
+![](images/motor2.jpg)
+
+Along with the driver, you will need to have RS485-USB converter to plug it on your PC. Please check on the following diagram.
+
+![](images/wiring_diagram.png)
+
+
+## Installation
+```sh
+#1. Install dependecies
+## For python2
+sudo pip install pymodbus
+## or python3
+sudo pip3 install pymodbus
+
+#2. Install this package
+## For python2
+sudo python setup.py install
+## or python3
+sudo python3 setup.py install
+
+#3. add user to dialout group
+sudo usermod -a -G dialout $USER
+```
+## Features
+
+- Velocity control, we can send command RPMs and also read feedback RPMs from the motors, please check on `test_speed_control.py`
+
+- 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`.
+
+Those two control modes can be switched during operation, the initialization step has to be done every times when changed to another mode.
+
+***Remark***
+
+`get_rpm()` can be called in velocity control mode, but couldn't get feedback if in position control mode.
+
+`get_wheels_travelled()` can be called in both modes.
+
+`modbus_fail_read_handler()` is a helper function to handle failure read because some there is error of ModbusIOException.
+
+## Registers
+
+For more information of data registers and example packets, please check on [docs](./docs/).
\ No newline at end of file
diff --git "a/src/ZLAC8015D_python/docs/ZLAC8015D RS485\351\200\232\344\277\241\344\276\213\347\250\213Version 1.00-20201019.pdf" "b/src/ZLAC8015D_python/docs/ZLAC8015D RS485\351\200\232\344\277\241\344\276\213\347\250\213Version 1.00-20201019.pdf"
new file mode 100644
index 0000000..df0db79
--- /dev/null
+++ "b/src/ZLAC8015D_python/docs/ZLAC8015D RS485\351\200\232\344\277\241\344\276\213\347\250\213Version 1.00-20201019.pdf"
Binary files differ
diff --git a/src/ZLAC8015D_python/docs/ZLAC8015D_eng.odt b/src/ZLAC8015D_python/docs/ZLAC8015D_eng.odt
new file mode 100644
index 0000000..4baa3ae
--- /dev/null
+++ b/src/ZLAC8015D_python/docs/ZLAC8015D_eng.odt
Binary files differ
diff --git a/src/ZLAC8015D_python/examples/test_position_control.py b/src/ZLAC8015D_python/examples/test_position_control.py
new file mode 100644
index 0000000..d52cf7e
--- /dev/null
+++ b/src/ZLAC8015D_python/examples/test_position_control.py
@@ -0,0 +1,28 @@
+
+from zlac8015d import ZLAC8015D
+import time
+
+motors = ZLAC8015D.Controller(port="/dev/ttyUSB0")
+
+motors.disable_motor()
+
+motors.set_accel_time(200,200)
+motors.set_decel_time(200,200)
+motors.set_maxRPM_pos(100,100)
+
+motors.set_mode(1)
+motors.set_position_async_control()
+motors.enable_motor()
+
+for i in range(10):
+
+	motors.set_relative_angle(90,90)
+	motors.move_left_wheel()
+	motors.move_right_wheel()
+	print(i)
+	# time.sleep(0.01)
+	l_meter, r_meter = motors.get_wheels_travelled()
+	print(l_meter, r_meter)
+	# reg = motors.get_wheels_travelled()
+	# print(reg)
+	time.sleep(2.0)
diff --git a/src/ZLAC8015D_python/examples/test_speed_control.py b/src/ZLAC8015D_python/examples/test_speed_control.py
new file mode 100644
index 0000000..fcff88c
--- /dev/null
+++ b/src/ZLAC8015D_python/examples/test_speed_control.py
@@ -0,0 +1,41 @@
+
+from zlac8015d import ZLAC8015D
+import importlib
+importlib.reload(ZLAC8015D)
+import time
+
+motors = ZLAC8015D.Controller(port="/dev/ZLAC8015D")
+
+motors.disable_motor()
+
+motors.set_accel_time(500,500)
+motors.set_decel_time(1000,1000)
+
+motors.set_mode(3)
+motors.enable_motor()
+
+cmds = [-140, 170]
+#cmds = [100, 50]
+#cmds = [150, -100]
+#cmds = [-50, 30]
+
+motors.set_rpm(cmds[0],cmds[1])
+
+last_time = time.time()
+i = 0
+while True:
+	try:
+		period = time.time() - last_time
+		# motors.set_rpm(cmds[0],cmds[1])
+		rpmL, rpmR = motors.get_rpm()
+
+		#print("period: {:.4f} rpmL: {:.1f} | rpmR: {:.1f}".format(period,rpmL,rpmR))
+		print("{:.4f},{:.1f},{:.1f}".format(period,rpmL,rpmR))
+		time.sleep(0.01)
+		
+
+	except KeyboardInterrupt:
+		motors.disable_motor()
+		break
+
+	#last_time = time.time()
diff --git a/src/ZLAC8015D_python/images/motor1.jpg b/src/ZLAC8015D_python/images/motor1.jpg
new file mode 100644
index 0000000..67132c1
--- /dev/null
+++ b/src/ZLAC8015D_python/images/motor1.jpg
Binary files differ
diff --git a/src/ZLAC8015D_python/images/motor2.jpg b/src/ZLAC8015D_python/images/motor2.jpg
new file mode 100644
index 0000000..d9c3cfb
--- /dev/null
+++ b/src/ZLAC8015D_python/images/motor2.jpg
Binary files differ
diff --git a/src/ZLAC8015D_python/images/wiring_diagram.png b/src/ZLAC8015D_python/images/wiring_diagram.png
new file mode 100644
index 0000000..440faab
--- /dev/null
+++ b/src/ZLAC8015D_python/images/wiring_diagram.png
Binary files differ
diff --git a/src/ZLAC8015D_python/setup.py b/src/ZLAC8015D_python/setup.py
new file mode 100644
index 0000000..65e835f
--- /dev/null
+++ b/src/ZLAC8015D_python/setup.py
@@ -0,0 +1,15 @@
+from setuptools import setup
+
+setup(
+    name = "zlac8015d",
+    version = "0.0.1",
+    author = "Rasheed Kittinanthapanya",
+    author_email = "rasheedo.kit@gmail.com",
+    url='https://github.com/rasheeddo/ZLAC8015D_python', 
+    license='GPLv3',
+    packages=['zlac8015d',],
+    install_requires = [
+        'pymodbus',
+        'numpy'
+    ]
+)
\ No newline at end of file
diff --git a/src/ZLAC8015D_python/zlac8015d/ZLAC8015D.py b/src/ZLAC8015D_python/zlac8015d/ZLAC8015D.py
new file mode 100644
index 0000000..787c593
--- /dev/null
+++ b/src/ZLAC8015D_python/zlac8015d/ZLAC8015D.py
@@ -0,0 +1,330 @@
+
+from pymodbus.client.sync import ModbusSerialClient as ModbusClient
+import numpy as np
+
+class Controller:
+
+	def __init__(self, port="/dev/ttyUSB0"):
+
+		self._port = port
+
+		self.client = ModbusClient(method='rtu', port=self._port, baudrate=115200, timeout=1)
+
+		self.client.connect()
+
+		self.ID = 1
+
+		######################
+		## Register Address ##
+		######################
+		## Common
+		self.CONTROL_REG = 0x200E
+		self.OPR_MODE = 0x200D
+		self.L_ACL_TIME = 0x2080
+		self.R_ACL_TIME = 0x2081
+		self.L_DCL_TIME = 0x2082
+		self.R_DCL_TIME = 0x2083
+
+		## Velocity control
+		self.L_CMD_RPM = 0x2088
+		self.R_CMD_RPM = 0x2089
+		self.L_FB_RPM = 0x20AB
+		self.R_FB_RPM = 0x20AC
+
+		## Position control
+		self.POS_CONTROL_TYPE = 0x200F
+
+		self.L_MAX_RPM_POS = 0x208E
+		self.R_MAX_RPM_POS = 0x208F
+
+		self.L_CMD_REL_POS_HI = 0x208A
+		self.L_CMD_REL_POS_LO = 0x208B
+		self.R_CMD_REL_POS_HI = 0x208C
+		self.R_CMD_REL_POS_LO = 0x208D
+
+		self.L_FB_POS_HI = 0x20A7
+		self.L_FB_POS_LO = 0x20A8
+		self.R_FB_POS_HI = 0x20A9
+		self.R_FB_POS_LO = 0x20AA
+
+		## Troubleshooting
+		self.L_FAULT = 0x20A5
+		self.R_FAULT = 0x20A6
+
+		########################
+		## Control CMDs (REG) ##
+		########################
+		self.EMER_STOP = 0x05
+		self.ALRM_CLR = 0x06
+		self.DOWN_TIME = 0x07
+		self.ENABLE = 0x08
+		self.POS_SYNC = 0x10
+		self.POS_L_START = 0x11
+		self.POS_R_START = 0x12
+
+		####################
+		## Operation Mode ##
+		####################
+		self.POS_REL_CONTROL = 1
+		self.POS_ABS_CONTROL = 2
+		self.VEL_CONTROL = 3
+
+		self.ASYNC = 0
+		self.SYNC = 1
+
+		#################
+		## Fault codes ##
+		#################
+		self.NO_FAULT = 0x0000
+		self.OVER_VOLT = 0x0001
+		self.UNDER_VOLT = 0x0002
+		self.OVER_CURR = 0x0004
+		self.OVER_LOAD = 0x0008
+		self.CURR_OUT_TOL = 0x0010
+		self.ENCOD_OUT_TOL = 0x0020
+		self.MOTOR_BAD = 0x0040
+		self.REF_VOLT_ERROR = 0x0080
+		self.EEPROM_ERROR = 0x0100
+		self.WALL_ERROR = 0x0200
+		self.HIGH_TEMP = 0x0400
+		self.FAULT_LIST = [self.OVER_VOLT, self.UNDER_VOLT, self.OVER_CURR, self.OVER_LOAD, self.CURR_OUT_TOL, self.ENCOD_OUT_TOL, \
+					self.MOTOR_BAD, self.REF_VOLT_ERROR, self.EEPROM_ERROR, self.WALL_ERROR, self.HIGH_TEMP]
+
+		##############
+		## Odometry ##
+		##############
+		## 8 inches wheel
+		self.travel_in_one_rev = 0.655
+		self.cpr = 16385
+		self.R_Wheel = 0.105 #meter
+
+	## Some time if read immediatly after write, it would show ModbusIOException when get data from registers
+	def modbus_fail_read_handler(self, ADDR, WORD):
+
+		read_success = False
+		reg = [None]*WORD
+		while not read_success:
+			result = self.client.read_holding_registers(ADDR, WORD, unit=self.ID)
+			try:
+				for i in range(WORD):
+					reg[i] = result.registers[i]
+				read_success = True
+			except AttributeError as e:
+				print(e)
+				pass
+
+		return reg
+
+	def rpm_to_radPerSec(self, rpm):
+		return rpm*2*np.pi/60.0
+
+	def rpm_to_linear(self, rpm):
+
+		W_Wheel = self.rpm_to_radPerSec(rpm)
+		V = W_Wheel*self.R_Wheel
+
+		return V
+
+	def set_mode(self, MODE):
+		if MODE == 1:
+			print("Set relative position control")
+		elif MODE == 2:
+			print("Set absolute position control")
+		elif MODE == 3:
+			print("Set speed rpm control")
+		else:
+			print("set_mode ERROR: set only 1, 2, or 3")
+			return 0
+
+		result = self.client.write_register(self.OPR_MODE, MODE, unit=self.ID)
+		return result
+
+	def get_mode(self):
+
+		# result = self.client.read_holding_registers(self.OPR_MODE, 1, unit=self.ID)
+		registers = self.modbus_fail_read_handler(self.OPR_MODE, 1)
+
+		mode = registers[0]
+
+		return mode
+
+	def enable_motor(self):
+		result = self.client.write_register(self.CONTROL_REG, self.ENABLE, unit=self.ID)
+
+	def disable_motor(self):
+		result = self.client.write_register(self.CONTROL_REG, self.DOWN_TIME, unit=self.ID)
+
+	def get_fault_code(self):
+
+		fault_codes = self.client.read_holding_registers(self.L_FAULT, 2, unit=self.ID)
+
+		L_fault_code = fault_codes.registers[0]
+		R_fault_code = fault_codes.registers[1]
+
+		L_fault_flag = L_fault_code in self.FAULT_LIST
+		R_fault_flag = R_fault_code in self.FAULT_LIST
+
+		return (L_fault_flag, L_fault_code), (R_fault_flag, R_fault_code)
+
+	def clear_alarm(self):
+		result = self.client.write_register(self.CONTROL_REG, self.ALRM_CLR, unit=self.ID)
+
+	def set_accel_time(self, L_ms, R_ms):
+
+		if L_ms > 32767:
+			L_ms = 32767
+		elif L_ms < 0:
+			L_ms = 0
+
+		if R_ms > 32767:
+			R_ms = 32767
+		elif R_ms < 0:
+			R_ms = 0
+
+		result = self.client.write_registers(self.L_ACL_TIME, [int(L_ms),int(R_ms)], unit=self.ID)
+
+
+	def set_decel_time(self, L_ms, R_ms):
+
+		if L_ms > 32767:
+			L_ms = 32767
+		elif L_ms < 0:
+			L_ms = 0
+
+		if R_ms > 32767:
+			R_ms = 32767
+		elif R_ms < 0:
+			R_ms = 0
+
+		result = self.client.write_registers(self.L_DCL_TIME, [int(L_ms), int(R_ms)], unit=self.ID)
+
+	def int16Dec_to_int16Hex(self,int16):
+
+		lo_byte = (int16 & 0x00FF)
+		hi_byte = (int16 & 0xFF00) >> 8
+
+		all_bytes = (hi_byte << 8) | lo_byte
+
+		return all_bytes
+
+
+	def set_rpm(self, L_rpm, R_rpm):
+
+		if L_rpm > 3000:
+			L_rpm = 3000
+		elif L_rpm < -3000:
+			L_rpm = -3000
+
+		if R_rpm > 3000:
+			R_rpm = 3000
+		elif R_rpm < -3000:
+			R_rpm = -3000
+
+		left_bytes = self.int16Dec_to_int16Hex(L_rpm)
+		right_bytes = self.int16Dec_to_int16Hex(R_rpm)
+
+		result = self.client.write_registers(self.L_CMD_RPM, [left_bytes, right_bytes], unit=self.ID)
+
+	def get_rpm(self):
+
+
+		# rpms = self.client.read_holding_registers(self.L_FB_RPM, 2, unit=self.ID)
+		# fb_L_rpm = np.int16(rpms.registers[0])/10.0
+		# fb_R_rpm = np.int16(rpms.registers[1])/10.0
+
+		registers = self.modbus_fail_read_handler(self.L_FB_RPM, 2)
+		fb_L_rpm = np.int16(registers[0])/10.0
+		fb_R_rpm = np.int16(registers[1])/10.0
+
+		return fb_L_rpm, fb_R_rpm
+
+	def get_linear_velocities(self):
+
+		rpmL, rpmR = self.get_rpm()
+
+		VL = self.rpm_to_linear(rpmL)
+		VR = self.rpm_to_linear(-rpmR)
+
+		return VL, VR
+
+	def map(self, val, in_min, in_max, out_min, out_max):
+
+			return (val - in_min) * (out_max - out_min) / (in_max - in_min) + out_min
+
+	def set_maxRPM_pos(self, max_L_rpm, max_R_rpm):
+
+		if max_L_rpm > 1000:
+			max_L_rpm = 1000
+		elif max_L_rpm < 1:
+			max_L_rpm = 1
+
+		if max_R_rpm > 1000:
+			max_R_rpm = 1000
+		elif max_R_rpm < 1:
+			max_R_rpm = 1
+
+		result = self.client.write_registers(self.L_MAX_RPM_POS, [int(max_L_rpm), int(max_R_rpm)], unit=self.ID)
+
+	def set_position_async_control(self):
+
+		result = self.client.write_register(self.POS_CONTROL_TYPE, self.ASYNC, unit=self.ID)
+
+	def move_left_wheel(self):
+
+		result = self.client.write_register(self.CONTROL_REG, self.POS_L_START, unit=self.ID)
+
+	def move_right_wheel(self):
+
+		result = self.client.write_register(self.CONTROL_REG, self.POS_R_START, unit=self.ID)
+
+	def deg_to_32bitArray(self, deg):
+
+		dec = int(self.map(deg, -1440, 1440, -65536, 65536))
+		HI_WORD = (dec & 0xFFFF0000) >> 16
+		LO_WORD = dec & 0x0000FFFF
+
+		return [HI_WORD, LO_WORD]
+
+	def set_relative_angle(self, ang_L, ang_R):
+
+		L_array = self.deg_to_32bitArray(ang_L)
+		R_array = self.deg_to_32bitArray(ang_R)
+		all_cmds_array = L_array + R_array
+
+		result = self.client.write_registers(self.L_CMD_REL_POS_HI, all_cmds_array, unit=self.ID)
+
+	def get_wheels_travelled(self):
+
+		# read_success = False
+		# while not read_success:
+
+		# 	result = self.client.read_holding_registers(self.L_FB_POS_HI, 4, unit=self.ID)
+		# 	try:
+		# 		l_pul_hi = result.registers[0]
+		# 		l_pul_lo = result.registers[1]
+		# 		r_pul_hi = result.registers[2]
+		# 		r_pul_lo = result.registers[3]
+
+		# 		l_pulse = ((l_pul_hi & 0xFFFF) << 16) | (l_pul_lo & 0xFFFF)
+		# 		r_pulse = ((r_pul_hi & 0xFFFF) << 16) | (r_pul_lo & 0xFFFF)
+		# 		l_travelled = (float(l_pulse)/self.cpr)*self.travel_in_one_rev  # unit in meter
+		# 		r_travelled = (float(r_pulse)/self.cpr)*self.travel_in_one_rev  # unit in meter
+
+		# 		read_success = True
+
+		# 	except AttributeError:
+		# 		# print("error")
+		# 		pass
+
+		registers = self.modbus_fail_read_handler(self.L_FB_POS_HI, 4)
+		l_pul_hi = registers[0]
+		l_pul_lo = registers[1]
+		r_pul_hi = registers[2]
+		r_pul_lo = registers[3]
+
+		l_pulse = np.int32(((l_pul_hi & 0xFFFF) << 16) | (l_pul_lo & 0xFFFF))
+		r_pulse = np.int32(((r_pul_hi & 0xFFFF) << 16) | (r_pul_lo & 0xFFFF))
+		l_travelled = (float(l_pulse)/self.cpr)*self.travel_in_one_rev  # unit in meter
+		r_travelled = (float(r_pulse)/self.cpr)*self.travel_in_one_rev  # unit in meter
+
+		return l_travelled, r_travelled
diff --git a/src/ZLAC8015D_python/zlac8015d/__init__.py b/src/ZLAC8015D_python/zlac8015d/__init__.py
new file mode 100644
index 0000000..47534ee
--- /dev/null
+++ b/src/ZLAC8015D_python/zlac8015d/__init__.py
@@ -0,0 +1 @@
+from zlac8015d import ZLAC8015D
\ No newline at end of file