├── .gitignore
├── README.md
├── package.xml
├── src
    ├── example.cpp
    └── zlac8015d.cpp
├── CMakeLists.txt
└── include
    ├── Comm
        └── crc_check.h
    └── zlac8015d.h


/.gitignore:
--------------------------------------------------------------------------------
1 | 


--------------------------------------------------------------------------------
/README.md:
--------------------------------------------------------------------------------
 1 | # ROS2_ZLAC8015D_serial
 2 | 
 3 | **ROS2 package for serial communication (RS485) with ZLAC8015D motor driver**
 4 | ZLTECH Dual-Channel Servo Driver ZLAC8015D
 5 | 
 6 | Driver Info Link: http://www.zlrobotmotor.com/info/401.html
 7 | 
 8 | ## required packages
 9 | 
10 | - rclcpp
11 | - [serial](https://github.com/wjwwood/serial)
12 | 
13 | ## How to use
14 | 
15 | ```bash
16 | cd <your_workspace>
17 | git clone https://github.com/oxcarxierra/ROS2_ZLAC8015D_serial.git
18 | colcon build --packages-select zlac8015d_serial
19 | ros2 run zlac8015d_serial example
20 | ```
21 | 


--------------------------------------------------------------------------------
/package.xml:
--------------------------------------------------------------------------------
 1 | <?xml version="1.0"?>
 2 | <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 3 | <package format="3">
 4 |   <name>zlac8015d_serial</name>
 5 |   <version>0.0.1</version>
 6 |   <description>ROS2 package for RS485 communication with ZLAC8015D driver</description>
 7 |   <maintainer email="oxcarxierra@gmail.com">Seung Seok Oh</maintainer>
 8 |   <license>MIT</license>
 9 |   <buildtool_depend>ament_cmake</buildtool_depend>
10 | 
11 |   <depend>rclcpp</depend>
12 |   <depend>serial</depend>
13 | 
14 |   <test_depend>ament_lint_auto</test_depend>
15 |   <test_depend>ament_lint_common</test_depend>
16 | 
17 |   <export>
18 |     <build_type>ament_cmake</build_type>
19 |   </export>
20 | </package>
21 | 


--------------------------------------------------------------------------------
/src/example.cpp:
--------------------------------------------------------------------------------
 1 | #include "zlac8015d.h"
 2 | 
 3 | #define FLIP -1
 4 | 
 5 | int main()
 6 | {
 7 |     printf("===begin===\n");
 8 |     ZLAC motorR;
 9 |     motorR.begin("/dev/ttyUSB0", 115200, 0x01);
10 |     ZLAC motorL;
11 |     motorL.begin("/dev/ttyUSB1", 115200, 0x01);
12 | 
13 |     printf("\n===set_vel_mode===\n");
14 |     motorR.set_vel_mode();
15 |     motorL.set_vel_mode();
16 | 
17 |     printf("\n===enable===\n");
18 |     motorR.enable();
19 |     motorL.enable();
20 | 
21 |     printf("\n===set_rpm===\n");
22 |     motorR.set_rpm(50, "LEFT");
23 |     motorR.set_rpm(50, "RIGHT");
24 |     motorL.set_rpm(-50, "LEFT");
25 |     motorL.set_rpm(-50, "RIGHT");
26 | 
27 |     printf("\n===set_sync_rpm===\n");
28 | 
29 |     printf("\n===disable===\n");
30 |     motorR.disable();
31 |     motorL.disable();
32 | }
33 | 


--------------------------------------------------------------------------------
/CMakeLists.txt:
--------------------------------------------------------------------------------
 1 | cmake_minimum_required(VERSION 3.8)
 2 | project(zlac8015d_serial)
 3 | 
 4 | if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
 5 |   add_compile_options(-Wall -Wextra -Wpedantic)
 6 | endif()
 7 | 
 8 | # find dependencies
 9 | find_package(ament_cmake REQUIRED)
10 | find_package(rclcpp REQUIRED)
11 | find_package(serial REQUIRED)
12 | 
13 | include_directories(include)
14 | 
15 | add_executable(example src/example.cpp src/zlac8015d.cpp)
16 | ament_target_dependencies(example serial rclcpp)
17 | 
18 | 
19 | add_library(zlac8015d src/zlac8015d.cpp) 
20 | 
21 | ament_export_targets(zlac8015d HAS_LIBRARY_TARGET)
22 | ament_target_dependencies(zlac8015d serial rclcpp)
23 | 
24 | install(
25 |   DIRECTORY include
26 |   DESTINATION include
27 | )
28 | 
29 | # install the executable in the lib folder to make it detectable through setup.bash
30 | install(TARGETS 
31 |   example
32 |   DESTINATION lib/${PROJECT_NAME}/
33 | )
34 | 
35 | install(
36 |   TARGETS zlac8015d
37 |   EXPORT zlac8015d
38 |   LIBRARY DESTINATION lib
39 |   ARCHIVE DESTINATION lib
40 |   RUNTIME DESTINATION bin
41 |   INCLUDES DESTINATION include
42 | )
43 | 
44 | ament_package()
45 | 


--------------------------------------------------------------------------------
/include/Comm/crc_check.h:
--------------------------------------------------------------------------------
 1 | #ifndef CRC_CHECK
 2 | #define CRC_CHECK
 3 | 
 4 | #include <stdlib.h>
 5 | #include <stdio.h>
 6 | #include <time.h>
 7 | #include <stdint.h>
 8 | #include <string>
 9 | #include <cstring>
10 | #include <iostream>
11 | #include <vector>
12 | #include <cstdio>
13 | // #include <sstream>
14 | 
15 | // using namespace std;
16 | // using std::cerr;
17 | // std::stringstream ss;
18 | 
19 | constexpr unsigned short crc16_table[] = {
20 |     0X0000, 0XC0C1, 0XC181, 0X0140, 0XC301, 0X03C0, 0X0280, 0XC241,
21 |     0XC601, 0X06C0, 0X0780, 0XC741, 0X0500, 0XC5C1, 0XC481, 0X0440,
22 |     0XCC01, 0X0CC0, 0X0D80, 0XCD41, 0X0F00, 0XCFC1, 0XCE81, 0X0E40,
23 |     0X0A00, 0XCAC1, 0XCB81, 0X0B40, 0XC901, 0X09C0, 0X0880, 0XC841,
24 |     0XD801, 0X18C0, 0X1980, 0XD941, 0X1B00, 0XDBC1, 0XDA81, 0X1A40,
25 |     0X1E00, 0XDEC1, 0XDF81, 0X1F40, 0XDD01, 0X1DC0, 0X1C80, 0XDC41,
26 |     0X1400, 0XD4C1, 0XD581, 0X1540, 0XD701, 0X17C0, 0X1680, 0XD641,
27 |     0XD201, 0X12C0, 0X1380, 0XD341, 0X1100, 0XD1C1, 0XD081, 0X1040,
28 |     0XF001, 0X30C0, 0X3180, 0XF141, 0X3300, 0XF3C1, 0XF281, 0X3240,
29 |     0X3600, 0XF6C1, 0XF781, 0X3740, 0XF501, 0X35C0, 0X3480, 0XF441,
30 |     0X3C00, 0XFCC1, 0XFD81, 0X3D40, 0XFF01, 0X3FC0, 0X3E80, 0XFE41,
31 |     0XFA01, 0X3AC0, 0X3B80, 0XFB41, 0X3900, 0XF9C1, 0XF881, 0X3840,
32 |     0X2800, 0XE8C1, 0XE981, 0X2940, 0XEB01, 0X2BC0, 0X2A80, 0XEA41,
33 |     0XEE01, 0X2EC0, 0X2F80, 0XEF41, 0X2D00, 0XEDC1, 0XEC81, 0X2C40,
34 |     0XE401, 0X24C0, 0X2580, 0XE541, 0X2700, 0XE7C1, 0XE681, 0X2640,
35 |     0X2200, 0XE2C1, 0XE381, 0X2340, 0XE101, 0X21C0, 0X2080, 0XE041,
36 |     0XA001, 0X60C0, 0X6180, 0XA141, 0X6300, 0XA3C1, 0XA281, 0X6240,
37 |     0X6600, 0XA6C1, 0XA781, 0X6740, 0XA501, 0X65C0, 0X6480, 0XA441,
38 |     0X6C00, 0XACC1, 0XAD81, 0X6D40, 0XAF01, 0X6FC0, 0X6E80, 0XAE41,
39 |     0XAA01, 0X6AC0, 0X6B80, 0XAB41, 0X6900, 0XA9C1, 0XA881, 0X6840,
40 |     0X7800, 0XB8C1, 0XB981, 0X7940, 0XBB01, 0X7BC0, 0X7A80, 0XBA41,
41 |     0XBE01, 0X7EC0, 0X7F80, 0XBF41, 0X7D00, 0XBDC1, 0XBC81, 0X7C40,
42 |     0XB401, 0X74C0, 0X7580, 0XB541, 0X7700, 0XB7C1, 0XB681, 0X7640,
43 |     0X7200, 0XB2C1, 0XB381, 0X7340, 0XB101, 0X71C0, 0X7080, 0XB041,
44 |     0X5000, 0X90C1, 0X9181, 0X5140, 0X9301, 0X53C0, 0X5280, 0X9241,
45 |     0X9601, 0X56C0, 0X5780, 0X9741, 0X5500, 0X95C1, 0X9481, 0X5440,
46 |     0X9C01, 0X5CC0, 0X5D80, 0X9D41, 0X5F00, 0X9FC1, 0X9E81, 0X5E40,
47 |     0X5A00, 0X9AC1, 0X9B81, 0X5B40, 0X9901, 0X59C0, 0X5880, 0X9841,
48 |     0X8801, 0X48C0, 0X4980, 0X8941, 0X4B00, 0X8BC1, 0X8A81, 0X4A40,
49 |     0X4E00, 0X8EC1, 0X8F81, 0X4F40, 0X8D01, 0X4DC0, 0X4C80, 0X8C41,
50 |     0X4400, 0X84C1, 0X8581, 0X4540, 0X8701, 0X47C0, 0X4680, 0X8641,
51 |     0X8201, 0X42C0, 0X4380, 0X8341, 0X4100, 0X81C1, 0X8081, 0X4040};
52 | 
53 | static inline unsigned short crc16(const unsigned char *data, unsigned short len)
54 | {
55 |     unsigned char nTemp;
56 |     unsigned short wCRCWord = 0xFFFF;
57 | 
58 |     while (len--)
59 |     {
60 |         nTemp = *data++ ^ wCRCWord;
61 |         wCRCWord >>= 8;
62 |         wCRCWord ^= crc16_table[nTemp];
63 |     }
64 |     return wCRCWord;
65 | }
66 | 
67 | #endif


--------------------------------------------------------------------------------
/include/zlac8015d.h:
--------------------------------------------------------------------------------
  1 | #ifndef ZLAC8015D
  2 | #define ZLAC8015D
  3 | 
  4 | #include <string>
  5 | #include <iostream>
  6 | #include <chrono>
  7 | 
  8 | // OS Specific sleep
  9 | #ifdef _WIN32
 10 | #include <windows.h>
 11 | #else
 12 | #include <unistd.h>
 13 | #endif
 14 | 
 15 | #include "serial/serial.h"
 16 | #include "Comm/crc_check.h"
 17 | 
 18 | class ZLAC
 19 | {
 20 | protected:
 21 |     std::chrono::time_point<std::chrono::steady_clock> start, end;
 22 | 
 23 |     uint8_t hex_cmd[8] = {0};
 24 |     uint8_t receive_hex[15] = {0};
 25 |     uint8_t ID = 0x00;
 26 |     const uint8_t READ = 0x03;
 27 |     const uint8_t WRITE = 0x06;
 28 |     const uint8_t MULTI_WRITE = 0x10;
 29 |     const uint8_t CONTROL_REG[2] = {0X20, 0X31};
 30 |     const uint8_t ENABLE[2] = {0x00, 0X08};
 31 |     const uint8_t DISABLE[2] = {0x00, 0X07};
 32 |     const uint8_t OPERATING_MODE[2] = {0X20, 0X32};
 33 |     const uint8_t VEL_MODE[2] = {0x00, 0X03};
 34 |     const uint8_t SET_RPM[2] = {0x20, 0X3A};
 35 |     const uint8_t GET_RPM[2] = {0x20, 0X2C};
 36 |     const uint8_t SET_ACC_TIME[2] = {0x20, 0X81};
 37 |     const uint8_t SET_DECC_TIME[2] = {0x20, 0X83};
 38 |     const uint8_t SET_KP[2] = {0x20, 0X1D};
 39 |     const uint8_t SET_KI[2] = {0x20, 0X1E};
 40 |     const uint8_t INITIAL_SPEED[2] = {0X20, 0X08};
 41 |     const uint8_t MAX_SPEED[2] = {0X20, 0X0A};
 42 |     const uint8_t ACTUAL_POSITION_H[2] = {0X20, 0X2A};
 43 |     const uint8_t ACTUAL_POSITION_L[2] = {0X20, 0X2B};
 44 | 
 45 |     /**
 46 |      * @brief calculates the crc and stores it in the hex_cmd array, so there is no return value
 47 |      */
 48 |     void calculate_crc();
 49 | 
 50 |     /**
 51 |      * @brief reads from the serial port and saves the string into the receive_hex array
 52 |      * @param num_bytes how many bytes to read from the buffer
 53 |      * @return return 0 when OK, 1 if crc error
 54 |      */
 55 |     uint8_t read_hex(uint8_t num_bytes);
 56 | 
 57 |     /**
 58 |      * @brief print the hex command for debugging
 59 |      */
 60 |     void print_hex_cmd() const;
 61 | 
 62 |     /**
 63 |      * @brief print received hex for debugging
 64 |      */
 65 |     void print_rec_hex() const;
 66 | 
 67 | public:
 68 |     serial::Serial _serial;
 69 | 
 70 |     /**
 71 |      * @brief open serial port communication
 72 |      * @param port COM port eg. "/dev/ttyUSB0"
 73 |      * @param baudRate default baudrate is 115200
 74 |      * @param _ID Set the modbus ID of the motor driver in HEX, default 0x00
 75 |      */
 76 |     void begin(std::string port, int baudrate = 115200, uint8_t ID = 0x00);
 77 |     uint8_t set_vel_mode();
 78 | 
 79 |     /**
 80 |      * @param acc_time_ms acceleration time in ms eg. 500
 81 |      * @return 0 when OK. 1 if crc error
 82 |      */
 83 |     uint8_t set_acc_time(uint16_t acc_time_ms);
 84 | 
 85 |     /**
 86 |      * @param decc_time_ms decceleration time in ms eg. 500
 87 |      * @return 0 when OK. 1 if crc error
 88 |      */
 89 |     uint8_t set_decc_time(uint16_t decc_time_ms);
 90 | 
 91 |     /**
 92 |      * @param proportional_gain Speed Proportional Gain. Default: 500
 93 |      * @return 0 when OK. 1 if crc error
 94 |      */
 95 |     uint8_t set_kp(uint16_t proportional_gain);
 96 | 
 97 |     /**
 98 |      * @param integral_gain Speed Integral Gain. Default: 100
 99 |      * @return 0 when OK. 1 if crc error
100 |      */
101 |     uint8_t set_ki(uint16_t integral_gain);
102 | 
103 |     /**
104 |      * @return 0 when OK. 1 if crc error
105 |      */
106 |     uint8_t enable();
107 | 
108 |     /**
109 |      * @brief when motor disabled wheel can spin freely but still can read the rpm
110 |      * @return 0 when OK. 1 if crc error
111 |      */
112 |     uint8_t disable();
113 | 
114 |     /**
115 |      * @param rpm
116 |      * @param side
117 |      * @return alwasy 0
118 |      */
119 |     uint8_t set_rpm(int16_t rpm, std::string side);
120 | 
121 |     /**
122 |      * @param rpm
123 |      * @return alwasy 0
124 |      */
125 |     uint8_t set_sync_rpm(int16_t rpm);
126 | 
127 |     /**
128 |      * @return rpm measured from wheel
129 |      */
130 |     float get_rpm();
131 | 
132 |     /**
133 |      * @return Actual position feedback, unit: counts
134 |      */
135 |     int32_t get_position();
136 | 
137 |     /**
138 |      * @return Actual torque feedback, unit: A
139 |      */
140 |     float get_torque();
141 | 
142 |     /**
143 |      * @return Error feedback,
144 |      *          0000h: no error;
145 |      *          0001h: over-voltage;
146 |      *          0002h: under-voltage;
147 |      *          0004h: over-current;
148 |      *          0008h: overload;
149 |      *          0010h: current is out of tolerance;
150 |      *          0020h: encoder is out of tolerance;
151 |      *          0040h: speed is out of tolerance;
152 |      *          0080h: reference voltage error;
153 |      *          0100h: EEPROM read and write error;
154 |      *          0200h: Hall error;
155 |      *          0400h: motor temperature is too high.
156 |      */
157 |     uint16_t get_error();
158 | 
159 |     /**
160 |      * @brief Read data form the motor
161 |      *        - position in counts, one rotation has about 4090 counts
162 |      *        - rpm
163 |      *        - torque in 0.1 A
164 |      *        - Error message
165 |      * @return 0 if ok, 1 if crc read error
166 |      */
167 |     uint8_t read_motor();
168 | 
169 |     /**
170 |      * @return The ini tial speed when moti on begins.
171 |      */
172 |     uint8_t initial_speed(uint16_t rpm);
173 | 
174 |     /**
175 |      * @return Max operating speed of motor.
176 |      */
177 |     uint8_t max_speed(uint16_t rpm);
178 | 
179 |     void sleep(unsigned long milliseconds);
180 |     void say_hello();
181 | };
182 | 
183 | #endif


--------------------------------------------------------------------------------
/src/zlac8015d.cpp:
--------------------------------------------------------------------------------
  1 | #include "zlac8015d.h"
  2 | 
  3 | void ZLAC::begin(std::string port, int baudrate, uint8_t ID)
  4 | {
  5 |     this->ID = ID;
  6 |     _serial.setPort(port);
  7 |     _serial.setBaudrate(baudrate);
  8 |     serial::Timeout timeout = serial::Timeout::simpleTimeout(100);
  9 | 
 10 |     _serial.setTimeout(timeout);
 11 | 
 12 |     _serial.open();
 13 |     _serial.flushInput();
 14 |     printf("%d: SERIAL OK!", ID);
 15 | }
 16 | 
 17 | uint8_t ZLAC::set_rpm(int16_t rpm, std::string side)
 18 | {
 19 |     memset(hex_cmd, 0, sizeof(hex_cmd));
 20 |     hex_cmd[0] = ID;
 21 |     hex_cmd[1] = WRITE;
 22 |     hex_cmd[2] = 0x20;
 23 |     if (side == "LEFT")
 24 |     {
 25 |         hex_cmd[3] = 0x88;
 26 |     }
 27 |     else if (side == "RIGHT")
 28 |     {
 29 |         hex_cmd[3] = 0x89;
 30 |     }
 31 |     // for left motor, 0x88
 32 | 
 33 |     hex_cmd[4] = (rpm >> 8) & 0xFF;
 34 |     hex_cmd[5] = rpm & 0xFF;
 35 | 
 36 |     calculate_crc();
 37 | 
 38 |     // TODO isn't save, to continue reading infinite in case of error
 39 |     // do // repeat sending and read command as long as it has crc error
 40 |     // {
 41 |     //     _serial.write(hex_cmd, 8);
 42 |     //     // print_rec_hex();
 43 |     // } while (read_hex(8));
 44 |     _serial.write(hex_cmd, 8);
 45 |     read_hex(8);
 46 |     return 0;
 47 | }
 48 | 
 49 | uint8_t ZLAC::set_sync_rpm(int16_t rpm)
 50 | {
 51 |     memset(hex_cmd, 0, sizeof(hex_cmd));
 52 |     hex_cmd[0] = ID;
 53 |     hex_cmd[1] = MULTI_WRITE;
 54 |     hex_cmd[2] = 0x20;
 55 |     hex_cmd[3] = 0x88;
 56 |     hex_cmd[4] = 0x00;
 57 |     hex_cmd[5] = 0x02;
 58 |     hex_cmd[6] = 0x04;
 59 | 
 60 |     hex_cmd[7] = (rpm >> 8) & 0xFF;
 61 |     hex_cmd[8] = rpm & 0xFF;
 62 | 
 63 |     hex_cmd[9] = (rpm >> 8) & 0xFF;
 64 |     hex_cmd[10] = rpm & 0xFF;
 65 | 
 66 |     // TODO isn't save, to continue reading infinite in case of error
 67 |     // do // repeat sending and read command as long as it has crc error
 68 |     // {
 69 |     //     _serial.write(hex_cmd, 8);
 70 |     //     // print_rec_hex();
 71 |     // } while (read_hex(8));
 72 |     calculate_crc();
 73 | 
 74 |     printf("serial.write :");
 75 |     for (int i = 0; i < 13; ++i)
 76 |     {
 77 |         printf("%02X ", hex_cmd[i]);
 78 |     }
 79 |     _serial.write(hex_cmd, 13);
 80 |     read_hex(8);
 81 |     return 0;
 82 | }
 83 | 
 84 | float ZLAC::get_rpm()
 85 | {
 86 |     memset(hex_cmd, 0, sizeof(hex_cmd));
 87 |     hex_cmd[0] = ID;
 88 |     hex_cmd[1] = READ;
 89 |     hex_cmd[2] = GET_RPM[0];
 90 |     hex_cmd[3] = GET_RPM[1];
 91 | 
 92 |     hex_cmd[4] = 0x00;
 93 |     hex_cmd[5] = 0x01;
 94 | 
 95 |     calculate_crc();
 96 |     // print_hex_cmd();
 97 | 
 98 |     // TODO isn't save, to continue reading infinite in case of error
 99 |     // do // repeat sending and read command as long as it has crc error
100 |     // {
101 |     //     _serial.write(hex_cmd, 8);
102 |     //     // print_rec_hex();
103 |     // } while (read_hex(7) );
104 | 
105 |     _serial.write(hex_cmd, 8);
106 |     read_hex(7);
107 |     int16_t rpm_tenth = receive_hex[8] + (receive_hex[7] << 8);
108 |     return (float)rpm_tenth / 10.0f;
109 | }
110 | 
111 | float ZLAC::get_torque()
112 | {
113 |     int16_t torque = receive_hex[10] + (receive_hex[9] << 8);
114 |     return (float)torque / 10.0f;
115 | }
116 | 
117 | uint16_t ZLAC::get_error()
118 | {
119 |     memset(hex_cmd, 0, sizeof(hex_cmd));
120 |     hex_cmd[0] = 0x03;
121 |     hex_cmd[1] = 0x03;
122 |     hex_cmd[2] = 0x20;
123 |     hex_cmd[3] = 0x00;
124 |     hex_cmd[4] = 0x01;
125 |     calculate_crc();
126 |     _serial.write(hex_cmd, 7);
127 |     printf("serial.write :");
128 |     for (int i = 0; i < 7; ++i)
129 |     {
130 |         printf("%02X ", hex_cmd[i]);
131 |     }
132 | 
133 |     printf("\nresponse:\n");
134 |     if (read_hex(7))
135 |     {
136 |         printf("\nNo response.... Nah\n");
137 |         return 1;
138 |     }
139 |     return 0;
140 |     // return receive_hex[12] + (receive_hex[11] << 8);
141 | }
142 | 
143 | int32_t ZLAC::get_position()
144 | {
145 |     // // memset(hex_cmd, 0, sizeof(hex_cmd));
146 |     // hex_cmd[0] = ID;
147 |     // hex_cmd[1] = READ;
148 |     // hex_cmd[2] = ACTUAL_POSITION_H[0];
149 |     // hex_cmd[3] = ACTUAL_POSITION_H[1];
150 |     // hex_cmd[4] = 0x00;
151 |     // hex_cmd[5] = 0x02;
152 | 
153 |     // calculate_crc();
154 | 
155 |     // _serial.write(hex_cmd, 8);
156 | 
157 |     // // read_hex(7);
158 |     // std::string line = _serial.read(9);
159 |     // // print_hex_cmd();
160 |     // // convert string to hex
161 |     // for (uint8_t i = 0; i < uint8_t(line.size()); i++)
162 |     // {
163 |     //     receive_hex[i] = uint8_t(line[i]);
164 |     //     // printf("rec %d, %02x\n", i, receive_hex[i]);
165 |     // }
166 | 
167 |     // // crc check of received data
168 |     // if (crc16(receive_hex, 9) != 0)
169 |     // {
170 |     //     printf("crc checking error get postion\n");
171 |     //     // return 1;
172 |     // }
173 |     // // print_rec_hex(8);
174 | 
175 |     return receive_hex[6] + (receive_hex[5] << 8) + (receive_hex[4] << 16) + (receive_hex[3] << 24);
176 | }
177 | 
178 | uint8_t ZLAC::read_motor()
179 | {
180 |     // memset(hex_cmd, 0, sizeof(hex_cmd));
181 |     hex_cmd[0] = ID;
182 |     hex_cmd[1] = READ;
183 |     hex_cmd[2] = ACTUAL_POSITION_H[0];
184 |     hex_cmd[3] = ACTUAL_POSITION_H[1];
185 |     hex_cmd[4] = 0x00;
186 |     hex_cmd[5] = 0x05;
187 | 
188 |     calculate_crc();
189 | 
190 |     _serial.write(hex_cmd, 8);
191 | 
192 |     // read_hex(7);
193 |     std::string line = _serial.read(15);
194 |     // print_hex_cmd();
195 |     // convert string to hex
196 |     for (uint8_t i = 0; i < uint8_t(line.size()); i++)
197 |     {
198 |         receive_hex[i] = uint8_t(line[i]);
199 |         // printf("rec %d, %02x\n", i, receive_hex[i]);
200 |     }
201 | 
202 |     // crc check of received data
203 |     // if (crc16(receive_hex, 15) != 0)
204 |     // {
205 |     //     _serial.flush();
206 |     //     // printf("crc checking error read_motor\n");
207 |     //     return 1;
208 |     // }
209 |     // print_rec_hex();
210 | 
211 |     return 0;
212 | }
213 | 
214 | uint8_t ZLAC::enable()
215 | {
216 |     memset(hex_cmd, 0, sizeof(hex_cmd));
217 |     hex_cmd[0] = ID;
218 |     hex_cmd[1] = WRITE;
219 |     hex_cmd[2] = 0x20;
220 |     hex_cmd[3] = 0x0E;
221 | 
222 |     hex_cmd[4] = ENABLE[0];
223 |     hex_cmd[5] = ENABLE[1];
224 | 
225 |     calculate_crc();
226 |     printf("serial.write :");
227 |     for (int i = 0; i < 8; ++i)
228 |     {
229 |         printf("%02X ", hex_cmd[i]);
230 |     }
231 |     _serial.write(hex_cmd, 8);
232 |     printf("\nresponse:\n");
233 |     if (read_hex(8))
234 |     {
235 |         printf("\nNo response.... Nah\n");
236 |         return 1;
237 |     }
238 |     return 0;
239 | }
240 | 
241 | uint8_t ZLAC::disable()
242 | {
243 |     memset(hex_cmd, 0, sizeof(hex_cmd));
244 |     hex_cmd[0] = ID;
245 |     hex_cmd[1] = WRITE;
246 |     hex_cmd[2] = 0x20;
247 |     hex_cmd[3] = 0x31;
248 | 
249 |     hex_cmd[4] = DISABLE[0];
250 |     hex_cmd[5] = DISABLE[1];
251 | 
252 |     calculate_crc();
253 |     printf("serial.write :");
254 |     for (int i = 0; i < 8; ++i)
255 |     {
256 |         printf("%02X ", hex_cmd[i]);
257 |     }
258 |     _serial.write(hex_cmd, 8);
259 |     if (read_hex(8))
260 |     {
261 |         printf("\nNo response.... Nah\n");
262 |         return 1;
263 |     }
264 |     return 0;
265 | }
266 | 
267 | uint8_t ZLAC::set_vel_mode()
268 | {
269 |     // memset(hex_cmd, 0, sizeof(hex_cmd));
270 |     hex_cmd[0] = ID;
271 |     hex_cmd[1] = WRITE;
272 | 
273 |     // hex_cmd[2] = OPERATING_MODE[0];
274 |     // hex_cmd[3] = OPERATING_MODE[1];
275 |     hex_cmd[2] = 0x20;
276 |     hex_cmd[3] = 0x0D;
277 |     hex_cmd[4] = VEL_MODE[0];
278 |     hex_cmd[5] = VEL_MODE[1];
279 | 
280 |     calculate_crc();
281 |     _serial.write(hex_cmd, 8);
282 |     printf("serial.write :");
283 |     for (int i = 0; i < 8; ++i)
284 |     {
285 |         printf("%02X ", hex_cmd[i]);
286 |     }
287 |     printf("\nresponse:\n");
288 |     if (read_hex(8))
289 |     {
290 |         printf("\nNo response.... Nah\n");
291 |         return 1;
292 |     }
293 |     return 0;
294 | }
295 | 
296 | uint8_t ZLAC::set_acc_time(uint16_t acc_time_ms)
297 | {
298 |     // memset(hex_cmd, 0, sizeof(hex_cmd));
299 |     hex_cmd[0] = ID;
300 |     hex_cmd[1] = WRITE;
301 |     hex_cmd[2] = SET_ACC_TIME[0];
302 |     hex_cmd[3] = SET_ACC_TIME[1];
303 | 
304 |     hex_cmd[4] = (acc_time_ms >> 8) & 0xFF;
305 |     hex_cmd[5] = acc_time_ms & 0xFF;
306 | 
307 |     calculate_crc();
308 |     _serial.write(hex_cmd, 8);
309 |     if (read_hex(8))
310 |         return 1;
311 |     return 0;
312 | }
313 | 
314 | uint8_t ZLAC::set_decc_time(uint16_t decc_time_ms)
315 | {
316 |     // memset(hex_cmd, 0, sizeof(hex_cmd));
317 |     hex_cmd[0] = ID;
318 |     hex_cmd[1] = WRITE;
319 |     hex_cmd[2] = SET_DECC_TIME[0];
320 |     hex_cmd[3] = SET_DECC_TIME[1];
321 | 
322 |     hex_cmd[4] = (decc_time_ms >> 8) & 0xFF;
323 |     hex_cmd[5] = decc_time_ms & 0xFF;
324 | 
325 |     calculate_crc();
326 |     _serial.write(hex_cmd, 8);
327 |     if (read_hex(8))
328 |         return 1;
329 |     return 0;
330 | }
331 | 
332 | uint8_t ZLAC::set_kp(uint16_t proportional_gain)
333 | {
334 |     // memset(hex_cmd, 0, sizeof(hex_cmd));
335 |     hex_cmd[0] = ID;
336 |     hex_cmd[1] = WRITE;
337 |     hex_cmd[2] = SET_KP[0];
338 |     hex_cmd[3] = SET_KP[1];
339 | 
340 |     hex_cmd[4] = (proportional_gain >> 8) & 0xFF;
341 |     hex_cmd[5] = proportional_gain & 0xFF;
342 | 
343 |     calculate_crc();
344 |     _serial.write(hex_cmd, 8);
345 |     if (read_hex(8))
346 |         return 1;
347 |     return 0;
348 | }
349 | 
350 | uint8_t ZLAC::set_ki(uint16_t integral_gain)
351 | {
352 |     // memset(hex_cmd, 0, sizeof(hex_cmd));
353 |     hex_cmd[0] = ID;
354 |     hex_cmd[1] = WRITE;
355 |     hex_cmd[2] = SET_KI[0];
356 |     hex_cmd[3] = SET_KI[1];
357 | 
358 |     hex_cmd[4] = (integral_gain >> 8) & 0xFF;
359 |     hex_cmd[5] = integral_gain & 0xFF;
360 | 
361 |     calculate_crc();
362 |     _serial.write(hex_cmd, 8);
363 |     if (read_hex(8))
364 |         return 1;
365 |     return 0;
366 | }
367 | 
368 | uint8_t ZLAC::initial_speed(uint16_t rpm)
369 | {
370 |     // memset(hex_cmd, 0, sizeof(hex_cmd));
371 |     hex_cmd[0] = ID;
372 |     hex_cmd[1] = WRITE;
373 |     hex_cmd[2] = INITIAL_SPEED[0];
374 |     hex_cmd[3] = INITIAL_SPEED[1];
375 | 
376 |     hex_cmd[4] = (rpm >> 8) & 0xFF;
377 |     hex_cmd[5] = rpm & 0xFF;
378 | 
379 |     calculate_crc();
380 |     _serial.write(hex_cmd, 8);
381 |     if (read_hex(8))
382 |         return 1;
383 |     return 0;
384 | }
385 | 
386 | uint8_t ZLAC::max_speed(uint16_t rpm)
387 | {
388 |     // memset(hex_cmd, 0, sizeof(hex_cmd));
389 |     hex_cmd[0] = ID;
390 |     hex_cmd[1] = WRITE;
391 |     hex_cmd[2] = MAX_SPEED[0];
392 |     hex_cmd[3] = MAX_SPEED[1];
393 | 
394 |     hex_cmd[4] = (rpm >> 8) & 0xFF;
395 |     hex_cmd[5] = rpm & 0xFF;
396 | 
397 |     calculate_crc();
398 |     _serial.write(hex_cmd, 8);
399 |     if (read_hex(8))
400 |         return 1;
401 |     return 0;
402 | }
403 | 
404 | // HELPER Functions *************************************************************
405 | 
406 | void ZLAC::sleep(unsigned long milliseconds)
407 | {
408 | #ifdef _WIN32
409 |     Sleep(milliseconds); // 100 ms
410 | #else
411 |     usleep(milliseconds * 1000); // 100 ms
412 | #endif
413 | }
414 | 
415 | void ZLAC::calculate_crc()
416 | {
417 |     // calculate crc and append to hex cmd
418 |     unsigned short result = crc16(hex_cmd, sizeof(hex_cmd) - 2);
419 |     hex_cmd[sizeof(hex_cmd) - 2] = result & 0xFF;
420 |     hex_cmd[sizeof(hex_cmd) - 1] = (result >> 8) & 0xFF;
421 | }
422 | 
423 | uint8_t ZLAC::read_hex(uint8_t num_bytes)
424 | {
425 |     serial::Timeout timeout = serial::Timeout::simpleTimeout(1000);
426 |     _serial.setTimeout(timeout);
427 | 
428 |     std::string line = _serial.read(num_bytes);
429 | 
430 |     // convert string to hex
431 |     printf("receive_hex:");
432 |     for (uint8_t i = 0; i < uint8_t(line.size()); i++)
433 |     {
434 |         receive_hex[i] = uint8_t(line[i]);
435 |         printf(" %02x", receive_hex[i]);
436 |     }
437 |     // crc check of received data
438 |     if (crc16(receive_hex, num_bytes) != 0)
439 |     {
440 |         printf("crc checking error\n");
441 |         return 1;
442 |     }
443 |     return 0;
444 | }
445 | 
446 | void ZLAC::print_hex_cmd() const
447 | {
448 |     // print
449 |     for (int i = 0; i < 8; i++)
450 |     {
451 |         printf("%d, %02x\n", i, hex_cmd[i]);
452 |     }
453 | }
454 | 
455 | void ZLAC::print_rec_hex() const
456 | {
457 |     // print
458 |     for (int i = 0; i < 8; i++)
459 |     {
460 |         printf("rec: %d, %02x\n", i, receive_hex[i]);
461 |     }
462 | }
463 | 
464 | void ZLAC::say_hello()
465 | {
466 |     printf("Hello World");
467 | }


--------------------------------------------------------------------------------