diff --git a/CMakeLists.txt b/CMakeLists.txt new file mode 100644 index 0000000..78fdd2d --- /dev/null +++ b/CMakeLists.txt @@ -0,0 +1,18 @@ +cmake_minimum_required(VERSION 3.0.2) +project(zlac8015d_ros) + +find_package(catkin REQUIRED COMPONENTS + rospy +) + +catkin_package( +# INCLUDE_DIRS include +# LIBRARIES zlac8015d_ros +# CATKIN_DEPENDS rospy +# DEPENDS system_lib +) + +include_directories( +# include + ${catkin_INCLUDE_DIRS} +) \ No newline at end of file diff --git a/README.md b/README.md new file mode 100644 index 0000000..4306d3f --- /dev/null +++ b/README.md @@ -0,0 +1,57 @@ +# ROS package for ZLAC8015D dual-channel servo driver +The latest repository is below. +[https://github.com/Alpaca-zip/zlac8015d_ros.git](https://github.com/Alpaca-zip/zlac8015d_ros.git) +[![](https://img.shields.io/badge/ROS-Noetic-brightgreen.svg)](https://github.com/Alpaca-zip/zlac8015d_ros) + + + +## 1 Installation +``` +$ cd catkin_ws/src + +$ git clone https://github.com/Alpaca-zip/zlac8015d_ros.git + +$ cd .. && catkin_make +``` + +## 2 Change full pathname of parameter file +`scripts/motor_driver_node.py` refers to `/home/ubuntu/catkin_ws/src/zlac8015d_ros/params/motor_driver_params.yaml` as default. +If you want to specify a different full pathname, change line 34 in `scripts/motor_driver_node.py` + +## 3 Usage +### Run motor_driver_node +``` +$ rosrun zlac8015d_ros motor_driver_node.py +``` +### Parameters +- `port`: Name of the zlac8015d port. Default is `dev/ttyUSB0`. +- `travel_in_one_rev`: Tire circumference. Default is `0.655`[m]. +- `R_Wheel`: Tire radius. Default is `0.105`[m]. +- `cpr`: CPR(Counts Per Revolution). Default is `16385`. +- `wheels_base_width`: Distance between tires. Default is `0.440`[m]. +- `control_mode`: `1` is relative position control mode, `3` is speed rpm control mode. Default is `3`. +- `callback_timeout`: Motor automatically stops if no topics are received for a certain period of time. Default is `0.5`[s]. +- `decimil_coefficient`: Must be specified in digits. Smaller values will cause small changes in tire motion to have a greater impact on the odometry calculations. `0.01` or `0.001` is highly recommended. Default is `0.001`. +- `set_accel_time_left`: Acceleration time for left tire. Default is `200`[ms]. +- `set_accel_time_right`: Acceleration time for right tire. Default is `200`[ms]. +- `set_decel_time_left`: Deceleration time for left tire. Default is `200`[ms]. +- `set_decel_time_right`: Deceleration time for right tire. Default is `200`[ms]. +- `max_left_rpm`: Maximum rpm of left tire. Default is `150`. +- `max_right_rpm`: Maximum rpm of right tire. Default is `150`. +- `deadband_rpm`: Width of rpm to be regarded as 0. If `3`, then -3 to 3 is considered rpm 0. Default is `3`. +- `TF_header_flame`: Header flame of TF. Default is `odom`. +- `TF_child_flame`: Child flame of TF. Default is `base_link`. +- `odom_header_flame`: Header flame of odom. Default is `odom`. +- `odom_child_flame`: Child flame of odom. Default is `base_link`. + +### Topics +This node publishes the following topics. +- `/wheels_rpm`: The speed in RPM of each tire as [left, right]. +- `/odom`: Odometry data for the robot. [More detail](http://docs.ros.org/en/diamondback/api/nav_msgs/html/msg/Odometry.html) + +This node subscribes to the following topics. +- `/zlac8015d/twist/cmd_vel`: Send command as linear velocity and angular velocity in speed rpm control. [More detail](https://docs.ros.org/en/diamondback/api/geometry_msgs/html/msg/Twist.html) +- `/zlac8015d/vel/cmd_vel`: Send command as velocity in speed rpm control, e.g. [0.6, 0.5] 0.6[m/s] of left tire, 0.5[m/s] of right tire. +- `/zlac8015d/vel/cmd_rpm`: Send command as rpm in speed rpm control, e.g. [100, 50] 100 rpm of left wtire, 50 rpm of right tire. +- `/zlac8015d/pos/deg_cmd`: Send command as angle degree in position control, e.g. [90,70] 90 [deg] of left tire, 70 [deg] of right tire. +- `/zlac8015d/pos/dist_cmd`: Send command as desired travelling distance in position control, e.g. [1.0, 1.0] for 1[m] travelling distance of each tire. diff --git a/circuit_scheme.png b/circuit_scheme.png new file mode 100644 index 0000000..04897a1 --- /dev/null +++ b/circuit_scheme.png Binary files differ diff --git a/package.xml b/package.xml new file mode 100644 index 0000000..2fa256b --- /dev/null +++ b/package.xml @@ -0,0 +1,14 @@ + + + zlac8015d_ros + 0.0.1 + The zlac8015d_ros package + Alpacazip + GPLv3 + catkin + rospy + rospy + rospy + + + \ No newline at end of file diff --git a/params/motor_driver_params.yaml b/params/motor_driver_params.yaml new file mode 100644 index 0000000..5ce5850 --- /dev/null +++ b/params/motor_driver_params.yaml @@ -0,0 +1,50 @@ +# Name of USB port for zlac8015d +port: /dev/ttyUSB0 + +# Tire & Encoder Parameters +# Default: 8 inches wheel +############################### +# Tire circumference +travel_in_one_rev: 0.655 # meter +# Tire radius +R_Wheel: 0.105 # meter +# Encoder CPR(counts per revolution) +cpr: 16385 +############################### + +# Distance between tires +wheels_base_width: 0.440 # meter +# Control mode +# 1: relative position control mode +# Subscribe to "/zlac8015d/pos/cmd_deg" and "/zlac8015d/pos/cmd_dist" +# 3: Speed rpm control mode +# Subscribe to "/zlac8015d/twist/cmd_vel", "/zlac8015d/vel/cmd_vel" and "/zlac8015d/vel/cmd_rpm" +control_mode: 3 +# Motor automatically stops if no topics are received for a certain period of time +callback_timeout: 0.5 # seconds +# Smaller values will cause small changes in tire motion to have a greater impact on the odometry calculations +# 0.01 or 0.001 is highly recommended +decimil_coefficient: 0.001 + +# Time to reach target position +set_accel_time_left: 200 # ms +set_accel_time_right: 200 # ms +set_decel_time_left: 200 # ms +set_decel_time_right: 200 # ms + +# Maximum rpm +max_left_rpm: 150 +max_right_rpm: 150 +# Width of rpm to be regarded as 0 +# If 3, then -3 to 3 is considered rpm 0 +deadband_rpm: 3 + +# Header flame of TF +TF_header_flame: odom +# Child flame of TF +TF_child_flame: base_link + +# Header flame of odom +odom_header_flame: odom +# Child flame of odom +odom_child_flame: base_link \ No newline at end of file diff --git a/scripts/ZLAC8015D.py b/scripts/ZLAC8015D.py new file mode 100644 index 0000000..04b9536 --- /dev/null +++ b/scripts/ZLAC8015D.py @@ -0,0 +1,339 @@ +# This script is taken from the following Python API. +# https://github.com/rasheeddo/ZLAC8015D_python.git + +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) + L_array = self.deg_to_32bitArray(ang_L/4) + R_array = self.deg_to_32bitArray(ang_R/4) + 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 + + l_travelled = (float(l_pulse)/self.cpr)*self.travel_in_one_rev*4 # unit in meter + r_travelled = (float(r_pulse)/self.cpr)*self.travel_in_one_rev*4 # unit in meter + + return l_travelled, r_travelled diff --git a/scripts/motor_driver_node.py b/scripts/motor_driver_node.py new file mode 100644 index 0000000..066c921 --- /dev/null +++ b/scripts/motor_driver_node.py @@ -0,0 +1,377 @@ +#!/usr/bin/env python3 + +import rospy +import ZLAC8015D +import tf2_ros +import numpy as np +import time +import yaml +from decimal import Decimal, ROUND_HALF_UP +from std_msgs.msg import Float32MultiArray +from geometry_msgs.msg import Twist +from nav_msgs.msg import Odometry +from tf.transformations import quaternion_from_euler +from geometry_msgs.msg import TransformStamped + +class MotorDriverNode: + def __init__(self): + print("\033[32m" + "Start motor driver node!!" + "\033[0m") + print("#########################") + #-----Initialize publisher----- + self.wheels_rpm_pub = rospy.Publisher("/wheels_rpm", Float32MultiArray, queue_size=10) + self.wheels_rpm_msg = Float32MultiArray() + self.odom_pub = rospy.Publisher("/odom", Odometry, queue_size=10) + self.odom_msg = Odometry() + + #-----Initialize subscriber----- + rospy.Subscriber("/zlac8015d/twist/cmd_vel", Twist, self.zlac8015d_twist_cmd_callback) + rospy.Subscriber("/zlac8015d/vel/cmd_vel", Float32MultiArray, self.zlac8015d_vel_cmd_callback) + rospy.Subscriber("/zlac8015d/vel/cmd_rpm", Float32MultiArray, self.zlac8015d_rpm_cmd_callback) + rospy.Subscriber("/zlac8015d/pos/cmd_deg", Float32MultiArray, self.zlac8015d_deg_cmd_callback) + rospy.Subscriber("/zlac8015d/pos/cmd_dist", Float32MultiArray, self.zlac8015d_dist_cmd_callback) + + #-----Import parameters from yaml file----- + self.load_file = "/home/ubuntu/catkin_ws/src/zlac8015d_ros/params/motor_driver_params.yaml" + with open(self.load_file) as file: + self.obj = yaml.safe_load(file) + + #-----Initialize ZLAC8015D----- + self.zlc = ZLAC8015D.Controller(self.obj["port"]) + self.zlc.travel_in_one_rev = float(self.obj["travel_in_one_rev"]) + self.zlc.cpr = int(self.obj["cpr"]) + self.zlc.R_Wheel = float(self.obj["R_Wheel"]) + self.control_mode = int(self.obj["control_mode"]) + if self.control_mode == 3: + self.zlac8015d_speed_mode_init() + elif self.control_mode == 1: + self.zlac8015d_position_mode_init() + + #-----Initialize variable----- + self.last_subscribed_time = 0.0 + self.callback_timeout = float(self.obj["callback_timeout"]) + self.deadband_rpm = int(self.obj["deadband_rpm"]) + self.linear_vel_cmd = 0.0 + self.angular_vel_cmd = 0.0 + self.got_twist_cmd = False + + self.left_vel_cmd = 0.0 + self.right_vel_cmd = 0.0 + self.got_vel_cmd = False + + self.left_rpm_cmd = 0.0 + self.right_rpm_cmd = 0.0 + self.got_vel_rpm_cmd = False + + self.left_pos_deg_cmd = 0.0 + self.right_pos_deg_cmd = 0.0 + self.got_pos_deg_cmd = False + + self.left_pos_dist_cmd = 0.0 + self.right_pos_dist_cmd = 0.0 + self.got_pos_dist_cmd = False + + self.theta = 0.0 + self.x = 0.0 + self.y = 0.0 + self.period = 0.05 + self.l_meter = 0.0 + self.r_meter = 0.0 + self.l_meter_init, self.r_meter_init = self.zlc.get_wheels_travelled() + self.prev_l_meter = 0.0 + self.prev_r_meter = 0.0 + + self.br = tf2_ros.TransformBroadcaster() + self.t = TransformStamped() + + """ + ++++++++++++++++++++++++++++++++++++++++++ + zlac8015d_twist_cmd_callback function + ++++++++++++++++++++++++++++++++++++++++++ + Callback function to subscribe for "/zlac8015d/twist/cmd_vel". + """ + def zlac8015d_twist_cmd_callback(self, msg): + self.linear_vel_cmd = msg.linear.x + self.angular_vel_cmd = msg.angular.z + self.got_twist_cmd = True + self.last_subscribed_time = rospy.Time.now().nsecs + + """ + ++++++++++++++++++++++++++++++++++++++++++ + zlac8015d_vel_cmd_callback function + ++++++++++++++++++++++++++++++++++++++++++ + Callback function to subscribe for "/zlac8015d/vel/cmd_vel". + """ + def zlac8015d_vel_cmd_callback(self, msg): + self.left_vel_cmd = msg.data[0] + self.right_vel_cmd = -msg.data[1] + self.got_vel_cmd = True + self.last_subscribed_time = time.perf_counter() + + """ + ++++++++++++++++++++++++++++++++++++++++++ + zlac8015d_rpm_cmd_callback function + ++++++++++++++++++++++++++++++++++++++++++ + Callback function to subscribe for "/zlac8015d/vel/cmd_rpm". + """ + def zlac8015d_rpm_cmd_callback(self, msg): + self.left_rpm_cmd = msg.data[0] + self.right_rpm_cmd = -msg.data[1] + self.got_vel_rpm_cmd = True + self.last_subscribed_time = time.perf_counter() + + """ + ++++++++++++++++++++++++++++++++++++++++++ + zlac8015d_deg_cmd_callback function + ++++++++++++++++++++++++++++++++++++++++++ + Callback function to subscribe for "/zlac8015d/pos/cmd_deg". + """ + def zlac8015d_deg_cmd_callback(self, msg): + self.left_pos_deg_cmd = msg.data[0] + self.right_pos_deg_cmd = -msg.data[1] + self.got_pos_deg_cmd = True + + """ + ++++++++++++++++++++++++++++++++++++++++++ + zlac8015d_dist_cmd_callback function + ++++++++++++++++++++++++++++++++++++++++++ + Callback function to subscribe for "/zlac8015d/pos/cmd_dist". + """ + def zlac8015d_dist_cmd_callback(self, msg): + self.left_pos_dist_cmd = msg.data[0] + self.right_pos_dist_cmd = msg.data[1] + self.got_pos_dist_cmd = True + + """ + ++++++++++++++++++++++++++++++++++++++++++ + zlac8015d_speed_mode_init function + ++++++++++++++++++++++++++++++++++++++++++ + Initialization of speed rpm control mode. + """ + def zlac8015d_speed_mode_init(self): + self.zlc.disable_motor() + self.zlc.set_accel_time(int(self.obj["set_accel_time_left"]), int(self.obj["set_accel_time_right"])) + self.zlc.set_decel_time(int(self.obj["set_decel_time_left"]), int(self.obj["set_decel_time_right"])) + self.zlc.set_mode(3) + self.zlc.enable_motor() + + """ + ++++++++++++++++++++++++++++++++++++++++++ + zlac8015d_position_mode_init function + ++++++++++++++++++++++++++++++++++++++++++ + Initialization of relative position control mode. + """ + def zlac8015d_position_mode_init(self): + self.zlc.disable_motor() + self.zlc.set_accel_time(int(self.obj["set_accel_time_left"]), int(self.obj["set_accel_time_right"])) + self.zlc.set_decel_time(int(self.obj["set_decel_time_left"]), int(self.obj["set_decel_time_right"])) + self.zlc.set_mode(1) + self.zlc.set_position_async_control() + self.zlc.set_maxRPM_pos(int(self.obj["max_left_rpm"]), int(self.obj["max_right_rpm"])) + self.zlc.enable_motor() + + """ + ++++++++++++++++++++++++++++++++++++++++++ + twist_to_rpm function + ++++++++++++++++++++++++++++++++++++++++++ + Convert from twist to rpm. + """ + def twist_to_rpm(self, linear_vel, angular_vel): + left_vel = linear_vel - float(self.obj["wheels_base_width"]) / 2 * angular_vel + right_vel = linear_vel + float(self.obj["wheels_base_width"]) / 2 * angular_vel + left_rpm, right_rpm = self.vel_to_rpm(left_vel, -right_vel) + return left_rpm, right_rpm + + """ + ++++++++++++++++++++++++++++++++++++++++++ + vel_to_rpm function + ++++++++++++++++++++++++++++++++++++++++++ + Convert from speed to rpm. + """ + def vel_to_rpm(self, left_vel, right_vel): + left_rpm = 60 * left_vel / (2 * np.pi * self.zlc.R_Wheel) + right_rpm = 60 * right_vel / (2 * np.pi * self.zlc.R_Wheel) + return left_rpm, right_rpm + + """ + ++++++++++++++++++++++++++++++++++++++++++ + zlac8015d_set_rpm_with_limit function + ++++++++++++++++++++++++++++++++++++++++++ + Set rpm with limit. + """ + def zlac8015d_set_rpm_with_limit(self, left_rpm, right_rpm): + if (-int(self.obj["max_left_rpm"]) < left_rpm < int(self.obj["max_left_rpm"])) and (-int(self.obj["max_right_rpm"]) < right_rpm < int(self.obj["max_right_rpm"])): + if (-self.deadband_rpm < left_rpm < self.deadband_rpm): + left_rpm = 0 + if (-self.deadband_rpm < right_rpm < self.deadband_rpm): + right_rpm = 0 + + self.zlc.set_rpm(int(left_rpm), int(right_rpm)) + else: + rospy.logerr("Set RPM exceeds the limit.") + + """ + ++++++++++++++++++++++++++++++++++++++++++ + calculate_odometry function + ++++++++++++++++++++++++++++++++++++++++++ + Odometry computation. + """ + def calculate_odometry(self): + self.l_meter, self.r_meter = self.zlc.get_wheels_travelled() + self.l_meter = self.l_meter - self.l_meter_init + self.r_meter = (-1 * self.r_meter) - (-1 * self.r_meter_init) + if self.control_mode == 3: + vl, vr = self.zlc.get_linear_velocities() + elif self.control_mode == 1: + vl = (self.l_meter - self.prev_l_meter) / self.period + vr = (self.r_meter - self.prev_r_meter) / self.period + + round_vl = float(Decimal(str(vl)).quantize(Decimal(self.obj["decimil_coefficient"]), rounding = ROUND_HALF_UP)) + round_vr = float(Decimal(str(vr)).quantize(Decimal('0.001'), rounding = ROUND_HALF_UP)) + self.obj["TF_header_flame"] + + #-----Rotatiing----- + if ((round_vl * round_vr) < 0.0) and (abs(round_vl) == abs(round_vr)): + V = 0.0 + Wz = 2.0 * vr / float(self.obj["wheels_base_width"]) + self.theta = self.theta + Wz * self.period + path = "Rotatiing" + + #-----Curving----- + elif (round_vl - round_vr) != 0: + V = (vl + vr) / 2.0 + Wz = (vr - vl) / float(self.obj["wheels_base_width"]) + R_ICC = (float(self.obj["wheels_base_width"]) / 2.0) * ((vr + vl) / (vr - vl)) + self.x = self.x - R_ICC * np.sin(self.theta) + R_ICC * np.sin(self.theta + Wz * self.period) + self.y = self.y + R_ICC * np.cos(self.theta) - R_ICC * np.cos(self.theta + Wz * self.period) + self.theta = self.theta + Wz * self.period + path = "Curving" + + #-----Going straight----- + else: + V = (vl + vr)/2.0 + Wz = 0.0 + self.x = self.x + V * np.cos(self.theta) * self.period + self.y = self.y + V * np.sin(self.theta) * self.period + path = "Going straight" + + #-----Construct tf----- + q = quaternion_from_euler(0, 0, self.theta) + self.t.header.stamp = rospy.Time.now() + self.t.header.frame_id = self.obj["TF_header_flame"] + self.t.child_frame_id = self.obj["TF_child_flame"] + self.t.transform.translation.x = self.x + self.t.transform.translation.y = self.y + self.t.transform.translation.z = 0.0 + self.t.transform.rotation.x = q[0] + self.t.transform.rotation.y = q[1] + self.t.transform.rotation.z = q[2] + self.t.transform.rotation.w = q[3] + self.br.sendTransform(self.t) + + #-----Construct odom message----- + self.odom_msg.header.stamp = rospy.Time.now() + self.odom_msg.header.frame_id = self.obj["odom_header_flame"] + self.odom_msg.child_frame_id = self.obj["odom_child_flame"] + self.odom_msg.pose.pose.position.x = self.x + self.odom_msg.pose.pose.position.y = self.y + self.odom_msg.pose.pose.position.z = 0.0 + self.odom_msg.pose.pose.orientation.x = q[0] + self.odom_msg.pose.pose.orientation.y = q[1] + self.odom_msg.pose.pose.orientation.z = q[2] + self.odom_msg.pose.pose.orientation.w = q[3] + self.odom_msg.pose.covariance[0] = 0.0001 + self.odom_msg.pose.covariance[7] = 0.0001 + self.odom_msg.pose.covariance[14] = 0.000001 + self.odom_msg.pose.covariance[21] = 0.000001 + self.odom_msg.pose.covariance[28] = 0.000001 + self.odom_msg.pose.covariance[35] = 0.0001 + self.odom_msg.twist.twist.linear.x = V + self.odom_msg.twist.twist.linear.y = 0.0 + self.odom_msg.twist.twist.angular.z = Wz + self.odom_pub.publish(self.odom_msg) + + return vl, vr + + """ + ++++++++++++++++++++++++++++++++++++++++++ + control_loop function + ++++++++++++++++++++++++++++++++++++++++++ + Control loop. + """ + def control_loop(self): + rate = rospy.Rate(20) # 20Hz + while True: + if rospy.is_shutdown(): + self.zlc.disable_motor() + break + + start_time = time.perf_counter() + + #-----Speed rpm control mode----- + if self.control_mode == 3: + if self.got_twist_cmd: + self.left_rpm_cmd, self.right_rpm_cmd = self.twist_to_rpm(self.linear_vel_cmd, self.angular_vel_cmd) + self.zlac8015d_set_rpm_with_limit(self.left_rpm_cmd, self.right_rpm_cmd) + self.got_twist_cmd = False + elif self.got_vel_cmd: + self.left_rpm_cmd, self.right_rpm_cmd = self.vel_to_rpm(self.left_vel_cmd, self.right_vel_cmd) + self.zlac8015d_set_rpm_with_limit(self.left_rpm_cmd, self.right_rpm_cmd) + self.got_vel_cmd = False + elif self.got_vel_rpm_cmd: + self.zlac8015d_set_rpm_with_limit(self.left_rpm_cmd, self.right_rpm_cmd) + self.got_vel_rpm_cmd = False + elif (time.perf_counter() - self.last_subscribed_time > self.callback_timeout): + self.left_rpm_cmd = 0.0 + self.right_rpm_cmd = 0.0 + self.zlac8015d_set_rpm_with_limit(self.left_rpm_cmd, self.right_rpm_cmd) + + fb_L_rpm, fb_R_rpm = self.zlc.get_rpm() + self.wheels_rpm_msg.data = [fb_L_rpm, fb_R_rpm] + self.wheels_rpm_pub.publish(self.wheels_rpm_msg) + + #-----relative position control mode----- + elif self.control_mode == 1: + if self.got_pos_deg_cmd: + self.zlc.set_relative_angle(self.left_pos_deg_cmd ,self.right_pos_deg_cmd) + self.zlc.move_left_wheel() + self.zlc.move_right_wheel() + self.got_pos_deg_cmd = False + elif self.got_pos_dist_cmd: + left_cmd_deg = (self.left_pos_dist_cmd * 360.0) / self.zlc.travel_in_one_rev + right_cmd_deg = (-self.right_pos_dist_cmd * 360.0) / self.zlc.travel_in_one_rev + self.zlc.set_relative_angle(left_cmd_deg, right_cmd_deg) + self.zlc.move_left_wheel() + self.zlc.move_right_wheel() + self.got_pos_dist_cmd = False + + #-----Odometry computation----- + vl, vr = self.calculate_odometry() + + self.period = time.perf_counter() - start_time + self.prev_l_meter = self.l_meter + self.prev_r_meter = self.r_meter + + #-----Logging to screen----- + if self.control_mode == 1: + print("\033[32m" + "\rcontrol_mode: {:d} | l_meter: {:.2f} | r_meter: {:.2f} | VL: {:.3f} | VR: {:.3f}".format(\ + self.control_mode, self.l_meter, self.r_meter, vl, vr) + "\033[0m", end = "") + else: + print("\033[32m" + "\rcontrol_mode: {:d} | left_rpm: {:.2f} | right_rpm: {:.2f} | l_meter: {:.2f} | r_meter: {:.2f} | VL: {:.3f} | VR: {:.3f}".format(\ + self.control_mode, fb_L_rpm, -fb_R_rpm, self.l_meter, self.r_meter, vl, vr) + "\033[0m", end = "") + + rate.sleep() + +""" +++++++++++++++++++++++++++++++++++++++++++ + main function +++++++++++++++++++++++++++++++++++++++++++ +Main. +""" +def main(): + rospy.init_node("motor_driver_node") + MDN = MotorDriverNode() + MDN.control_loop() + rospy.spin() + +if __name__ == "__main__": + main() \ No newline at end of file