1. 把sensor安装到机器人法兰,485接线可以接到机器人末端法兰(建议升级到V3.9版本)或者直接接到控制柜的RS485口
2. 根据鑫精诚sensor手册,机器人发送"0910461C00020400000000C2F5"可以清零sensor,发送"0910019A0001020000CCAA",sensor会以100hz频率返回机器人sensor数据,返回格式:
20 4E Fx_L Fx_H Fy_L Fy_H Fz_L Fz_H Mx_L Mx_H My_L My_H Mz_L Mz_H Crc_L Crc_H
即从第三个字节开始,每2个字节构成一个数据,为int数据,需要自行转化为带符号的整数。
实际力值(N) = 带符号十进制数据 / 10
实际力矩值(N·m) = 带符号十进制数据 / 100
可以使用以下代码(以下代码以sensor接到控制器的rs485为例)。
(代码中也有部分介绍使用坤为sensor的方法,注:坤为返回数据为4个字节一组的float,低字节在前)
机器人示教器中,力控数据源选择lua。
--[[ -- @Author: qiaoli -- @Date: 2023-02-17 -- @LastEditors:chenliao@elibot.cn ]] -- sleep(0.5) ---------------------------Functions user should implement-------------------- function sensor_init() -- 传感器工作前的一些初始化操作,在此函数内实现 if (TCIorController == 1) then tci_send("0910461C00020400000000C2F5", 1) sleep(1) tci_send("0910019A0001020000CCAA", 1) tci_flush() end if (TCIorController == 2) then repeat rs485_flush() local lenth = rs485_send("0910019A0001020000CCAA", 1) ret, recv_buff = rs485_recv(2, 1) until (ret ~= 0) rs485_flush() end end function Str2Int(d, BigEndian) -- 输入2个字节的字符串,默认低字节在前,高字节在后,例如'6BFF',对应带符号十进制为-149 -- 如果高字节在前,使用参数BigEndian out = tonumber(string.sub(d, 1, 2), 16) out = (tonumber(string.sub(d, 3, 4), 16) << 8) | out if BigEndian == nil then out = string.pack("I2", out) else out = string.pack(">I2", out) end out = string.unpack("i2", out) return out end function Str2float(d,BigEndian) -- 输入的d为4个字节 的16进制数据的字符串,默认为低字节在前 -- 若高字节前,例如'4623A314',则使用BigEndian,'46 23 A3 14'(高字节在前)对应的浮点数是10472.76953125 out = tonumber(string.sub(d,1,2),16) out = (tonumber(string.sub(d,3,4),16) <<8)|out out = (tonumber(string.sub(d,5,6),16) <<16)|out out = (tonumber(string.sub(d,7,8),16) <<24)|out if BigEndian==nil then out = string.pack('I',out) else out = string.pack('>I',out) end out = string.unpack('f',out) return out end function convert_str_to_torque_array(data) -- 将帧字符串转换成力矩数组,每种传感器的实现不唯一,在此函数内实现 -- 此处以鑫精诚sensor为例,20 4E 6B FF 51 01 39 01 C4 FF B9 FF 35 00 E6 BD -- 20 4E 为数据头,之后每2个字节(51 FF)为一个数据的INT数据,E6 BD 为crc校验码 local f1 = string.sub(data, 5, 8) --截取6BFF 4个字符 local f2 = string.sub(data, 9, 12) --截取5101 4个字符 local f3 = string.sub(data, 13, 16) --截取3901 4个字符 local f4 = string.sub(data, 17, 20) --截取C4FF 4个字符 local f5 = string.sub(data, 21, 24) --截取B9FF 4个字符 local f6 = string.sub(data, 25, 28) --截取3500 4个字符 local F1 = Str2Int(f1) / 10 --将0x6BFF 转为带符号数据(-149)并除10,得到-14.9 local F2 = Str2Int(f2) / 10 local F3 = Str2Int(f3) / 10 local F4 = Str2Int(f4) / 1000 local F5 = Str2Int(f5) / 1000 local F6 = Str2Int(f6) / 1000 -- -- 以下以坤为sensor为例 -- -- 从第3字节开始,每4个字节构成一个float,低字节在前。以0D0A结尾 -- -- 49 AA B0751CC1 9B3FEDC0 AE5CF040 D4B52DBE 5F55993E 7C1AB0BB 0D0A -- local f1 = string.sub(data, 5, 12) --截取B0751CC1 8个字符 -- local f2 = string.sub(data, 13, 20) --截取9B3FEDC0 8个字符 -- local f3 = string.sub(data, 21, 28) --截取AE5CF040 8个字符 -- local f4 = string.sub(data, 29, 36) --截取D4B52DBE 8个字符 -- local f5 = string.sub(data, 37, 44) --截取5F55993E 8个字符 -- local f6 = string.sub(data, 45, 52) --截取7C1AB0BB 8个字符 -- local F1 = Str2float(f1) --将0xB0751CC1 转为浮点数-9.7787kg(坤为的力单位是kg) -- local F2 = Str2float(f2) -- local F3 = Str2float(f3) -- local F4 = Str2float(f4) --截取D4B52DBE 转为浮点数-0.1696(坤为的力矩单位是kg·m) -- local F5 = Str2float(f5) -- local F6 = Str2float(f6) local torque = {F1, F2, F3, F4, F5, F6} set_global_variable(FX, F1) set_global_variable(FY, F2) set_global_variable(FZ, F3) set_global_variable(MX, F4) set_global_variable(MY, F5) set_global_variable(MZ, F6) return torque end ---------------------------End of Functions user should implement------------- ---------------------------Internal function---------------------------------- function str2hex(str) --判断输入类型 if (type(str) ~= "string") then return nil, "str2hex invalid input type" end --滤掉分隔符 str = str:gsub("[%s%p]", ""):upper() --检查内容是否合法 if (str:find("[^0-9A-Fa-f]") ~= nil) then return nil, "str2hex invalid input content" end --检查字符串长度 if (str:len() % 2 ~= 0) then return nil, "str2hex invalid input lenth" end --拼接字符串 local index = 1 local ret = "" for index = 1, str:len(), 2 do ret = ret .. string.char(tonumber(str:sub(index, index + 1), 16)) end return ret end --[[ -- extract_nice_frame: 从raw_data中返回符合要求的数据帧 -- return: 若无有效数据返回nil, 否则返回完整的一帧数据 --]] function extract_nice_frame(raw_data) local begin_idx, end_idx, valid_data local len = string.len(raw_data) local actual_frame_len = FRAME_LEN * 2 -- FRAME_LEN * 2 代表16进制数据转换成ASCII后的长度 if len < actual_frame_len then -- 先判断字符长度是否满足 --elite_print("recv wrong data len: "..len) return nil end if (FRAME_END) then -- 一帧数据包含固定帧尾的情况 begin_idx, end_idx = string.find(raw_data, FRAME_BEGIN..'.*'..FRAME_END, 1, false) --string.find 会匹配满足要求的最长字串 if (not begin_idx or not end_idx) then return nil elseif (end_idx - begin_idx == actual_frame_len - 1) then -- 正好符合帧的格式,这是大多数情况 valid_data = string.sub(recv_buff, begin_idx, begin_idx + actual_frame_len -1) return valid_data elseif (end_idx - begin_idx > actual_frame_len - 1) then local idx1,idx2 idx1 = begin_idx repeat -- 比较帧最后的字符是否为帧尾 local sub_star_idx = idx1 + actual_frame_len - string.len(FRAME_END) local sub_end_idx = sub_star_idx + string.len(FRAME_END) - 1 local tmp_tail = string.sub(raw_data, sub_star_idx, sub_end_idx) if (tmp_tail == FRAME_END) then valid_data = string.sub(raw_data, idx1, idx1 + actual_frame_len -1) return valid_data end idx1, idx2 = string.find(raw_data, FRAME_BEGIN, idx1 + string.len(FRAME_BEGIN)) until(not idx1) return nil end else -- 没有固定帧尾的情况 begin_idx, end_idx = string.find(raw_data, FRAME_BEGIN) if (begin_idx and end_idx and begin_idx <= len - actual_frame_len) then local idx1,idx2 = string.find(raw_data, FRAME_BEGIN, end_idx + 1) if (not idx1 or idx1 > len - actual_frame_len) then -- 没固定帧尾时,这两种条件下截取的一定是有效帧 valid_data = string.sub(recv_buff, begin_idx, begin_idx + actual_frame_len -1) return valid_data end else return nil end end return nil end ---------------------------End of Internal Function-------------------------- -------------------------- User Config First--------------------------------- -- 脚本运行前请先配置下面的基本参数 FRAME_LEN = 16 -- 传感器发送的一帧数据帧的长度,单位:字节 FRAME_BEGIN = "204E" -- 数据帧的帧头标识,此处以鑫精诚sensor为例,如果是坤为sensor填入"48AA" FRAME_END = nil -- 数据帧的帧尾标识, 如果没有帧尾请填入nil,此处以鑫精诚sensor为例.如果是坤为sensor填入"0D0A" FRAME_MAX_RECV_TM = 40 -- 串口每次接收等待的最大时间,unit: ms SERIAL_speed = 115200 -- 串口波特率 SERIAL_bits = 8 -- 串口传输数据位 SERIAL_event = "N" -- 串口传输停止位 SERIAL_stop = 1 -- 串口传输停止位 TCIorController = 2 -- 1使用末端Tci, 2使用控制柜485接口 -------------------------- End of User Config First ------------------------- -- 用户变量 Control_script = "D0" --控制脚本运行 SCRIPT_MODE = "D1" --将脚本指令写入到传感器1:停止持续传输数据 FX = "D2" --传感器参数 FY = "D3" --传感器参数 FZ = "D4" --传感器参数 MX = "D5" --传感器参数 MY = "D6" --传感器参数 MZ = "D7" --传感器参数 ID = "D8" --传感器地址 ---------------------------Script start------------------------------------ frame_idx = 0 torque = {0, 0, 0, 0, 0, 0} err_tm = 0 stop_push_force() if (TCIorController == 1) then tci_close() end if (TCIorController == 2) then rs485_close() end sleep(0.5) if (TCIorController == 1) then open = tci_open() end if (TCIorController == 2) then open = rs485_open() end if (open >= 0) then if (TCIorController == 1) then tci_setopt(SERIAL_speed, SERIAL_bits, SERIAL_event, SERIAL_stop) end if (TCIorController == 2) then rs485_setopt(SERIAL_speed, SERIAL_bits, SERIAL_event, SERIAL_stop) end else elite_print("串口打开失败!") exit() -- 并没有exit函数,只是为了让系统报错,脚本退出执行 end sensor_init() -- User implement start_push_force() last_tm = os.clock() -------------------------------main loop------------------------------------------------------- repeat now_tm = os.clock() if TCIorController==1 then ret, recv_buff = tci_recv(FRAME_MAX_RECV_TM, 1, FRAME_LEN * 2) --接收2帧长度的数据,确保有一帧为有效数据 end if TCIorController ==2 then ret,recv_buff = rs485_recv(FRAME_MAX_RECV_TM, 1, FRAME_LEN * 2) --接收2帧长度的数据,确保有一帧为有效数据 end valid_frame = extract_nice_frame(recv_buff) --elite_print("valid_frame : ",valid_frame) if (valid_frame) then torque = convert_str_to_torque_array(valid_frame) -- User implement last_tm = now_tm else torque = {0, 0, 0, 0, 0, 0} err_tm = now_tm - last_tm -- 计算传感器数据异常的持继时间 set_global_variable("D9", err_tm) end push_external_force(frame_idx, torque) tci_flush() frame_idx = frame_idx + 1 if frame_idx >= 65535 then frame_idx = 1 end sleep(0.001) until (get_global_variable("D0") == 0) stop_push_force() if TCIorController==1 then tci_close() end if TCIorController==2 then rs485_close() end