├── lib.so ├── safety.jpg ├── modbusrtu.h ├── main.h ├── modbustcp.h ├── README.md ├── pycontrol.py ├── modbusrtu.c └── modbustcp.c /lib.so: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Orienfish/ur3-python/HEAD/lib.so -------------------------------------------------------------------------------- /safety.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Orienfish/ur3-python/HEAD/safety.jpg -------------------------------------------------------------------------------- /modbusrtu.h: -------------------------------------------------------------------------------- 1 | /**************************************************** 2 | * This file control the gripper through modbus rtu 3 | * Modified by xfyu on Jan 22 4 | ****************************************************/ 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | 14 | #include "main.h" 15 | 16 | #define MODBUS_DEV "/dev/ttyUSB0" 17 | #define BAUDRATE B115200 18 | 19 | /* already defined otherwhere */ 20 | // #define BUF_SIZE 512 21 | 22 | // #define DEBUG 23 | 24 | /************************************** 25 | * Function Definitions 26 | **************************************/ 27 | int bufcmp(unsigned char *s1, unsigned char *s2); 28 | int open_modbus(); 29 | int gripper_activate(); 30 | int gripper_close(unsigned char speed, unsigned char force); 31 | int gripper_open(unsigned char speed, unsigned char force); 32 | unsigned short ModBusCRC(unsigned char * ptr, unsigned char size); 33 | void Generate_Open_Close_Instruct(unsigned char speed, unsigned char force); -------------------------------------------------------------------------------- /main.h: -------------------------------------------------------------------------------- 1 | /******************************************************************************** 2 | ** main.h - necessary definitions 3 | ** 4 | ** Created by xfyu on Jan 19, 2018 5 | ******************************************************************************** 6 | /* necessary macro definitions */ 7 | // #define DEBUG 8 | 9 | #define BUF_SIZE 512 /* maximum rev length */ 10 | #define REG_NUM 6 /* 6 values for 1 pose */ 11 | #define PICNAMESIZE 256 12 | 13 | /* Used in read_pose */ 14 | #define FOR_CUR 0 15 | #define FOR_COARSE 1 16 | #define FOR_FINE 2 17 | 18 | /* choices of LastOP */ 19 | #define UP 1 20 | #define DOWN -1 21 | 22 | /* choices of coarse_or_fine */ 23 | #define COARSE 0 24 | #define FINE 1 25 | #define FOCUS_POS_FILE "init_pos.txt" 26 | 27 | /* The focus step */ 28 | #define FOCUS_STEP 0.3 29 | 30 | /* Control the gripper */ 31 | /* Speed and force are set from python side */ 32 | #define FORCE_DEFAULT 0 33 | #define SPEED_DEFAULT 40 34 | 35 | /* Threshold used in judging whether last 36 | operation is finished */ 37 | #define THRESHOLD 0.005 38 | 39 | /* min and max limitation of wrist 3 */ 40 | #define MIN_ANGLE 0.31 41 | #define MAX_ANGLE 5.97 42 | 43 | /* one loop coarse = TIMES * one loop fine */ 44 | #define TIMES 9 45 | 46 | -------------------------------------------------------------------------------- /modbustcp.h: -------------------------------------------------------------------------------- 1 | /************************************************** 2 | * This file is for modbus tcp 3 | * Modified by xfyu on Jan 22 4 | **************************************************/ 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | 14 | #include 15 | #include 16 | #include 17 | #include 18 | 19 | #include "main.h" 20 | #include "modbusrtu.h" 21 | 22 | #define ROBOT_ADDR "192.168.0.1" /* server addr */ 23 | #define MODBUS_PORT 502 /* server port */ 24 | #define REG_NUM 6 /* num of reg to read */ 25 | 26 | /* for realtime socket connection */ 27 | #define REALTIME_ADDR "192.168.0.1" 28 | #define REALTIME_PORT 30003 /* real time client */ 29 | 30 | /************************************************** 31 | * Function Definitions 32 | **************************************************/ 33 | int connect_modbustcp(); 34 | int connect_realtime(); 35 | int read_pos(float * recv_value); 36 | int read_wrist(float * recv_value); 37 | void send_movel_instruct(float * desired_pose); 38 | void send_movej_instruct(float * desired_joint); 39 | float wait_until_nodiff(float *a1, float *a2); 40 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # ur3-python 2 | 能够读取ur3机械臂位置信息、控制ur3移动、控制机械爪开关的python API。
3 | 以Ｃ语言作为底层交互代码可以保证执行速度，因此将之前完成的基于modbus的C代码生成动态链接库，在python中调用，以此达到python接口控制机械臂和读取机械臂信息的目的。
4 | 5 | # 文件概览 6 | * **pycontrol.py**
7 | 核心python源码，main函数中为简单的测试和调用接口示例。提供的python函数接口如下：
8 | * read_pos：读取基座坐标系下机械臂的位置。
无输入参数，返回一个c_float类型的数组，为当前基座坐标系中ur3 TCP的位置坐标[x,y,z,rx,ry,rz]，其中x,y,z的单位是米，rx,ry,rz的单位是rad，均保留到小数点后三位小数。
9 | * read_wrist：读取机械臂的各关节的角度。
无输入参数，返回一个c_float类型的数组，为当前ur3机械臂六个关节的角度值[base,shoulder,elbow,wrist1,wrist2,wrist3]，单位是rad，保留到小数点后三位小数。由于modbus传输的只能是无符号16bit数，所以将机械臂的安全范围设定为： 10 | ![image](https://github.com/Orienfish/ur3-python/raw/master/safety.jpg) 11 | * send_movel_instruct：在基座坐标系下控制机械臂移动。
输入参数是一个c_float类型的指针，指向基座坐标系下目标位置的坐标[x,y,z,rx,ry,rz],其中x,y,z的单位是米，rx,ry,rz的单位是rad。无返回值，错误信息会显示在终端。
12 | * send_movej_instruct：控制各关节角度移动到目标位置。
输入参数是一个c_float类型的指针，指向目标位置的各关节角度[base,shoulder,elbow,wrist1,wrist2,wrist3]，单位是rad。无返回值，错误信息会显示在终端。
13 | * gripper_activate：激活机械爪，需要在每次使用机械爪前调用。
无输入参数，返回０表示成功，返回-１表示失败。
14 | * gripper_open：打开机械爪。
输入参数是速度和力，两者作为全局变量在pycontrol.py的第11-12行定义。返回０表示成功，返回-1表示失败。
15 | * gripper_close：闭合机械爪。
输入参数同样是速度和力。返回０表示成功，返回-1表示失败。
16 | * **modbustcp.c modbustcp.h**
17 | 基于Modbus TCP协议控制的Ｃ语言代码。主要建立TCP连接，并根据相关协议读取实时位置信息、发送控制指令给机械臂。
18 | * **modbusrtu.c modbusrtu.h**
19 | 基于Modbus RTU协议控制的Ｃ语言代码。主要通过USB控制机械爪开、关、激活。
20 | * **main.h** 保存很多宏定义变量的文件。
21 | * **lib.so** 由Ｃ代码生成的动态链接库。
生成指令为gcc -o lib.so -fPIC -shared modbustcp.c modbusrtu.c.
22 | 23 | # 硬件连接 24 | 主要有两条连接： 25 | * **Modbus TCP连接**　通过网线连接PC机和ur3机械臂。机械臂端的IP为192.168.0.1，PC机端的IP为192.168.0.2。在机械臂开机后，通过ifconfig设置PC机的IP，之后可通过ping检查是否奏效。另一种判断方法时，屏幕右上角显示连接有线连接2时则连接成功。 26 | ``` 27 | sudo ifconfig enx30b49e79f639 192.168.0.2 28 | ping 192.168.0.1 29 | ``` 30 | * **Modbus RTU** 通过USB线连接PC机和ur3机械臂。 31 | 可以通过机械臂顶端侧面的灯是否亮来判断机箱中的24v,0v是否连接上，此处易掉。 32 | 用以下指令 33 | ``` 34 | ls /dev/ttyUSB* 35 | ``` 36 | 可检查USB是否连接上。如果终端显示有/dev/ttyUSB0，则说明已经连接好；否则，可以尝试重新插拔PC机端的USB接口。 37 | -------------------------------------------------------------------------------- /pycontrol.py: -------------------------------------------------------------------------------- 1 | ####################################################################### 2 | # This file is the python API to control UR3 robotic arm. 3 | # Modified by xfyu on Apr 6 4 | ####################################################################### 5 | # -*- coding: utf-8 -*- 6 | # !/usr/bin/python 7 | from ctypes import * 8 | import time 9 | 10 | REG_NUM = 6 11 | FORCE = 20 12 | SPEED = 40 13 | 14 | # import library 15 | # gcc -o lib.so -fPIC -shared modbustcp.c modbusrtu.c 16 | lib = cdll.LoadLibrary("/home/robot/RL/lib.so") 17 | 18 | # 19 | # read_pos - Read 6 pos value 20 | # p[x, y, z, rx, ry, rz]. The x, y, z are in 0.1mm base. 21 | # The rx, ry, rz are in 0.001rad base. 22 | # 23 | # Parameter: None 24 | # Return value: success - 6 values in m base and rad base 25 | # fail - None 26 | def read_pos(): 27 | # set the type of parameters 28 | lib.read_pos.argtype = [POINTER(c_float),] 29 | lib.read_pos.restype = c_int 30 | ret_value = (c_float * REG_NUM)() 31 | 32 | # read pos position 33 | res = lib.read_pos(ret_value) 34 | if not res: # read successfully 35 | return ret_value 36 | return None # fail 37 | 38 | # 39 | # read_wrist - Read 6 joint value 40 | # [base, shoulder, elbow, wrist1, wrist2, wrist3]. 41 | # All values are in 0.001rad base. 42 | # 43 | # Parameter: None 44 | # Return value: success - 6 values in rad base 45 | # fail - None 46 | # 47 | def read_wrist(): 48 | # set the type of parameters 49 | lib.read_wrist.argtype = [POINTER(c_float),] 50 | lib.read_wrist.restype = c_int 51 | ret_value = (c_float * REG_NUM)() 52 | 53 | # read pos position 54 | res = lib.read_wrist(ret_value) 55 | if not res: # read successfully 56 | return ret_value 57 | return None # fail 58 | 59 | # 60 | # send_movel_instruct - send move intruction to robot 61 | # Using position. 62 | # 63 | # Input: desired_pose - the target position 64 | # Return: None. 65 | # Error message will display in the Msg box. 66 | # 67 | def send_movel_instruct(desired_pose_pointer): 68 | # set the type of parameters 69 | lib.send_movel_instruct.argtype = [POINTER(c_float),] 70 | lib.send_movel_instruct(desired_pose_pointer) 71 | 72 | # 73 | # send_movej_instruct - send move intruction to robot 74 | # Using joint positions. 75 | # move to the position decribed by angles' values 76 | # 77 | # Input: float * desired_joint - target position 78 | # Return: none. 79 | # Error message will display in the Msg box. 80 | # 81 | def send_movej_instruct(desired_joint_pointer): 82 | # set the type of parameters 83 | lib.send_movej_instruct.argtype = [POINTER(c_float),] 84 | lib.send_movej_instruct(desired_joint_pointer) 85 | 86 | # 87 | # gripper_activate - activate the gripper 88 | # 89 | # Speed and force are set in the above 90 | # Return Value: 0 - success 91 | # -1 - fail 92 | # 93 | def gripper_activate(): 94 | lib.gripper_activate.restype = c_int 95 | res = lib.gripper_activate() 96 | return res 97 | 98 | # 99 | # gripper_close - close the gripper 100 | # 101 | # Speed and force are set in the above 102 | # Return Value: 0 - success 103 | # -1 - fail 104 | # 105 | def gripper_close(): 106 | lib.gripper_close.argtype = [c_ubyte, c_ubyte] 107 | lib.gripper_close.restype = c_int 108 | res = lib.gripper_close(SPEED, FORCE) 109 | return res 110 | 111 | # 112 | # gripper_open - open the gripper 113 | # 114 | # Speed and force are set in the above 115 | # Return Value: 0 - success 116 | # -1 - fail 117 | # 118 | def gripper_open(): 119 | lib.gripper_close.argtype = [c_ubyte, c_ubyte] 120 | lib.gripper_close.restype = c_int 121 | res = lib.gripper_open(SPEED, FORCE) 122 | return res 123 | 124 | # main test 125 | if __name__ == '__main__': 126 | gripper_activate() 127 | 128 | ''' 129 | Test 1: read current position and send movl instruction 130 | ''' 131 | ret = read_pos() 132 | print "%f %f %f %f %f %f" %(ret[0], ret[1], ret[2], ret[3], ret[4], ret[5]) 133 | ret[2] = ret[2] + 0.05 134 | send_movel_instruct(byref(ret)) 135 | print "finish test 1" 136 | time.sleep(3) 137 | 138 | ''' 139 | Test 2: read current wrist angle and send movej instruction 140 | ''' 141 | ret = read_wrist() 142 | print "%f %f %f %f %f %f" %(ret[0], ret[1], ret[2], ret[3], ret[4], ret[5]) 143 | ret[2] = ret[2] + 0.3 144 | send_movej_instruct(byref(ret)) 145 | print "finish test 2" 146 | time.sleep(3) 147 | 148 | ''' 149 | Test 3: when reach minimum and maximum bound 150 | ''' 151 | # target angle value 152 | goal = (c_float*REG_NUM)(5.523,-1.145,0.812,0.815,0.455,0.054) 153 | gripper_close() 154 | send_movej_instruct(byref(goal)) 155 | send_movej_screw(DOWN) 156 | send_movej_screw(UP) 157 | send_movej_screw(UP) 158 | -------------------------------------------------------------------------------- /modbusrtu.c: -------------------------------------------------------------------------------- 1 | /**************************************************** 2 | * This file control the gripper through modbus rtu 3 | * Modified by xfyu on Jan 22 4 | ****************************************************/ 5 | #include "modbusrtu.h" 6 | #include "main.h" 7 | 8 | /**************************************************** 9 | * Instruction Set 10 | ****************************************************/ 11 | unsigned char activate[] = { 12 | 0x09, 0x10, 13 | 0x03, 0xe8, 14 | 0x00, 0x03, 15 | 0x06, 16 | 0x00, 0x00, 17 | 0x00, 0x00, 18 | 0x00, 0x00, 19 | 0x73, 0x30 20 | }; 21 | 22 | unsigned char read_gripper_status[] = { 23 | 0x09, 0x03, 24 | 0x07, 0xd0, 25 | 0x00, 0x01, 26 | 0x85, 0xcf 27 | }; 28 | 29 | unsigned char activate_success[] = { 30 | 0x09, 0x03, 31 | 0x02, 32 | 0x00, 0x00, 33 | 0x59, 0x85 34 | }; 35 | 36 | unsigned char close_with_certain_speed_certain_force[] = { 37 | 0x09, 0x10, 38 | 0x03, 0xe8, 39 | 0x00, 0x03, 40 | 0x06, 41 | 0x09, 0x00, 42 | 0x00, 0xff, 43 | 0xff, 0xff, 44 | 0x42, 0x29 45 | }; 46 | 47 | unsigned char read_until_grip_completed[] = { 48 | 0x09, 0x03, 49 | 0x07, 0xd0, 50 | 0x00, 0x03, 51 | 0x04, 0x0e 52 | }; 53 | 54 | unsigned char grip_is_completed1[] = { 55 | 0x09, 0x03, 56 | 0x02, 0xb9, 57 | 0x00, 0x2a, 0x15 58 | }; 59 | 60 | unsigned char grip_is_completed2[] = { 61 | 0x09, 0x03, 62 | 0x02, 0xf9, 63 | 0x00, 0x1b, 0xd5 64 | }; 65 | 66 | unsigned char open_with_certain_speed_certain_force[] = { 67 | 0x09, 0x10, 68 | 0x03, 0xe8, 69 | 0x00, 0x03, 70 | 0x06, 71 | 0x09, 0x00, 72 | 0x00, 0x00, 73 | 0xff, 0xff, 74 | 0x72, 0x19 75 | }; 76 | 77 | unsigned char read_until_open_completed[] = { 78 | 0x09, 0x03, 79 | 0x07, 0xd0, 80 | 0x00, 0x03, 81 | 0x04, 0x0e 82 | }; 83 | 84 | unsigned char open_is_completed[] = { 85 | 0x09, 0x03, 86 | 0x06, 87 | 0xf9, 0x00, 88 | 0x00, 0x00, 89 | 0x03, 0x00, 90 | 0x52, 0x2c 91 | }; 92 | 93 | /**************************************************** 94 | * Functions 95 | ****************************************************/ 96 | /* 97 | * bufcmp - Compare the recv buf with what we 98 | * already have 99 | * 100 | * Input: s1 - addr of the first buf 101 | * s2 - addr of the second buf 102 | * Return Value: 0 - same 103 | * 1 - different 104 | * We don't have to know which buf is smaller, we 105 | * only care whether they are same. 106 | */ 107 | int bufcmp(unsigned char *s1, unsigned char *s2) { 108 | int len1 = sizeof(s1) - 1; 109 | int len2 = sizeof(s2) - 1; 110 | // printf("%d %d\n", len1, len2); 111 | if (len1 != len2) 112 | return 1; /* match fail */ 113 | for (int i = 0; i < len1; ++i) 114 | if (s1[i] != s2[i]) 115 | return 1; 116 | return 0; 117 | } 118 | 119 | /* 120 | * open_modbus - open the serial port 121 | * 122 | * Return Value: >0 - the fd. success 123 | * <=0 - fail 124 | */ 125 | int open_modbus() { 126 | int fd; 127 | 128 | fd = open(MODBUS_DEV, O_RDWR); 129 | if (fd < 0) { 130 | perror("open tty error"); 131 | return -1; 132 | } 133 | 134 | struct termios options; 135 | tcgetattr(fd, &options); 136 | memset(&options, 0, sizeof(options)); 137 | // options.c_cflag |= CLOCAL | CREAD; 138 | options.c_cflag &= ~CSIZE; 139 | options.c_cflag |= CS8; /* 8 data bit */ 140 | 141 | options.c_cflag &= ~PARENB; /* no parity */ 142 | options.c_cflag &= ~CSTOPB; /* 1 stop bit */ 143 | 144 | /* set the baudrate */ 145 | if (cfsetispeed(&options, BAUDRATE) < 0) { 146 | perror("baudrate seti error"); 147 | return -1; 148 | } 149 | if (cfsetospeed(&options, BAUDRATE) < 0) { 150 | perror("baudrate seto error"); 151 | return -1; 152 | } 153 | /* set the wait time */ 154 | options.c_cc[VTIME] = 10; 155 | options.c_cc[VMIN] = 4; 156 | 157 | /* bind the options to fd */ 158 | if (tcsetattr(fd, TCSANOW, &options) < 0) { 159 | perror("attr set error"); 160 | return -1; 161 | } 162 | 163 | return fd; 164 | } 165 | /* 166 | * gripper_activate - activate the gripper 167 | * 168 | * Return Value: 0 - success 169 | * -1 - fail 170 | */ 171 | int gripper_activate() { 172 | int fd; 173 | int read_cnt; 174 | unsigned char recv_buf[BUF_SIZE]; 175 | 176 | if ((fd = open_modbus()) < 0) 177 | return -1; 178 | 179 | /* activate */ 180 | if (write(fd, activate, sizeof(activate)) < 0) { 181 | perror("write error"); 182 | return -1; 183 | } 184 | 185 | /* #ifdef DEBUG 186 | if ((read_cnt = read(fd, recv_buf, BUF_SIZE)) < 0) { 187 | perror("read error"); 188 | return -1; 189 | } 190 | 191 | fprintf(stdout, "Activate Receive: "); 192 | for (int i = 0; i < read_cnt; ++i) 193 | fprintf(stdout, "0x%x ", recv_buf[i]); 194 | fprintf(stdout, "\n"); 195 | #endif */ 196 | 197 | while (1) { 198 | if (write(fd, read_gripper_status, 199 | sizeof(read_gripper_status)) < 0) { 200 | perror("write error"); 201 | return -1; 202 | } 203 | /* recv gripper status */ 204 | if ((read_cnt = read(fd, recv_buf, BUF_SIZE)) < 0) { 205 | perror("read error"); 206 | return -1; 207 | } 208 | 209 | #ifdef DEBUG 210 | fprintf(stdout, "Activate Receive: "); 211 | for (int i = 0; i < read_cnt; ++i) 212 | fprintf(stdout, "0x%x ", recv_buf[i]); 213 | fprintf(stdout, "\n"); 214 | #endif 215 | if (!bufcmp(activate_success, recv_buf)) 216 | break; /* complete */ 217 | else 218 | continue; /* not complete */ 219 | } 220 | 221 | close(fd); 222 | return 0; 223 | } 224 | 225 | /* 226 | * gripper_close - close the gripper 227 | * 228 | * Input: speed: 0-255 229 | * force: 0-255 230 | * Return Value: 0 - success 231 | * -1 - fail 232 | */ 233 | int gripper_close(unsigned char speed, unsigned char force) { 234 | int fd; 235 | int read_cnt; 236 | unsigned char recv_buf[BUF_SIZE]; 237 | 238 | /* generate certain instruction */ 239 | Generate_Open_Close_Instruct(speed, force); 240 | 241 | if ((fd = open_modbus()) < 0) 242 | return -1; 243 | 244 | /* grip */ 245 | if (write(fd, close_with_certain_speed_certain_force, 246 | sizeof(close_with_certain_speed_certain_force)) < 0) { 247 | perror("write error"); 248 | return -1; 249 | } 250 | while (1) { 251 | if (write(fd, read_gripper_status, 252 | sizeof(read_gripper_status)) < 0) { 253 | perror("write error"); 254 | return -1; 255 | } 256 | /* recv gripper status */ 257 | if ((read_cnt = read(fd, recv_buf, BUF_SIZE)) < 0) { 258 | perror("read error"); 259 | return -1; 260 | } 261 | #ifdef DEBUG 262 | fprintf(stdout, "Close Receive: "); 263 | for (int i = 0; i < read_cnt; ++i) 264 | fprintf(stdout, "0x%x ", recv_buf[i]); 265 | fprintf(stdout, "\n"); 266 | #endif 267 | if (!bufcmp(grip_is_completed1, recv_buf)) 268 | break; /* complete 1 */ 269 | else if (!bufcmp(grip_is_completed2, recv_buf)) 270 | break; /* complete 2 */ 271 | else 272 | continue; /* not complete */ 273 | } 274 | 275 | close(fd); 276 | return 0; 277 | } 278 | 279 | /* 280 | * gripper_open - open the gripper 281 | * 282 | * Input: speed: 0-255 283 | * force: 0-255 284 | * Return Value: 0 - success 285 | * -1 - fail 286 | */ 287 | int gripper_open(unsigned char speed, unsigned char force) { 288 | int fd; 289 | int read_cnt; 290 | unsigned char recv_buf[BUF_SIZE]; 291 | 292 | /* generate certain instruction */ 293 | Generate_Open_Close_Instruct(speed, force); 294 | 295 | if ((fd = open_modbus()) < 0) 296 | return -1; 297 | 298 | /* open */ 299 | if (write(fd, open_with_certain_speed_certain_force, 300 | sizeof(open_with_certain_speed_certain_force)) < 0) { 301 | perror("write error"); 302 | return -1; 303 | } 304 | while (1) { 305 | if (write(fd, read_until_open_completed, 306 | sizeof(read_until_open_completed)) < 0) { 307 | perror("write error"); 308 | return -1; 309 | } 310 | /* recv gripper status */ 311 | if ((read_cnt = read(fd, recv_buf, BUF_SIZE)) < 0) { 312 | perror("read error"); 313 | return -1; 314 | } 315 | #ifdef DEBUG 316 | fprintf(stdout, "Open Receive: "); 317 | for (int i = 0; i < read_cnt; ++i) 318 | fprintf(stdout, "0x%x ", recv_buf[i]); 319 | fprintf(stdout, "\n"); 320 | #endif 321 | if (!bufcmp(open_is_completed, recv_buf)) 322 | break; /* complete */ 323 | else 324 | continue; /* not complete */ 325 | } 326 | 327 | close(fd); 328 | return 0; 329 | } 330 | 331 | /* 332 | * ModBusCRC - compute the crc of a certain string 333 | * 334 | * Parameter: ptr - the start of that string 335 | * size - the length of that string 336 | * Return value: the crc result, a short. 337 | */ 338 | unsigned short ModBusCRC(unsigned char * ptr, unsigned char size) { 339 | unsigned short a, b, tmp, CRC16; 340 | CRC16 = 0xffff; /* initiate CRC16 register value */ 341 | 342 | for (a = 0; a < size; ++a) { 343 | CRC16 = *ptr ^ CRC16; 344 | for (b = 0; b < 8; ++b) { 345 | tmp = CRC16 & 0x0001; 346 | CRC16 >>= 1; 347 | if (tmp) /* check the bit that move out */ 348 | CRC16 = CRC16 ^ 0xa001; 349 | } 350 | ptr++; /* move to the next byte */ 351 | } 352 | return ((CRC16 & 0xFF) << 8) | ((CRC16 & 0xFF00) >> 8); 353 | } 354 | 355 | /* 356 | * Generate_Open_Close_Instruct - generate the specific intruction 357 | * according to the Macro Definitions in main.h 358 | */ 359 | void Generate_Open_Close_Instruct(unsigned char speed, unsigned char force) { 360 | unsigned char length; 361 | unsigned short CRC; 362 | 363 | /* generate open instruction */ 364 | open_with_certain_speed_certain_force[11] = speed; 365 | open_with_certain_speed_certain_force[12] = force; 366 | length = sizeof(open_with_certain_speed_certain_force) - 2; 367 | /* Calculate the CRC */ 368 | CRC = ModBusCRC(open_with_certain_speed_certain_force, length); 369 | /* Add the CRC to the result */ 370 | open_with_certain_speed_certain_force[13] = CRC >> 8; 371 | open_with_certain_speed_certain_force[14] = CRC & 0xff; 372 | 373 | /* generate close instruction */ 374 | close_with_certain_speed_certain_force[11] = speed; 375 | close_with_certain_speed_certain_force[12] = force; 376 | /* Calculate the CRC, length is same */ 377 | CRC = ModBusCRC(close_with_certain_speed_certain_force, length); 378 | /* Add the CRC to the result */ 379 | close_with_certain_speed_certain_force[13] = CRC >> 8; 380 | close_with_certain_speed_certain_force[14] = CRC & 0xff; 381 | 382 | #ifdef DEBUG 383 | length += 2; 384 | char buf[BUF_SIZE]; 385 | sprintf(buf, "Open Instruction: "); 386 | for (int i = 0; i < length; ++i) 387 | sprintf(buf, "%s0x%x ", buf, open_with_certain_speed_certain_force[i]); 388 | fprintf(stdout, "%s\r\n", buf); 389 | 390 | sprintf(buf, "Close Instruction: "); 391 | for (int i = 0; i < length; ++i) 392 | sprintf(buf, "%s0x%x ", buf, close_with_certain_speed_certain_force[i]); 393 | fprintf(stdout, "%s\r\n", buf); 394 | #endif 395 | return; 396 | } -------------------------------------------------------------------------------- /modbustcp.c: -------------------------------------------------------------------------------- 1 | /************************************************** 2 | * This file is for modbus tcp 3 | * Modified by xfyu on April 13 4 | **************************************************/ 5 | #include "modbustcp.h" 6 | #include "modbusrtu.h" /* Check Wrist Bound need to open gripper */ 7 | #include "main.h" 8 | 9 | /************************************************** 10 | * Global Variables 11 | **************************************************/ 12 | /* request frame for the tcp pose */ 13 | unsigned char pos_req_frm[12] = { 14 | 0x00, 0x01, /* sequence number */ 15 | 0x00, 0x00, /* protocol identifier */ 16 | 0x00, 0x06, /* package length */ 17 | 0x00, 0x04, /* function code for read input registers */ 18 | 0x01, 0x90, /* addr of first reg: 400 */ 19 | 0x00, 0x06 /* total number of reg to read */ 20 | }; 21 | 22 | /* request frame for the wrist angle */ 23 | unsigned char wrist_req_frm[12] = { 24 | 0x00, 0x01, /* sequence number */ 25 | 0x00, 0x00, /* protocol identifier */ 26 | 0x00, 0x06, /* package length */ 27 | 0x00, 0x04, /* function code for read input registers */ 28 | 0x01, 0x0e, /* addr of first reg: 400 */ 29 | 0x00, 0x06 /* total number of reg to read */ 30 | }; 31 | /* wrist offset, to make the sign right */ 32 | const float OFFSET[] = {0, -6.283, 0, 0, 0, 0}; 33 | 34 | /*************************************************** 35 | * Functions 36 | ***************************************************/ 37 | /* 38 | * connect_modbus - establish the connection with modbus 39 | * Only open the connection, need the caller to close 40 | * the connection once finished. 41 | * 42 | * Parameter: none 43 | * Return value: >0 - success, the modbus_fd 44 | * -1 - error 45 | */ 46 | int connect_modbustcp() { 47 | /* variables used in TCP connection */ 48 | int clientSocket; 49 | struct sockaddr_in serverAddr; 50 | 51 | /* set up the socket of client */ 52 | if ((clientSocket = socket(AF_INET, SOCK_STREAM, 0)) < 0) { 53 | perror("socket error"); 54 | return -1; 55 | } 56 | 57 | /* set the parameters */ 58 | serverAddr.sin_family = AF_INET; 59 | serverAddr.sin_port = htons(MODBUS_PORT); 60 | serverAddr.sin_addr.s_addr = inet_addr(ROBOT_ADDR); 61 | /* try to connect to the server */ 62 | if (connect(clientSocket, (struct sockaddr *)&serverAddr, 63 | sizeof(serverAddr)) < 0) { 64 | perror("connect error"); 65 | return -1; 66 | } 67 | /*#ifdef DEBUG 68 | fprintf(stdout, "connect to port %d suceeded, clientSocket = %d\n", 69 | MODBUS_PORT, clientSocket); 70 | #endif*/ 71 | 72 | return clientSocket; 73 | } 74 | 75 | /* 76 | * connect_realtime - establish the connection with robot server 77 | * Only open the connection, need the caller to close 78 | * the connection once finished. 79 | * Set the conn_sock. 80 | * 81 | * Parameter: none 82 | * Return value: >0 - success, the modbus_fd 83 | * -1 - error 84 | */ 85 | int connect_realtime() { 86 | /* variables used in TCP connection */ 87 | struct sockaddr_in serverAddr; 88 | int conn_sock; 89 | 90 | /* set up the socket of client */ 91 | if ((conn_sock = socket(AF_INET, SOCK_STREAM, 0)) < 0) { 92 | perror("socket error"); 93 | return -1; 94 | } 95 | 96 | /* set the parameters */ 97 | serverAddr.sin_family = AF_INET; 98 | serverAddr.sin_port = htons(REALTIME_PORT); /* the only diff */ 99 | serverAddr.sin_addr.s_addr = inet_addr(REALTIME_ADDR); 100 | 101 | #ifdef DEBUG 102 | printf("connecting..."); 103 | #endif 104 | 105 | /* try to connect to the server */ 106 | if (connect(conn_sock, (struct sockaddr *)&serverAddr, 107 | sizeof(serverAddr)) < 0) { 108 | perror("connect error"); 109 | return -1; 110 | } 111 | #ifdef DEBUG 112 | fprintf(stdout, "connect to port %d suceeded, conn_sock = %d\n", 113 | REALTIME_PORT, conn_sock); 114 | #endif 115 | 116 | return conn_sock; 117 | } 118 | 119 | /* 120 | * read_pos - Read 6 pos value 121 | * p[x, y, z, rx, ry, rz]. The x, y, z are in 0.1mm base. 122 | * The rx, ry, rz are in 0.001rad base. 123 | * 124 | * Parameter: recv_value - the space to store the processed data 125 | * Return value: 0 - succeed 126 | * nonzero - trasaction error 127 | * 1 - invalid data 128 | */ 129 | int read_pos(float * ret_value) { 130 | int clientSocket; 131 | /* receive buffer */ 132 | unsigned char recvbuf[BUF_SIZE]; 133 | short recv_value[6]; /* signed directly receive value */ 134 | int res; /* ret num */ 135 | short divider[2] = {10000, 1000}; 136 | 137 | /* connect to the modbus server */ 138 | if ((clientSocket = connect_modbustcp()) < 0) 139 | return -1; 140 | 141 | /* Send request */ 142 | if (write(clientSocket, pos_req_frm, 143 | sizeof(pos_req_frm)) < 0) { 144 | perror("send error"); 145 | close(clientSocket); 146 | return 1; 147 | } 148 | 149 | /* store the size that receive */ 150 | if ((res = read(clientSocket, recvbuf, BUF_SIZE)) < 0) { 151 | perror("receive error"); 152 | close(clientSocket); 153 | return 1; 154 | } 155 | /*#ifdef DEBUG 156 | fprintf(stdout, "Receive Bytes:"); 157 | for (int i = 0; i < res; ++i) { 158 | fprintf(stdout, "%x ", recvbuf[i]); 159 | } 160 | fprintf(stdout, "\n"); 161 | #endif*/ 162 | 163 | if (res >= 21) /* check if all 6 regs are read */ 164 | for (int i = 0; i < REG_NUM; ++i) { 165 | int index = 9 + 2 * i; /* offset in the recvbuf */ 166 | recv_value[i] = recvbuf[index] * 256 + 167 | recvbuf[index + 1]; 168 | /* process the value to its right base */ 169 | ret_value[i] = recv_value[i] * 1.0 / divider[i>=3]; 170 | } 171 | else { 172 | close(clientSocket); 173 | return 1; 174 | } 175 | 176 | close(clientSocket); 177 | return 0; 178 | } 179 | 180 | /* 181 | * read_wrist - Read 6 joint value 182 | * [base, shoulder, elbow, wrist1, wrist2, wrist3]. 183 | * All values are in 0.001rad base. 184 | * 185 | * Parameter: recv_value - the space to store the data 186 | * Return value: 0 - succeed 187 | * nonzero - error 188 | * 1 - invalid data 189 | */ 190 | int read_wrist(float * ret_value) { 191 | int clientSocket; 192 | /* receive buffer */ 193 | unsigned char recvbuf[BUF_SIZE]; 194 | short recv_value[6]; /* signed directly receive value */ 195 | int res; /* ret num */ 196 | 197 | /* connect to the modbus server */ 198 | if ((clientSocket = connect_modbustcp()) < 0) 199 | return -1; 200 | 201 | /* write request */ 202 | if ((write(clientSocket, wrist_req_frm, 203 | sizeof(wrist_req_frm))) < 0) { 204 | perror("send error"); 205 | close(clientSocket); 206 | return 1; 207 | } 208 | 209 | /* store the size that receive */ 210 | if ((res = read(clientSocket, recvbuf, BUF_SIZE)) < 0) { 211 | perror("receive error"); 212 | close(clientSocket); 213 | return 1; 214 | } 215 | #ifdef DEBUG 216 | fprintf(stdout, "Receive Bytes:"); 217 | for (int i = 0; i < res; ++i) { 218 | fprintf(stdout, "%x ", recvbuf[i]); 219 | } 220 | fprintf(stdout, "\n"); 221 | #endif 222 | 223 | if (res >= 21) /* check if all 6 regs are read */ 224 | for (int i = 0; i < REG_NUM; ++i) { 225 | int index = 9 + 2 * i; /* offset in the recvbuf */ 226 | recv_value[i] = recvbuf[index] * 256 + recvbuf[index + 1]; 227 | /* process the value to its base and add the offset */ 228 | ret_value[i] = recv_value[i] * 1.0 / 1000 + OFFSET[i]; 229 | } 230 | else { 231 | close(clientSocket); 232 | return 1; /* invalid data */ 233 | } 234 | 235 | /* close the transaction */ 236 | close(clientSocket); 237 | return 0; 238 | } 239 | 240 | /* 241 | * send_movel_instruct - send move intruction to robot 242 | * Using position. 243 | * 244 | * Input: desired_pose - the target position 245 | * Return: None. 246 | * Error message will display in the Msg box. 247 | */ 248 | void send_movel_instruct(float * desired_pose) { 249 | /* establish the realtime connection */ 250 | int conn_sock = connect_realtime(); 251 | if (conn_sock < 0) 252 | return; /* connect failed */ 253 | char sendbuf[BUF_SIZE]; /* buffer to send */ 254 | 255 | /* send the instrction */ 256 | sprintf(sendbuf, "movel(p[%.3f,%.3f,%.3f,%.3f,%.3f,%.3f])\r\n", 257 | desired_pose[0], desired_pose[1], desired_pose[2], 258 | desired_pose[3], desired_pose[4], desired_pose[5]); 259 | sprintf(sendbuf, "%s\r\n", sendbuf); 260 | if (write(conn_sock, sendbuf, sizeof(sendbuf)) < 0) { 261 | perror("Send instruction error"); 262 | return; 263 | } 264 | 265 | fprintf(stdout, "movel instruction:%s", sendbuf); 266 | 267 | /* wait until the move complete */ 268 | float * cur_pose = (float *)malloc(REG_NUM * sizeof(float)); 269 | read_pos(cur_pose); 270 | int cnt = 0; 271 | while ((wait_until_nodiff(cur_pose, desired_pose) > THRESHOLD) && 272 | cnt <= 10000) { 273 | read_pos(cur_pose); 274 | cnt++; 275 | // delay(10000); 276 | } 277 | if (cnt > 10000) 278 | perror("Move failed!"); 279 | close(conn_sock); 280 | free(cur_pose); 281 | return; 282 | } 283 | 284 | /* 285 | * send_movej_instruct - send move intruction to robot 286 | * Using joint positions. 287 | * move to the position decribed by angles' values 288 | * 289 | * Input: float * desired_joint - target position 290 | * Return: none. 291 | * Error message will display in the Msg box. 292 | */ 293 | void send_movej_instruct(float * desired_joint) { 294 | char sendbuf[BUF_SIZE]; /* buffer to send */ 295 | 296 | /* establish the realtime connection */ 297 | int conn_sock = connect_realtime(); 298 | if (conn_sock < 0) 299 | return; /* connect failed */ 300 | /* send the instrction */ 301 | sprintf(sendbuf, "movej([%.3f,%.3f,%.3f,%.3f,%.3f,%.3f])\r\n", 302 | desired_joint[0], desired_joint[1], desired_joint[2], 303 | desired_joint[3], desired_joint[4], desired_joint[5]); 304 | sprintf(sendbuf, "%s\r\n", sendbuf); 305 | if (write(conn_sock, sendbuf, sizeof(sendbuf)) < 0) { 306 | perror("Send instruction error"); 307 | return; 308 | } 309 | 310 | fprintf(stdout, "instruction:%s", sendbuf); 311 | 312 | /* wait until the move complete */ 313 | float * cur_joint = (float *)malloc(REG_NUM*sizeof(float)); 314 | int cnt = 0; 315 | read_wrist(cur_joint); 316 | while ((wait_until_nodiff(cur_joint, desired_joint) > THRESHOLD) && 317 | cnt <= 10000) { 318 | int res = read_wrist(cur_joint); 319 | cnt++; 320 | } 321 | if (cnt > 10000) 322 | perror("Move failed!"); 323 | 324 | close(conn_sock); 325 | free(cur_joint); 326 | return; 327 | } 328 | 329 | /* 330 | * wait_until_nodiff - calculate the difference between 2 vectors 331 | */ 332 | float wait_until_nodiff(float *a1, float *a2) { 333 | float sum = 0; 334 | /* calculate the ^2 sum */ 335 | for (int i = 0; i < REG_NUM; ++i) { 336 | sum += (a1[i] - a2[i]) * (a1[i] - a2[i]); 337 | // fprintf(stdout, "*a1 %f, *a2 %f\n", a1[i], a2[i]); 338 | } 339 | #ifdef DEBUG 340 | fprintf(stdout, "sum is %f\n", sum); 341 | #endif 342 | return sum; 343 | } 344 | --------------------------------------------------------------------------------